Merge pull request #108 from blair-robot-project/noah_pose_estimation

Noah pose estimation

Author: Noah Gleason

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

@@ -139,7 +139,6 @@ public static void main(String[] args) throws IOException {
 		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);
@@ -152,11 +151,8 @@ public static void main(String[] args) throws IOException {
 		// the circumference of a circle moved by the robot via C = 360 * n / θ
 		//You then find the diameter via C / π.
 		double balbasaurWheelbase = 30. / 12.;
-		//200 in: 29.96
-		//50 in: 34.2
 
-		//433.415
-		double calciferWheelbase = 26. / 12.;
+		double calciferWheelbase = 26.6536/12.;
 
 		Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH,
 				0.05, 5., 4.5, 9.); //Units are seconds, feet/second, feet/(second^2), and feet/(second^3)

R scripts/drawMP.R

@@ -1,4 +1,4 @@
-plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, centerToFront, centerToBack, centerToSide, startY = 0, startPos = c(-1,-1,-1,-1,-1), usePosition = TRUE){
+plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, centerToFront, centerToBack, centerToSide, startY = 0, startPos = c(-1,-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)
   startingCenter <- c(startY, centerToBack)
@@ -11,17 +11,18 @@ plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, center
   #Position,Velocity,Delta t, Elapsed time
   left$V4 <- (0:(length(left$V1)-1))*left$V3[1]
   right$V4 <- (0:(length(right$V1)-1))*right$V3[1]
-  #Time, Left X, Left Y, Right X, Right Y
-  out <- array(dim=c(length(left$V1),5))
-  if(identical(startPos, c(-1,-1,-1,-1,-1))){
-    out[1,]<-c(0, startingCenter[2], (startingCenter[1]+wheelbaseDiameter/2.), startingCenter[2], (startingCenter[1]-wheelbaseDiameter/2.))
+  #Time, Left X, Left Y, Right X, Right Y, Angle
+  out <- array(dim=c(length(left$V1),6))
+  if(identical(startPos, c(-1,-1,-1,-1,-1,-1))){
+    out[1,]<-c(0, startingCenter[2], (startingCenter[1]+wheelbaseDiameter/2.), startingCenter[2], (startingCenter[1]-wheelbaseDiameter/2.), 0)
   } else {
     out[1,]<-startPos
   }
 
   for(i in 2:length(left$V4)){
     #Get the angle the robot is facing.
-    perpendicular <- angleBetween(leftX = out[i-1,2], leftY = out[i-1,3], rightX = out[i-1,4], rightY = out[i-1,5])-pi/2
+    perpendicular <- out[i-1,6]
+    print(perpendicular)
 
     #Add the change in time
     out[i,1] <- out[i-1,1]+left$V3[i]
@@ -41,56 +42,42 @@ plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, center
       deltaRight <- -deltaRight
     }
 
+    diffTerm <- deltaRight - deltaLeft
     #So in this next part, we figure out the turning center of the robot
     #and the angle it turns around that center. Note that the turning center is
     #often outside of the robot.
 
     #Calculate how much we turn first, because if theta = 0, turning center is infinitely far away and can't be calcualted.
-    theta <- (deltaLeft - deltaRight)/wheelbaseDiameter
+    theta <- diffTerm/wheelbaseDiameter
+    
+    out[i,6] <- out[i-1,6]+theta
 
     # If theta is 0, we're going straight and need to treat it as a special case.
     if (identical(theta, 0)){
-      
-      #If inverted, swap which wheel gets which input
-      if(inverted){
-        out[i, 2] <- out[i-1,2]+deltaRight*cos(perpendicular)
-        out[i, 3] <- out[i-1,3]+deltaRight*sin(perpendicular)
-        out[i, 4] <- out[i-1,4]+deltaLeft*cos(perpendicular)
-        out[i, 5] <- out[i-1,5]+deltaLeft*sin(perpendicular)
-      } else {
-        out[i, 2] <- out[i-1,2]+deltaLeft*cos(perpendicular)
-        out[i, 3] <- out[i-1,3]+deltaLeft*sin(perpendicular)
-        out[i, 4] <- out[i-1,4]+deltaRight*cos(perpendicular)
-        out[i, 5] <- out[i-1,5]+deltaRight*sin(perpendicular)
-      }
+      out[i, 2] <- out[i-1,2]+deltaLeft*cos(perpendicular)
+      out[i, 3] <- out[i-1,3]+deltaLeft*sin(perpendicular)
+      out[i, 4] <- out[i-1,4]+deltaRight*cos(perpendicular)
+      out[i, 5] <- out[i-1,5]+deltaRight*sin(perpendicular)
     } else {
 
       #We do this with sectors, so this is the radius of the turning circle for the
       #left and right sides. They just differ by the diameter of the wheelbase.
-      rightR <- (wheelbaseDiameter/2) * (deltaLeft + deltaRight) / (deltaLeft - deltaRight) - wheelbaseDiameter/2
-      leftR <- rightR + wheelbaseDiameter
+      rightR <- (wheelbaseDiameter/2) * (deltaRight + deltaLeft) / diffTerm + wheelbaseDiameter/2
+      leftR <- rightR - wheelbaseDiameter
 
       #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 <- perpendicular+theta/2
+      vectorTheta <- (out[i-1,6]+out[i,6])/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 <- 2*sin(theta/2)
-      
-      #If inverted, swap which wheel gets which input
-      if(inverted){
-        out[i, 2] <- out[i-1,2]+vectorDistanceWithoutR*rightR*cos(vectorTheta)
-        out[i, 3] <- out[i-1,3]+vectorDistanceWithoutR*rightR*sin(vectorTheta)
-        out[i, 4] <- out[i-1,4]+vectorDistanceWithoutR*leftR*cos(vectorTheta)
-        out[i, 5] <- out[i-1,5]+vectorDistanceWithoutR*leftR*sin(vectorTheta)
-      } else {
-        out[i, 2] <- out[i-1,2]+vectorDistanceWithoutR*leftR*cos(vectorTheta)
-        out[i, 3] <- out[i-1,3]+vectorDistanceWithoutR*leftR*sin(vectorTheta)
-        out[i, 4] <- out[i-1,4]+vectorDistanceWithoutR*rightR*cos(vectorTheta)
-        out[i, 5] <- out[i-1,5]+vectorDistanceWithoutR*rightR*sin(vectorTheta)
-      }
+    
+      out[i, 2] <- out[i-1,2]+vectorDistanceWithoutR*leftR*cos(vectorTheta)
+      out[i, 3] <- out[i-1,3]+vectorDistanceWithoutR*leftR*sin(vectorTheta)
+      out[i, 4] <- out[i-1,4]+vectorDistanceWithoutR*rightR*cos(vectorTheta)
+      out[i, 5] <- out[i-1,5]+vectorDistanceWithoutR*rightR*sin(vectorTheta)
     }
   }
   return(out)
@@ -100,9 +87,9 @@ drawProfile <- function (coords, centerToFront, centerToBack, wheelbaseDiameter,
 
   if (clear){
     if (linePlot){
-      plot(coords[,2],coords[,3], type="l", col="Green", ylim=c(-16, 16),xlim = c(0,54), asp=1)
+      plot(coords[,2],coords[,3], type="l", col="Green", ylim=c(-16, 16),xlim = c(0,54), xlab = "X Position (feet)", ylab="Y Position (feet)", asp=1)
     } else {
-      plot(coords[,2],coords[,3], col="Green", ylim=c(-16, 16), xlim = c(0,54), asp=1)
+      plot(coords[,2],coords[,3], col="Green", ylim=c(-16, 16), xlim = c(0,54), xlab = "X Position (feet)", ylab="Y Position (feet)", asp=1)
     }
     field <- read.csv("field.csv")
     #Strings are read as factors by default, so we need to do this to make it read them as strings
@@ -124,38 +111,9 @@ drawProfile <- function (coords, centerToFront, centerToBack, wheelbaseDiameter,
   }
 }
 
-angleBetween <- function(leftX, leftY, rightX, rightY){
-  deltaX <- leftX-rightX
-  deltaY <- leftY-rightY
-  if (identical(deltaX, 0)){
-    ans <- pi/2
-  } else {
-    #Pretend it's first quadrant because we manually determine quadrants
-    ans <- atan(abs(deltaY/deltaX))
-  }
-  if (deltaY > 0){
-    if (deltaX > 0){
-      #If it's actually quadrant 1
-      return(ans)
-    }else {
-      #quadrant 2
-      return(pi - ans)
-    }
-    return(ans)
-  } else {
-    if (deltaX > 0){
-      #quadrant 4
-      return(-ans)
-    }else {
-      #quadrant 3
-      return(-(pi - ans))
-    }
-  }
-}
-
 drawRobot <- function(robotFile, robotPos){
-  theta <- angleBetween(leftX = robotPos[2], leftY = robotPos[3], rightX = robotPos[4], rightY = robotPos[5])
-  perp <- theta - pi/2
+  theta <- atan2(robotPos[5] - robotPos[3], robotPos[4] - robotPos[2])
+  perp <- theta + pi/2
   robotCenter <- c((robotPos[2]+robotPos[4])/2.,(robotPos[3]+robotPos[5])/2.)
   robot <- read.csv(robotFile)
   rotMatrix <- matrix(c(cos(perp), -sin(perp), sin(perp), cos(perp)), nrow=2, ncol=2, byrow=TRUE)
@@ -177,16 +135,16 @@ wheelbaseDiameter <- 26./12.
 centerToFront <- (27./2.)/12.
 centerToBack <- (27./2.+3.25)/12.
 centerToSide <- (29./2.+3.25)/12.
-
-out <- plotProfile(profileName = "BlueRight", inverted = FALSE, wheelbaseDiameter = wheelbaseDiameter, centerToFront = centerToFront,centerToBack =  centerToBack,centerToSide = centerToSide, startY= -10.3449+centerToSide, usePosition = TRUE)
+#                                                                                                                                                                                         Time, Left X, Left Y, Right X, Right Y
+out <- plotProfile(profileName = "RedRight", inverted = FALSE, wheelbaseDiameter = wheelbaseDiameter, centerToFront = centerToFront,centerToBack =  centerToBack,centerToSide = centerToSide, startPos=c(0, 54 - centerToBack, 10.3449-wheelbaseDiameter,54-centerToBack,10.3449, pi), usePosition = TRUE)
 drawProfile(coords=out, centerToFront=centerToFront, centerToBack=centerToBack, wheelbaseDiameter = wheelbaseDiameter, clear = TRUE, linePlot = TRUE)
 tmp <- out[length(out[,1]),]
 drawRobot("robot.csv", tmp)
-out2 <- plotProfile(profileName = "BlueBackup",inverted = TRUE,wheelbaseDiameter =  wheelbaseDiameter,centerToFront = centerToFront,centerToBack = centerToBack,centerToSide = centerToSide,startPos = tmp)
+out2 <- plotProfile(profileName = "RedBackup",inverted = TRUE,wheelbaseDiameter =  wheelbaseDiameter,centerToFront = centerToFront,centerToBack = centerToBack,centerToSide = centerToSide,startPos = tmp)
 drawProfile(coords = out2, centerToFront = centerToFront, centerToBack = centerToBack, wheelbaseDiameter = wheelbaseDiameter, clear = FALSE)
 tmp2 <- out2[length(out2[,1]),]
 drawRobot("robot.csv", out2[length(out2[,1]),])
-out3 <- plotProfile(profileName = "BlueLoadingToLoading",inverted = FALSE,wheelbaseDiameter =  wheelbaseDiameter,centerToFront = centerToFront,centerToBack = centerToBack,centerToSide = centerToSide,startPos = tmp2)
+out3 <- plotProfile(profileName = "RedBoilerToLoading",inverted = FALSE,wheelbaseDiameter =  wheelbaseDiameter,centerToFront = centerToFront,centerToBack = centerToBack,centerToSide = centerToSide,startPos = tmp2)
 drawProfile(coords = out3, centerToFront = centerToFront, centerToBack = centerToBack, wheelbaseDiameter = wheelbaseDiameter, clear = FALSE)
 drawRobot("robot.csv", out3[length(out3[,1]),])