Skip to content

Commit

Permalink
Add actual time to logger
Browse files Browse the repository at this point in the history
  • Loading branch information
Noah Gleason committed Oct 23, 2017
1 parent ec83211 commit 8d72aa9
Show file tree
Hide file tree
Showing 4 changed files with 75 additions and 129 deletions.
83 changes: 45 additions & 38 deletions R scripts/poseEstimationChecker.R
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,36 +2,8 @@
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){
library("zoo")
avg <- weighted.mean(x=effectiveWheelbase, w=angularVel, na.rm=TRUE)
error <- effectiveWheelbase-avg
error <- error^2
Expand All @@ -44,12 +16,10 @@ logLogSlope <- function(effectiveWheelbase, angularVel, n){
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.)
drawRobot <- function(robotFile, x, y, theta){
robotCenter <- c(x,y)
robot <- read.csv(robotFile)
rotMatrix <- matrix(c(cos(perp), -sin(perp), sin(perp), cos(perp)), nrow=2, ncol=2, byrow=TRUE)
rotMatrix <- matrix(c(cos(theta), -sin(theta), sin(theta), cos(theta)), 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])
Expand All @@ -69,7 +39,7 @@ plotField <- function(filename, xOffset=0, yOffset=0){
#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])
lines(c(field$x1[i]+xOffset, field$x2[i]+xOffset), c(-field$y1[i]+yOffset, -field$y2[i]+yOffset), col=field$col[i])
}
}

Expand Down Expand Up @@ -109,7 +79,6 @@ 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 @@ -119,7 +88,7 @@ plotWheelVsVel <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angul
abline(a=avgWheelbase, b=0, col='green')

#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")
plot(x=combinedAngular[,3]/combinedAngular[,1], y=combinedAngular[,2], xlab="Turn Radius (feet)", ylab="Effective wheelbase diameter (feet)", main="Effective Wheelbase Diameter vs. Turn Radius")
abline(a=avgWheelbase, b=0, col='green')

return(avgWheelbase)
Expand Down Expand Up @@ -324,4 +293,42 @@ doEverything <- function(leftPos, rightPos, rawAngleDegrees, timeMillis, angular
ignoreWorstPoseEstimation(leftPos, rightPos, rawAngleDegrees, timeMillis, actualWheelbase)
encoderOnlyPoseEstimation(leftPos=leftPos, rightPos=rightPos, startingAngleDegrees=rawAngleDegrees[1], timeMillis=timeMillis, fakeWheelbase=avg)
return(avg)
}
}

animateRobot <- function(x, y, headingRadians, deltaTime, fieldFile, robotFile, robotRadius, frameSize=-1, filename="animation.mp4"){
library("animation")
theta <- deg2rad(headingRadians)
saveVideo({
for(i in 1:length(x)){
#Set up frame
if(frameSize == -1){
plot(x=c(),y=c(),xlim=c(min(x)-robotRadius, max(x)+robotRadius),ylim=c(min(y)-robotRadius, max(y)+robotRadius), asp=1, xlab="X position (feet)", ylab="Y position (feet)")
} else {
plot(x=c(),y=c(),xlim=c(x[i]-frameSize/2, x[i]+frameSize/2),ylim=c(y[i]-frameSize/2, y[i]+frameSize/2),asp=1, xlab="X position (feet)", ylab="Y position (feet)")
}
plotField(fieldFile, 0, 0)
drawRobot(robotFile, x[i],y[i],theta[i])
}
}, interval = deltaTime, ani.width = 600, ani.height = 600, video.name=filename)
}

tracedAnimation <- function(x, y, leftX, leftY, rightX, rightY, headingRadians, deltaTime, fieldFile, robotFile, robotRadius, frameSize=-1, filename="animation.mp4"){
library("animation")
theta <- deg2rad(headingRadians)
saveVideo({
for(i in 1:length(x)){
#Set up frame
if(frameSize == -1){
plot(x=c(),y=c(),xlim=c(min(x)-robotRadius, max(x)+robotRadius),ylim=c(min(y)-robotRadius, max(y)+robotRadius), asp=1, xlab="X position (feet)", ylab="Y position (feet)")
} else {
plot(x=c(),y=c(),xlim=c(x[i]-frameSize/2, x[i]+frameSize/2),ylim=c(y[i]-frameSize/2, y[i]+frameSize/2),asp=1, xlab="X position (feet)", ylab="Y position (feet)")
}
lines(x=leftX[1:i], y=leftY[1:i], col="Green")
lines(x=rightX[1:i], y=rightY[1:i], col="Red")
plotField(fieldFile, 0, 0)
drawRobot(robotFile, x[i],y[i],theta[i])
}
}, interval = deltaTime, ani.width = 1920, ani.height = 1080, video.name=filename)
}

