diff --git a/Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java b/Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java index 85f4f7cf..012ecea8 100644 --- a/Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java +++ b/Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java @@ -149,7 +149,7 @@ public static void main(String[] args) throws IOException { //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 diff --git a/R scripts/drawMP.R b/R scripts/drawMP.R index 5c7f4ccb..42be277e 100644 --- a/R scripts/drawMP.R +++ b/R scripts/drawMP.R @@ -73,7 +73,7 @@ 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 <- perpendicular-theta/2 + 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. diff --git a/R scripts/poseEstimationChecker.R b/R scripts/poseEstimationChecker.R index 5628f027..68f2782e 100644 --- a/R scripts/poseEstimationChecker.R +++ b/R scripts/poseEstimationChecker.R @@ -2,34 +2,9 @@ rad2deg <- function(rad) {(rad * 180) / (pi)} deg2rad <- function(deg) {(deg * pi) / (180)} -#Get the angle between two points -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)) - } - } +#Find the distance between the end position of two things +dist <- function(x1, y1, x2, y2){ + return(sqrt((x1[length(x1)] - x2[length(x2)])^2+(y1[length(y1)] - y2[length(y2)])^2)) } #Calculate the effective wheelbase for a given delta left, right, and angle @@ -43,8 +18,8 @@ smoothDerivative <- function(value, timeMillis, n){ return(c(rep(0, ceiling(n/2)), smoothed, rep(0, floor(n/2)))); } -#Just kinda does everything -plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst, actualWheelbase, fakeWheelbase){ +#Plot the effective wheelbase against a bunch of different things +plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst){ #Convert because degrees suuuuck rawAngle <- deg2rad(rawAngleDegrees) @@ -73,24 +48,21 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul #Plot wheelbase vs turn radius plot(x=combinedAngular[,3]/combinedAngular[,1], y=combinedAngular[,2]-2.0833, xlab="Turn Radius (feet)", ylab="Error in wheelbase diameter (feet)", main="Error in Wheelbase Diameter vs. Turn Radius") - abline(a=avgWheelbase-2.0833, b=0, col='green') + abline(a=avgWheelbase, b=0, col='green') + + return(avgWheelbase) +} + +#A pose estimation algorithm that assumes the left and right sides have equal scrub, in opposite directions +equalScrubPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad){ + #Convert because degrees suuuuck + rawAngle <- deg2rad(rawAngleDegrees) - #Eli's algorithm + #Set up output array out <- array(dim=c(length(timeMillis),8)) colnames(out)<-c("X","Y","leftX","leftY","rightX","rightY","time","wheelbase") out[1,] <- c(leftPos[1],rightPos[1],NA,NA,NA,NA,timeMillis[1],NA) - #Noah's algorithm - noah <- array(dim=c(length(timeMillis),7)) - colnames(noah) <- c("X","Y","leftX","leftY","rightX","rightY","time") - angle <- rawAngle[1] - noah[1,] <- c(leftPos[1],rightPos[1],actualWheelbase/2*cos(angle+pi/2), actualWheelbase/2*sin(angle+pi/2), actualWheelbase/2*cos(angle-pi/2), actualWheelbase/2*sin(angle-pi/2),timeMillis[1]) - - #Encoder-only algorithm - encOnly <- array(dim=c(length(timeMillis), 7)) - colnames(encOnly) <- c("X","Y","leftX","leftY","rightX","rightY","time") - encOnly[1,] <- c(leftPos[1],rightPos[1],actualWheelbase/2*cos(angle+pi/2), actualWheelbase/2*sin(angle+pi/2), actualWheelbase/2*cos(angle-pi/2), actualWheelbase/2*sin(angle-pi/2),timeMillis[1]) - #Loop through each logged tic, calculating pose iteratively for(i in 2:length(timeMillis)){ #Find change in time, in seconds @@ -108,9 +80,8 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul avgMoved <- (deltaLeft+deltaRight)/2 #The angle of the movement vector - angle <- rawAngle[i-1]-(deltaTheta/2) - - #Eli's approach + angle <- rawAngle[i-1]+(deltaTheta/2) + if (deltaTheta == 0){ #If we're driving straight, we know the magnitude is just the average of the two sides out[i,] <- c(out[i-1,1]+avgMoved*cos(angle),out[i-1,2]+avgMoved*sin(angle), NA, NA, NA, NA, timeMillis[i],NA) @@ -126,27 +97,41 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul if (deltaTheta/deltaTime < angularVelThreshRad){ out[i,] <- c(x, y, NA,NA,NA,NA, timeMillis[i],NA) } else { - out[i,] <- c(x, y, x+wheelbase/2*cos(angle+pi/2), y+wheelbase/2*sin(angle+pi/2), x+wheelbase/2*cos(angle-pi/2), y+wheelbase/2*sin(angle-pi/2), timeMillis[i],wheelbase) + out[i,] <- c(x, y, x+wheelbase/2*cos(rawAngle[i]+pi/2), y+wheelbase/2*sin(rawAngle[i]+pi/2), x+wheelbase/2*cos(rawAngle[i]-pi/2), y+wheelbase/2*sin(rawAngle[i]-pi/2), timeMillis[i],wheelbase) } } + } + + #Plot results, with fake wheelbase, only showing points within 1 actual wheelbase of the path + plot(out[,1], out[,2], t="l", xlim = c(min(out[,1], na.rm = TRUE)-3,max(out[,1],na.rm = TRUE)+3), ylim=c(min(out[,2], na.rm = TRUE)-3,max(out[,2], na.rm=TRUE)+3), xlab="X position (Feet)", ylab="Y position (Feet)", main="Equal Scrub Pose Estimation Algorithm", asp=1) + lines(out[,3], out[,4], col="Green") + lines(out[,5], out[,6], col="Red") + + return(out) +} + +#A pose estimation algorithm that ignores the worse encoder reading. +ignoreWorstPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, actualWheelbase){ + #Convert because degrees suuuuck + rawAngle <- deg2rad(rawAngleDegrees) + + #Set up output array + out <- array(dim=c(length(timeMillis),7)) + colnames(out) <- c("X","Y","leftX","leftY","rightX","rightY","time") + angle <- rawAngle[1] + out[1,] <- c(leftPos[1],rightPos[1],actualWheelbase/2*cos(angle+pi/2), actualWheelbase/2*sin(angle+pi/2), actualWheelbase/2*cos(angle-pi/2), actualWheelbase/2*sin(angle-pi/2),timeMillis[1]) + + #Loop through each logged tic, calculating pose iteratively + for(i in 2:length(timeMillis)){ - #Encoder only - perpendicular <- angleBetween(leftX=encOnly[i-1,3],leftY=encOnly[i-1,4],rightX = encOnly[i-1,5],rightY = encOnly[i-1,6]) - theta <- (deltaLeft - deltaRight)/fakeWheelbase - if(theta == 0){ - x <- encOnly[i-1,1]+avgMoved*cos(perpendicular) - y <- encOnly[i-1,2]+avgMoved*sin(perpendicular) - encOnly[i,] <- c(x,y,x+fakeWheelbase/2*cos(perpendicular+pi/2), y+fakeWheelbase/2*sin(perpendicular+pi/2), x+fakeWheelbase/2*cos(perpendicular-pi/2), y+fakeWheelbase/2*sin(perpendicular-pi/2), timeMillis[i]) - } else { - r <- fakeWheelbase / 2. * (deltaLeft + deltaRight) / (deltaLeft - deltaRight); - mag <- 2*r*sin(theta/2) - encAngle <- perpendicular - theta/2 - x <- encOnly[i-1,1]+mag*cos(encAngle) - y <- encOnly[i-1,2]+mag*sin(encAngle) - encOnly[i,] <- c(x,y,x+fakeWheelbase/2*cos(perpendicular+pi/2), y+fakeWheelbase/2*sin(perpendicular+pi/2), x+fakeWheelbase/2*cos(perpendicular-pi/2), y+fakeWheelbase/2*sin(perpendicular-pi/2), timeMillis[i]) - } + #Directly find change in position and angle + deltaLeft <- leftPos[i]-leftPos[i-1] + deltaRight <- rightPos[i]-rightPos[i-1] + deltaTheta <- rawAngle[i]-rawAngle[i-1] - #Noah's approach + #The angle of the movement vector + angle <- rawAngle[i-1]+(deltaTheta/2) + #Take the side that slipped more and recalculate it from the wheelbase, change in angle, and other side's change if (deltaTheta < (deltaLeft - deltaRight) / actualWheelbase) { if (deltaLeft > 0) { @@ -162,39 +147,93 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul } } - #Recalculate average after recalculating one of the sides + #Calculate average after recalculating one of the sides avgMoved <- (deltaLeft+deltaRight)/2 #If we're driving straight, the magnitude is just the average of the two sides if (deltaTheta == 0){ - noah[i,] <- c(noah[i-1,1]+avgMoved*cos(angle),noah[i-1,2]+avgMoved*sin(angle), x+actualWheelbase/2*cos(angle+pi/2), y+actualWheelbase/2*sin(angle+pi/2), x+actualWheelbase/2*cos(angle-pi/2), y+actualWheelbase/2*sin(angle-pi/2), timeMillis[i]) + out[i,] <- c(out[i-1,1]+avgMoved*cos(angle),out[i-1,2]+avgMoved*sin(angle), x+actualWheelbase/2*cos(angle+pi/2), y+actualWheelbase/2*sin(angle+pi/2), x+actualWheelbase/2*cos(angle-pi/2), y+actualWheelbase/2*sin(angle-pi/2), timeMillis[i]) } else { - #If the sides moved the same distance but the angle changed, the radius is just the sector length over the angle - if (deltaLeft-deltaRight == 0){ - r <- avgMoved/deltaTheta; - } else { - #If they moved a different distance, do a more complicated equation (may be the same as the other one, not doing the math yet) - r <- actualWheelbase / 2. * (deltaLeft + deltaRight) / (deltaLeft - deltaRight); - } - mag <- 2. * r * sin(deltaTheta / 2.); - - #Vector decomposition - x <- noah[i-1,1]+mag*cos(angle) - y <- noah[i-1,2]+mag*sin(angle) - noah[i,] <- c(x, y, x+actualWheelbase/2*cos(angle+pi/2), y+actualWheelbase/2*sin(angle+pi/2), x+actualWheelbase/2*cos(angle-pi/2), y+actualWheelbase/2*sin(angle-pi/2), timeMillis[i]) + #If the sides moved the same distance but the angle changed, the radius is just the sector length over the angle + if (deltaLeft-deltaRight == 0){ + r <- avgMoved/deltaTheta; + } else { + #If they moved a different distance, do a more complicated equation (may be the same as the other one, not doing the math yet) + r <- actualWheelbase / 2. * (deltaLeft + deltaRight) / (deltaLeft - deltaRight); + } + mag <- 2. * r * sin(deltaTheta / 2.); + + #Vector decomposition + x <- out[i-1,1]+mag*cos(angle) + y <- out[i-1,2]+mag*sin(angle) + out[i,] <- c(x, y, x+actualWheelbase/2*cos(rawAngle[i]+pi/2), y+actualWheelbase/2*sin(rawAngle[i]+pi/2), x+actualWheelbase/2*cos(rawAngle[i]-pi/2), y+actualWheelbase/2*sin(rawAngle[i]-pi/2), timeMillis[i]) } } - #Plot Eli results, with fake wheelbase, only showing points within 1 actual wheelbase of the path - plot(out[,1], out[,2], t="l", xlim = c(min(out[,1], na.rm = TRUE)-actualWheelbase,max(out[,1],na.rm = TRUE)+actualWheelbase), ylim=c(min(out[,2], na.rm = TRUE)-actualWheelbase,max(out[,2], na.rm=TRUE)+actualWheelbase), xlab="X position (Feet)", ylab="Y position (Feet)", main="Eli's Pose Estimation Algorithm", asp=1) + #Plot results, with real wheelbase + plot(out[,1], out[,2], t="l", xlim = c(min(out[,1], out[,3], out[,5]),max(out[,1], out[,3], out[,5])), ylim=c(min(out[,2], out[,4], out[,6]),max(out[,2], out[,4], out[,6])), xlab="X position (Feet)", ylab="Y position (Feet)", main="Ignore-Worst Pose Estimation Algorithm",asp=1) lines(out[,3], out[,4], col="Green") lines(out[,5], out[,6], col="Red") - #Plot Noah results, with real wheelbase - plot(noah[,1], noah[,2], t="l", xlim = c(min(noah[,1], noah[,3], noah[,5]),max(noah[,1], noah[,3], noah[,5])), ylim=c(min(noah[,2], noah[,4], noah[,6]),max(noah[,2], noah[,4], noah[,6])), xlab="X position (Feet)", ylab="Y position (Feet)", main="Noah's Pose Estimation Algorithm",asp=1) - #plot(noah[,1],noah[,2],t="l") - lines(noah[,3], noah[,4], col="Green") - lines(noah[,5], noah[,6], col="Red") + return(out) +} + +#A pose estimation algorithm that only uses the encoders +encoderOnlyPoseEstimation <- function(leftPos, rightPos, startingAngleDegrees, timeMillis, fakeWheelbase){ + #Convert because degrees suuuuck + startingAngle <- deg2rad(startingAngleDegrees) + + wheelRadius <- fakeWheelbase/2 + + #Set up output array + out <- array(dim=c(length(timeMillis), 8)) + colnames(out) <- c("X","Y","leftX","leftY","rightX","rightY","angle","time") + out[1,] <- c(leftPos[1],rightPos[1],wheelRadius*cos(startingAngle+pi/2), wheelRadius*sin(startingAngle+pi/2), wheelRadius*cos(startingAngle-pi/2), wheelRadius*sin(startingAngle-pi/2),startingAngle,timeMillis[1]) + + #Loop through each logged tic, calculating pose iteratively + for(i in 2:length(timeMillis)){ + + #Directly find change in position and angle + deltaLeft <- leftPos[i]-leftPos[i-1] + deltaRight <- rightPos[i]-rightPos[i-1] + + #Average + avgMoved <- (deltaLeft+deltaRight)/2 + + #Points in the direction the robot is facing at the start of tic + perpendicular <- out[i-1,7] + + #The angle of the sector the path is tracing + theta <- (deltaLeft - deltaRight)/fakeWheelbase + + #If not turning, magnitude is just the average moved + if(theta == 0){ + mag <- avgMoved + } else { + #If turning, calculate the radius and use that to get the magnitude of the turn + mag <- 2*(avgMoved/theta)*sin(theta/2) + } + + angle <- perpendicular + (theta/2) + x <- out[i-1,1]+mag*cos(angle) + y <- out[i-1,2]+mag*sin(angle) + newHeading <- perpendicular + theta + out[i,] <- c(x,y,x+wheelRadius*cos(newHeading+pi/2), y+wheelRadius*sin(newHeading+pi/2), x+wheelRadius*cos(newHeading-pi/2), y+wheelRadius*sin(newHeading-pi/2), newHeading, timeMillis[i]) + } + + #Plot results, with fake wheelbase + plot(out[,1], out[,2], t="l", xlim = c(min(out[,1], out[,3], out[,5]),max(out[,1], out[,3], out[,5])), ylim=c(min(out[,2], out[,4], out[,6]),max(out[,2], out[,4], out[,6])), xlab="X position (Feet)", ylab="Y position (Feet)", main="Encoder-only Pose Estimation Algorithm",asp=1) + lines(out[,3], out[,4], col="Green") + lines(out[,5], out[,6], col="Red") - return(encOnly) + return(out) } + +#Make all the graphs, for wheelbase and all 3 algorithms, using the average effective wheelbase for encoder-only +doEverything <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst, actualWheelbase){ + avg <- plotWheelVsVel(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, smoothingConst) + equalScrubPoseEstimation(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad) + ignoreWorstPoseEstimation(leftPos, rightPos, rawAngleDegrees, timeMillis, actualWheelbase) + encoderOnlyPoseEstimation(leftPos=leftPos, rightPos=rightPos, startingAngleDegrees=rawAngleDegrees[1], timeMillis=timeMillis, fakeWheelbase=avg) + return(avg) +} \ No newline at end of file diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java index 10ce59f3..fefc2ebb 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java @@ -157,7 +157,7 @@ private static double[] calcEliVector(double left, double right, double deltaThe //This next part is too complicated to explain in comments. Read this wiki page instead: // http://team449.shoutwiki.com/wiki/Pose_Estimation double r = ((left+right)/2.)/deltaTheta; - double vectorAngle = lastAngle - deltaTheta/2.; + double vectorAngle = lastAngle + deltaTheta/2.; double vectorMagnitude = 2. * r * Math.sin(deltaTheta / 2.); vector[0] = vectorMagnitude * Math.cos(vectorAngle); vector[1] = vectorMagnitude * Math.sin(vectorAngle); @@ -183,7 +183,7 @@ private static double[] calcVector(double left, double right, double robotDiamet } else { r = robotDiameter / 2. * (left + right) / (left - right); } - double vectorAngle = lastAngle - deltaTheta/2.; + double vectorAngle = lastAngle + deltaTheta/2.; double vectorMagnitude = 2. * r * Math.sin(deltaTheta / 2.); vector[0] = vectorMagnitude * Math.cos(vectorAngle); vector[1] = vectorMagnitude * Math.sin(vectorAngle);