Merge pull request #100 from blair-robot-project/iroc_code

Iroc code

Author: Noah Gleason

.gitignore

@@ -41,6 +41,7 @@ bash.exe.stackdump
 
 #Copied log files
 logs/
+*Log-*.csv
 
 #Generated profiles in the base-level directory
 *Profile.csv

Pathgen/lib/Pathfinder-Java.jar

@@ -22,17 +22,18 @@ public static void main(String[] args) throws IOException {
 		final double BACK_FROM_PEG = -5;
 		//DO NOT TOUCH THE ONES BELOW
 		final double CARRIAGE_LEN = 3.63;
-		final double BLUE_WALL_TO_CENTER_PEG = 113.75;
-		final double BLUE_WALL_TO_SIDE_PEG = 130.75;
-		final double BLUE_BACK_CORNER_TO_SIDE_PEG = 91.;
-		final double BLUE_HALF_KEY_LENGTH = 150.5/2.;
-		final double BLUE_KEY_CORNER_TO_SIDE_PEG = 15.;
-		final double RED_WALL_TO_CENTER_PEG = 114.;
-		final double RED_WALL_TO_SIDE_PEG = 130.;
-		final double RED_BACK_CORNER_TO_SIDE_PEG = 93.5;
-		final double RED_HALF_KEY_LENGTH = 152.5/2.;
-		final double RED_KEY_CORNER_TO_SIDE_PEG = 18.75;
-		final double AIRSHIP_PARALLEL_OFFSET = 6.;
+		final double BLUE_WALL_TO_CENTER_PEG = 114.;
+		final double BLUE_WALL_TO_SIDE_PEG = 130.5;
+		final double BLUE_BACK_CORNER_TO_SIDE_PEG = 89.;
+		final double BLUE_HALF_KEY_LENGTH = 152./2.;
+		final double BLUE_KEY_CORNER_TO_SIDE_PEG = 16.;
+		final double RED_WALL_TO_CENTER_PEG = 113.5;
+		final double RED_WALL_TO_SIDE_PEG = 131.;
+		final double RED_BACK_CORNER_TO_SIDE_PEG = 97.;
+		final double RED_HALF_KEY_LENGTH = 152./2.;
+		final double RED_KEY_CORNER_TO_SIDE_PEG = 21.;
+		final double AIRSHIP_PARALLEL_OFFSET_BLUE = 1.;
+		final double AIRSHIP_PARALLEL_OFFSET_RED = 2.;
 
 		final double PEG_BASE_TO_CENTER = CENTER_TO_FRONT + CARRIAGE_LEN + BACK_FROM_PEG;
 
@@ -43,14 +44,14 @@ public static void main(String[] args) throws IOException {
 
 		Waypoint[] blueLeft = new Waypoint[]{
 				new Waypoint(0, 0, 0),
-				new Waypoint((BLUE_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.cos(5.*Math.PI/6.))/12.
-						,-(BLUE_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.sin(5.*Math.PI/6.))/12.,-Math.PI/3.)
+				new Waypoint((BLUE_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_BLUE*Math.cos(5.*Math.PI/6.))/12.
+						,-(BLUE_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_BLUE*Math.sin(5.*Math.PI/6.))/12.,-Math.PI/3.)
 		};
 
 		Waypoint[] blueRight = new Waypoint[]{
 				new Waypoint(0, 0, 0),
-				new Waypoint((BLUE_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.cos(5.*Math.PI/6.))/12.
-						,(BLUE_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.sin(5.*Math.PI/6.))/12.,Math.PI/3.)
+				new Waypoint((BLUE_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_BLUE*Math.cos(5.*Math.PI/6.))/12.
+						,(BLUE_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_BLUE*Math.sin(5.*Math.PI/6.))/12.,Math.PI/3.)
 		};
 
 		Waypoint[] blueCenter = new Waypoint[]{
@@ -60,14 +61,14 @@ public static void main(String[] args) throws IOException {
 
 		Waypoint[] redLeft = new Waypoint[]{
 				new Waypoint(0, 0, 0),
-				new Waypoint((RED_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.cos(5.*Math.PI/6.))/12.
-						,-(RED_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.sin(5.*Math.PI/6.))/12.,-Math.PI/3.)
+				new Waypoint((RED_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_RED*Math.cos(5.*Math.PI/6.))/12.
+						,-(RED_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_RED*Math.sin(5.*Math.PI/6.))/12.,-Math.PI/3.)
 		};
 
 		Waypoint[] redRight = new Waypoint[]{
 				new Waypoint(0, 0, 0),
-				new Waypoint((RED_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.cos(5.*Math.PI/6.))/12.
-						,(RED_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.sin(5.*Math.PI/6.))/12.,Math.PI/3.)
+				new Waypoint((RED_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_RED*Math.cos(5.*Math.PI/6.))/12.
+						,(RED_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_RED*Math.sin(5.*Math.PI/6.))/12.,Math.PI/3.)
 		};
 
 		Waypoint[] redCenter = new Waypoint[]{
@@ -105,24 +106,24 @@ public static void main(String[] args) throws IOException {
 
 		Waypoint[] blueLoadingToLoading = new Waypoint[]{
 				new Waypoint(0, 0, 0),
-				new Waypoint(27, -5, 0)
+				new Waypoint(22, -5, 0)
 		};
 
 		Waypoint[] blueBoilerToLoading = new Waypoint[]{
 				new Waypoint(0, 0, 0),
 				new Waypoint(5, 0, 0),
-				new Waypoint(27, -15, 0)
+				new Waypoint(22, -15, 0)
 		};
 
 		Waypoint[] redLoadingToLoading = new Waypoint[]{
 				new Waypoint(0, 0, 0),
-				new Waypoint(27, 5, 0)
+				new Waypoint(22, 5, 0)
 		};
 
 		Waypoint[] redBoilerToLoading = new Waypoint[]{
 				new Waypoint(0, 0, 0),
 				new Waypoint(5, 0, 0),
-				new Waypoint(27, 15, 0)
+				new Waypoint(22, 15, 0)
 		};
 
 		Map<String, Waypoint[]> profiles = new HashMap<>();
@@ -136,25 +137,28 @@ public static void main(String[] args) throws IOException {
 		profiles.put("BlueShoot", bluePegToKey);
 		profiles.put("RedBackup", backupRed);
 		profiles.put("BlueBackup", backupBlue);
+		profiles.put("forward100In", points);
+		profiles.put("BlueBackup", backupBlue);
 		profiles.put("BlueLoadingToLoading", blueLoadingToLoading);
 		profiles.put("BlueBoilerToLoading", blueBoilerToLoading);
 		profiles.put("RedLoadingToLoading", redLoadingToLoading);
 		profiles.put("RedBoilerToLoading", redBoilerToLoading);
+//		profiles.put("forward100In", points);
 
 		final String ROBOT_NAME = "calcifer";
 
 		//Calculated by driving each wheel n inches in opposite directions, then taking the angle moved, θ, and finding
 		// the circumference of a circle moved by the robot via C = 360 * n / θ
 		//You then find the diameter via C / π.
-		double balbasaurWheelbase = 33.3 / 12.;
+		double balbasaurWheelbase = 30. / 12.;
 		//200 in: 29.96
 		//50 in: 34.2
 
 		//433.415
 		double calciferWheelbase = 26./12.;
 
 		Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH,
-				0.05, 5., 3, 6); //Units are seconds, feet/second, feet/(second^2), and feet/(second^3)
+				0.05, 5., 4.5, 9.); //Units are seconds, feet/second, feet/(second^2), and feet/(second^3)
 
 		for (String profile : profiles.keySet()) {
 			Trajectory trajectory = Pathfinder.generate(profiles.get(profile), config);
@@ -168,14 +172,14 @@ public static void main(String[] args) throws IOException {
 			lfw.write(tm.getLeftTrajectory().length() + "\n");
 			for (int i = 0; i < tm.getLeftTrajectory().length(); i++) {
 				lfw.write(tm.getLeftTrajectory().get(i).position + ",\t" + tm.getLeftTrajectory().get(i).velocity + ",\t"
-						+ tm.getLeftTrajectory().get(i).dt + ",");
+						+ tm.getLeftTrajectory().get(i).acceleration + ",\t" + tm.getLeftTrajectory().get(i).dt);
 				lfw.write("\n");
 			}
 
 			rfw.write(tm.getRightTrajectory().length() + "\n");
 			for (int i = 0; i < tm.getRightTrajectory().length(); i++) {
-				rfw.write(tm.getRightTrajectory().get(i).position + ",\t" + tm.getRightTrajectory().get(i).velocity + "," +
-						"\t" + tm.getRightTrajectory().get(i).dt + ",");
+				rfw.write(tm.getRightTrajectory().get(i).position + ",\t" + tm.getRightTrajectory().get(i).velocity +
+						",\t" + tm.getLeftTrajectory().get(i).acceleration + ",\t" + tm.getRightTrajectory().get(i).dt);
 				rfw.write("\n");
 			}
 

Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java

@@ -1,6 +1,6 @@
 plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, centerToFront, centerToBack, centerToSide, startY = 0, startPos = c(-1,-1,-1,-1,-1), usePosition = TRUE){
-  left <- read.csv(paste("../../../../calciferLeft",profileName,"Profile.csv",sep=""), header=FALSE)
-  right <- read.csv(paste("../../../../calciferRight",profileName,"Profile.csv",sep=""), header=FALSE)
+  left <- read.csv(paste("../calciferLeft",profileName,"Profile.csv",sep=""), header=FALSE)
+  right <- read.csv(paste("../calciferRight",profileName,"Profile.csv",sep=""), header=FALSE)
   startingCenter <- c(startY, centerToBack)
   left$V1[1] <- 0
   left$V2[1] <- 0
@@ -73,11 +73,11 @@ plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, center
       #This is the angle for the vector pointing towards the new position of each
       #wheel.
       #To understand why this formula is correct, overlay isoclese triangles on the sectors
-      vectorTheta <- (pi - theta)/2 - (pi/2 - perpendicular)
+      vectorTheta <- perpendicular+theta/2
 
       #The is the length of the vector pointing towards the new position of each
       #wheel divided by the radius of the turning circle.
-      vectorDistanceWithoutR <- sin(theta)/sin((pi-theta)/2)
+      vectorDistanceWithoutR <- 2*sin(theta/2)
 
       #If inverted, swap which wheel gets which input
       if(inverted){

Pathgen/src/main/R/drawMP.R renamed to R scripts/drawMP.R

@@ -0,0 +1,24 @@
+# Title     : findVelVsVoltage
+# Objective : Find the line or curve describing how
+# Created by: Noah Gleason
+# Created on: 9/30/17
+
+trials = 1
+voltages = seq(1.5,5,by=0.5)
+#voltages = c(0.6, 2, 2.5)
+raw <- read.csv("eventLog-2017.10.01.13.58.02.csv")
+raw$class <- as.character(raw$class)
+maxes <- raw[grep("class org.usfirst.frc.team449.robot.drive.unidirectional.commands.DetermineVelVsVoltage", raw$class), 3]
+maxes <- as.numeric(as.vector(maxes))
+
+model <- lm(maxes ~ voltages)
+coeffs <- c(coefficients(model))
+View(coeffs)
+xInt <- -coeffs[1]/coeffs[2]
+
+res <- resid(model)
+
+plot(x=voltages, y=maxes, main = "Max Velocity vs. Voltage", xlab="Voltage (volts)", ylab="Max speed (feet/sec)", xlim=c(0,12), ylim=c(0,12))
+abline(model)
+plot(x=voltages, y=res, main="Residuals", xlab="Voltage (volts)", ylab="Residuals (feet/sec)", xlim=c(0,12))
+abline(0,0)