#tracedAnimation(equalScrub[,1],equalScrub[,2], equalScrub[,3],equalScrub[,4],equalScrub[,5],equalScrub[,6],rel$Drive.raw_angle,0.05,"field.csv","robot.csv",27.138/12, -1, "traceAnimation.mp4")
14 changes: 12 additions & 2 deletions 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 @@ -59,6 +59,11 @@ public class Logger implements Runnable {
*/
private final double loopTimeSecs;

/**
* The time this that logging started. We don't use {@link Clock} because this is a separate thread.
*/
private final long startTime;

/**
* Default constructor.
*
Expand All @@ -77,6 +82,7 @@ public Logger(@NotNull @JsonProperty(required = true) Loggable[] subsystems,
@NotNull @JsonProperty(required = true) String telemetryLogFilename) throws IOException {
//Set up the file names, using a time stamp to avoid overwriting old log files.
String timeStamp = new SimpleDateFormat("yyyy.MM.dd.HH.mm.ss").format(new Date());
startTime = System.currentTimeMillis();
this.eventLogFilename = eventLogFilename + timeStamp + ".csv";
this.telemetryLogFilename = telemetryLogFilename + timeStamp + ".csv";

Expand All @@ -95,7 +101,7 @@ public Logger(@NotNull @JsonProperty(required = true) Loggable[] subsystems,
eventLogWriter.write("time,class,message"+"\n");
//We use a StringBuilder because it's better for building up a string via concatenation.
StringBuilder telemetryHeader = new StringBuilder();
telemetryHeader.append("time,");
telemetryHeader.append("time,Clock.time,");
for (int i = 0; i < this.subsystems.length; i++) {
String[] items = this.subsystems[i].getHeader();
//Initialize itemNames rows
Expand Down Expand Up @@ -161,8 +167,12 @@ public void run() {
events = new ArrayList<>();
//We use a StringBuilder because it's better for building up a string via concatenation.
StringBuilder telemetryData = new StringBuilder();
//Loop through each datum

//Log the times
telemetryData.append(System.currentTimeMillis()-startTime).append(",");
telemetryData.append(Clock.currentTimeMillis()).append(",");

//Loop through each datum
for (int i = 0; i < subsystems.length; i++) {
Object[] data = subsystems[i].getData();
for (int j = 0; j < data.length; j++) {
Expand Down
105 changes: 17 additions & 88 deletions RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,6 @@
import com.fasterxml.jackson.annotation.JsonProperty;
import com.fasterxml.jackson.annotation.ObjectIdGenerators;
import org.jetbrains.annotations.NotNull;
import org.jetbrains.annotations.Nullable;
import org.usfirst.frc.team449.robot.Robot;
import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
import org.usfirst.frc.team449.robot.generalInterfaces.loggable.Loggable;
import org.usfirst.frc.team449.robot.jacksonWrappers.MappedRunnable;
Expand All @@ -21,12 +19,6 @@
@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
public class UnidirectionalPoseEstimator <T extends SubsystemNavX & DriveUnidirectional> implements MappedRunnable, Loggable {

/**
* The wheel-to-wheel diameter of the robot, in feet.
*/
@Nullable
private final Double robotDiameter;

/**
* The subsystem to get gyro and encoder data from.
*/
Expand Down Expand Up @@ -108,7 +100,6 @@ public class UnidirectionalPoseEstimator <T extends SubsystemNavX & DriveUnidire
/**
* Default constructor.
*
* @param robotDiameter The wheel-to-wheel diameter of the robot, in feet.
* @param subsystem The subsystem to get gyro and encoder data from.
* @param absolutePosAngleTolerance The maximum amount, in degrees, a new absolute position's angle can be off from
* the gyro reading and still be accepted as valid.
Expand All @@ -117,13 +108,11 @@ public class UnidirectionalPoseEstimator <T extends SubsystemNavX & DriveUnidire
* @param startTheta The starting angle of the robot, in degrees. Defaults to 0.
*/
@JsonCreator
public UnidirectionalPoseEstimator(@Nullable Double robotDiameter,
@JsonProperty(required = true) @NotNull T subsystem,
public UnidirectionalPoseEstimator(@JsonProperty(required = true) @NotNull T subsystem,
@JsonProperty(required = true) double absolutePosAngleTolerance,
double startX,
double startY,
double startTheta) {
this.robotDiameter = robotDiameter;
this.subsystem = subsystem;
this.absolutePosAngleTolerance = absolutePosAngleTolerance;
lastTheta = startTheta;
Expand All @@ -144,47 +133,29 @@ public UnidirectionalPoseEstimator(@Nullable Double robotDiameter,
lastTime = 0;
}

private static double[] calcEliVector(double left, double right, double deltaTheta, double lastAngle){
//The vector for how much the robot moves, element 0 is x and element 1 is y.
double[] vector = new double[2];

//If we're going in a straight line
if (deltaTheta == 0) {
//we could use deltaRight here, doesn't matter. Going straight means no change in angle and left and right are the same.
vector[0] = left * Math.cos(lastAngle);
vector[1] = left * Math.sin(lastAngle);
} else {
//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 vectorMagnitude = 2. * r * Math.sin(deltaTheta / 2.);
vector[0] = vectorMagnitude * Math.cos(vectorAngle);
vector[1] = vectorMagnitude * Math.sin(vectorAngle);
}
return vector;
}

private static double[] calcVector(double left, double right, double robotDiameter, double deltaTheta, double lastAngle) {
/**
* Calculate the x and y movement vector for the robot.
*
* @param left The displacement of the left encoder, in feet
* @param right The displacement of the right encoder, in feet
* @param deltaTheta The angular displacement, in radians
* @param lastAngle The previous heading, in radians
* @return An array of length 2 containing the [x, y] displacement of the robot.
*/
private static double[] calcVector(double left, double right, double deltaTheta, double lastAngle){
//The vector for how much the robot moves, element 0 is x and element 1 is y.
double[] vector = new double[2];

//If we're going in a straight line
if (deltaTheta == 0) {
//we could use deltaRight here, doesn't matter. Going straight means no change in angle and left and right are the same.
vector[0] = left * Math.cos(lastAngle);
vector[1] = left * Math.sin(lastAngle);
vector[0] = (left+right)/2. * Math.cos(lastAngle);
vector[1] = (left+right)/2. * Math.sin(lastAngle);
} else {
//This next part is too complicated to explain in comments. Read this wiki page instead:
// http://team449.shoutwiki.com/wiki/Pose_Estimation
double r;
if (left-right == 0){
r = left/deltaTheta;
} else {
r = robotDiameter / 2. * (left + right) / (left - right);
}
double vectorAngle = lastAngle + deltaTheta/2.;
double vectorMagnitude = 2. * r * Math.sin(deltaTheta / 2.);
double vectorMagnitude = 2. * ((left+right)/2.)/deltaTheta * Math.sin(deltaTheta / 2.);
vector[0] = vectorMagnitude * Math.cos(vectorAngle);
vector[1] = vectorMagnitude * Math.sin(vectorAngle);
}
Expand All @@ -206,53 +177,14 @@ public synchronized void run() {
double deltaLeft = left - lastLeftPos;
double deltaRight = right - lastRightPos;
double deltaTheta = theta - lastTheta;
double robotDiameter;

if (deltaTheta == 0){
fudgedWheelbaseDiameter = -1;
} else {
fudgedWheelbaseDiameter = (deltaLeft - deltaRight) / deltaTheta;
}

double[] vector;
if (this.robotDiameter != null) {
//Noah's Approach:

//For this next part, we assume that the gyro is 100% accurate at measuring the change in angle over the given

//time period and that the encoders will possibly overmeasure (due to wheel slip) but never undermeasure.
//Given those constraints, we have an overdetermined system because deltaTheta should be equal to
//(deltaLeft-deltaRight)/robotDiameter. We can use this to determine which wheel slipped more, and replace its
//reading with a value calculated from the other encoder and the gyro.
robotDiameter = this.robotDiameter;
if (deltaTheta < (deltaLeft - deltaRight) / robotDiameter) {
if (deltaLeft > 0) {
percentChanged = ((deltaRight + robotDiameter * deltaTheta) - deltaLeft) / deltaLeft;
deltaLeft = deltaRight + robotDiameter * deltaTheta;
recalcedLeft = true;
} else {
percentChanged = ((deltaLeft - robotDiameter * deltaTheta) - deltaRight) / deltaRight;
deltaRight = deltaLeft - robotDiameter * deltaTheta;
recalcedLeft = false;
}
} else if (deltaTheta > (deltaLeft - deltaRight) / robotDiameter) {
if (deltaLeft < 0) {
percentChanged = ((deltaRight + robotDiameter * deltaTheta) - deltaLeft) / deltaLeft;
deltaLeft = deltaRight + robotDiameter * deltaTheta;
recalcedLeft = true;
} else {
percentChanged = ((deltaLeft - robotDiameter * deltaTheta) - deltaRight) / deltaRight;
deltaRight = deltaLeft - robotDiameter * deltaTheta;
recalcedLeft = false;
}
}
vector = calcVector(deltaLeft, deltaRight, robotDiameter, deltaTheta, lastTheta);
} else {

//Eli's Approach

//Here we assume all the measured values are correct and adjust the diameter to match.
vector = calcEliVector(deltaLeft, deltaRight, deltaTheta, lastTheta);
}
double[] vector = calcVector(deltaLeft, deltaRight, deltaTheta, lastTheta);

//The vector for how much the robot moves, element 0 is x and element 1 is y.

Expand Down Expand Up @@ -447,9 +379,6 @@ public Object[] getData() {
@NotNull
@Override
public String getName() {
if(robotDiameter != null){
return "NoahPoseEstimator";
}
return "EliPoseEstimator";
return "PoseEstimator";
}
}
2 changes: 1 addition & 1 deletion RoboRIO/src/main/resources/calcifer_map.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -305,7 +305,7 @@ logger:
absolutePosAngleTolerance: 5
# - org.usfirst.frc.team449.robot.subsystem.complexooter.LoggingShooter:
# multiSubsystem
loopTimeSecs: 0.05
loopTimeSecs: 0.02
eventLogFilename: "/home/lvuser/logs/eventLog-"
telemetryLogFilename: "/home/lvuser/logs/telemetryLog-"
centerAuto:
Expand Down

0 comments on commit 8d72aa9

Please sign in to comment.