Skip to content

Commit

Permalink
Add way to measure error in wheelbase
Browse files Browse the repository at this point in the history
  • Loading branch information
Noah Gleason committed Oct 22, 2017
1 parent 383bcc9 commit ec83211
Show file tree
Hide file tree
Showing 3 changed files with 124 additions and 35 deletions.
51 changes: 26 additions & 25 deletions Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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[]{
Expand All @@ -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[]{
Expand Down Expand Up @@ -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<>();
Expand Down Expand Up @@ -154,10 +155,10 @@ public static void main(String[] args) throws IOException {
//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., 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);
Expand Down
106 changes: 97 additions & 9 deletions R scripts/poseEstimationChecker.R
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,77 @@
rad2deg <- function(rad) {(rad * 180) / (pi)}
deg2rad <- function(deg) {(deg * pi) / (180)}

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))
}
}
}

logLogSlope <- function(effectiveWheelbase, angularVel, n){
avg <- weighted.mean(x=effectiveWheelbase, w=angularVel, na.rm=TRUE)
error <- effectiveWheelbase-avg
error <- error^2
avgError <- sqrt(rollmean(error, n, fill = NA))
logError <- log(avgError[(1+floor((n-1)/2)):(length(avgError)-floor(n/2))])
logVel <- log(angularVel[(1+floor((n-1)/2)):(length(angularVel)-floor(n/2))])
plot(x = logVel, y= logError)
model <- lm(logError ~ logVel)
abline(model, col="Green")
return(model)
}

drawRobot <- function(robotFile, robotPos){
theta <- angleBetween(leftX = robotPos[2], leftY = robotPos[3], rightX = robotPos[4], rightY = robotPos[5])
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)

point1s <- rotMatrix %*% matrix(c(robot$x1, robot$y1), nrow = 2, ncol = length(robot$x1), byrow = TRUE)
point1s <- point1s + c(robotCenter[1], robotCenter[2])

point2s <- rotMatrix %*% matrix(c(robot$x2, robot$y2), nrow = 2, ncol = length(robot$x1), byrow = TRUE)
point2s <- point2s + c(robotCenter[1], robotCenter[2])

#Interleave the point1s and point2s so lines() draws them correctly.
xs <- c(rbind(point1s[1,], point2s[1,]))
ys <- c(rbind(point1s[2,], point2s[2,]))

lines(x=xs, y=ys, col="Blue")
}

plotField <- function(filename, xOffset=0, yOffset=0){
field <- read.csv(filename)
#Strings are read as factors by default, so we need to do this to make it read them as strings
field$col <- as.character(field$col)
for (i in 1:length(field$x1)){
lines(c(field$x1[i]+xOffset, field$x2[i]+xOffset), c(field$y1[i]+yOffset, field$y2[i]+yOffset), col=field$col[i])
}
}

#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))
Expand Down Expand Up @@ -38,6 +109,7 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
#Find the mean wheelbase, weighted by angular vel because higher angular vel decreases variance
avgWheelbase = weighted.mean(x=combinedAngular[,2], w=combinedAngular[,1], na.rm=TRUE)

return(combinedAngular)
#plot wheelbase vs angular vel
plot(x=combinedAngular[,1], y=combinedAngular[,2], xlab="Angular Velocity (rad/sec)", ylab="Effective wheelbase diameter (feet)", main="Effective Wheelbase Diameter vs. Angular Velocity")
abline(a=avgWheelbase, b=0, col='green')
Expand All @@ -54,14 +126,18 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
}

#A pose estimation algorithm that assumes the left and right sides have equal scrub, in opposite directions
equalScrubPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad){
equalScrubPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angularVelThreshRad, realWheelbase = -1){
#Convert because degrees suuuuck
rawAngle <- deg2rad(rawAngleDegrees)

#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)
if(realWheelbase == -1){
out[1,] <- c(0,0,NA,NA,NA,NA,timeMillis[1],NA)
} else {
out[1,] <- c(0,0, realWheelbase/2*cos(rawAngle[1]+pi/2), realWheelbase/2*sin(rawAngle[1]+pi/2), realWheelbase/2*cos(rawAngle[1]-pi/2), realWheelbase/2*sin(rawAngle[1]-pi/2), timeMillis[1],realWheelbase)
}

#Loop through each logged tic, calculating pose iteratively
for(i in 2:length(timeMillis)){
Expand All @@ -83,8 +159,14 @@ equalScrubPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMil
angle <- rawAngle[i-1]+(deltaTheta/2)

if (deltaTheta == 0){
x <- out[i-1,1]+avgMoved*cos(angle)
y <- out[i-1,2]+avgMoved*sin(angle)
#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)
if(realWheelbase == -1){
out[i,] <- c(out[i-1,1]+avgMoved*cos(angle),out[i-1,2]+avgMoved*sin(angle), NA, NA, NA, NA, timeMillis[i],NA)
} else {
out[i,] <- c(x, y, x+realWheelbase/2*cos(rawAngle[i]+pi/2), y+realWheelbase/2*sin(rawAngle[i]+pi/2), x+realWheelbase/2*cos(rawAngle[i]-pi/2), y+realWheelbase/2*sin(rawAngle[i]-pi/2), timeMillis[i],realWheelbase)
}
} else {
#Magnitude of movement vector is 2*r*sin(deltaTheta/2), but r is just avg/deltaTheta
mag <- 2*(avgMoved/deltaTheta)*sin(deltaTheta/2)
Expand All @@ -93,11 +175,15 @@ equalScrubPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMil
x <- out[i-1,1]+mag*cos(angle)
y <- out[i-1,2]+mag*sin(angle)

#Only log left and right wheel positions if the angular vel is above threshhold
if (deltaTheta/deltaTime < angularVelThreshRad){
out[i,] <- c(x, y, NA,NA,NA,NA, timeMillis[i],NA)
if(realWheelbase == -1){
#Only log left and right wheel positions if the angular vel is above threshhold
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(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)
}
} else {
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)
out[i,] <- c(x, y, x+realWheelbase/2*cos(rawAngle[i]+pi/2), y+realWheelbase/2*sin(rawAngle[i]+pi/2), x+realWheelbase/2*cos(rawAngle[i]-pi/2), y+realWheelbase/2*sin(rawAngle[i]-pi/2), timeMillis[i],realWheelbase)
}
}
}
Expand All @@ -119,7 +205,7 @@ ignoreWorstPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMi
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])
out[1,] <- c(0,0,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)){
Expand Down Expand Up @@ -152,6 +238,8 @@ ignoreWorstPoseEstimation <- function(leftPos, rightPos, rawAngleDegrees, timeMi

#If we're driving straight, the magnitude is just the average of the two sides
if (deltaTheta == 0){
x <- out[i-1,1]+avgMoved*cos(angle)
y <- out[i-1,2]+avgMoved*sin(angle)
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
Expand Down Expand Up @@ -188,7 +276,7 @@ encoderOnlyPoseEstimation <- function(leftPos, rightPos, startingAngleDegrees, t
#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])
out[1,] <- c(0,0,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)){
Expand Down
2 changes: 1 addition & 1 deletion RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/Logger.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ public Logger(@NotNull @JsonProperty(required = true) Loggable[] subsystems,
//Delete the trailing comma
telemetryHeader.deleteCharAt(telemetryHeader.length() - 1);

telemetryHeader.append(",\n");
telemetryHeader.append("\n");
//Write the telemetry file header
telemetryLogWriter.write(telemetryHeader.toString());
eventLogWriter.close();
Expand Down

0 comments on commit ec83211

Please sign in to comment.