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 471a83e4..944c79d6 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 @@ -77,12 +77,20 @@ public static void main(String[] args) throws IOException { }; Waypoint[] redPegToKey = new Waypoint[]{ - new Waypoint(0, 0, 0), - new Waypoint((PEG_BASE_TO_CENTER*Math.cos(Math.toRadians(180)) + RED_WALL_TO_SIDE_PEG*Math.cos(Math.toRadians(-60)) + RED_KEY_CORNER_TO_SIDE_PEG*Math.cos(Math.toRadians(30)) - + RED_HALF_KEY_LENGTH*Math.cos(Math.toRadians(75)) + CENTER_TO_BACK*Math.cos(Math.toRadians(165)))/12., - (RED_WALL_TO_SIDE_PEG*Math.sin(Math.toRadians(-60)) + RED_KEY_CORNER_TO_SIDE_PEG*Math.sin(Math.toRadians(30)) - + RED_HALF_KEY_LENGTH*Math.sin(Math.toRadians(75)) + CENTER_TO_BACK*Math.sin(Math.toRadians(165)))/12., - -Math.toRadians(16)) +// new Waypoint(0, 0, 0), +// new Waypoint((PEG_BASE_TO_CENTER*Math.cos(Math.toRadians(180)) + RED_WALL_TO_SIDE_PEG*Math.cos(Math.toRadians(-60)) + RED_KEY_CORNER_TO_SIDE_PEG*Math.cos(Math.toRadians(30)) +// + RED_HALF_KEY_LENGTH*Math.cos(Math.toRadians(75)) + CENTER_TO_BACK*Math.cos(Math.toRadians(165)))/12., +// (RED_WALL_TO_SIDE_PEG*Math.sin(Math.toRadians(-60)) + RED_KEY_CORNER_TO_SIDE_PEG*Math.sin(Math.toRadians(30)) +// + RED_HALF_KEY_LENGTH*Math.sin(Math.toRadians(75)) + CENTER_TO_BACK*Math.sin(Math.toRadians(165)))/12., +// -Math.toRadians(16)) + new Waypoint((CENTER_TO_BACK*Math.cos(Math.PI)+(RED_BACK_CORNER_TO_SIDE_PEG-CENTER_TO_SIDE-RED_KEY_CORNER_TO_SIDE_PEG)*Math.cos(Math.PI/2)+ + RED_HALF_KEY_LENGTH*Math.cos(-Math.PI/4)+CENTER_TO_BACK*Math.cos(Math.PI/4))/12., + (CENTER_TO_BACK*Math.sin(Math.PI)+(RED_BACK_CORNER_TO_SIDE_PEG-CENTER_TO_SIDE-RED_KEY_CORNER_TO_SIDE_PEG)*Math.sin(Math.PI/2)+ + RED_HALF_KEY_LENGTH*Math.sin(-Math.PI/4)+CENTER_TO_BACK*Math.sin(Math.PI/4))/12.,Math.PI/4.), + 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[] bluePegToKey = new Waypoint[]{ diff --git a/R scripts/drawMP.R b/R scripts/drawMP.R index 42be277e..be670ceb 100644 --- a/R scripts/drawMP.R +++ b/R scripts/drawMP.R @@ -51,18 +51,10 @@ plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, center # 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) - } } else { #We do this with sectors, so this is the radius of the turning circle for the @@ -79,18 +71,11 @@ plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, center #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) - } + } } return(out) @@ -177,12 +162,12 @@ 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), 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 = "RedShoot",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]),]) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java index eb1e9a15..69896dbd 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java @@ -709,7 +709,7 @@ public void loadProfile(MotionProfileData data) { Logger.addEvent("Point " + Arrays.toString(data.getData()[i]) + " has an unattainable velocity+acceleration setpoint!", this.getClass()); } point.timeDurMs = (int) data.getData()[i][3]; - point.zeroPos = i == 0; // If it's the first point, set the encoder position to 0. + point.zeroPos = i == 0 && data.resetPosition(); // If it's the first point, set the encoder position to 0. point.isLastPoint = (i + 1) == data.getData().length; // If it's the last point, isLastPoint = true // Send the point to the Talon's buffer diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/MotionProfileData.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/MotionProfileData.java index 99381ee1..42d2c52f 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/MotionProfileData.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/MotionProfileData.java @@ -22,6 +22,11 @@ public class MotionProfileData { */ private final boolean inverted; + /** + * Whether or not to reset the talon position when this profile starts. + */ + private final boolean resetPosition; + /** * Whether to use position PID or not. */ @@ -41,13 +46,16 @@ public class MotionProfileData { * @param inverted Whether or not the profile is inverted (would be inverted if we're driving it backwards) * @param velocityOnly Whether or not to only use velocity feed-forward. Used for tuning kV and kA. Defaults to * false. + * @param resetPosition Whether or not to reset the talon position when this profile starts. */ @JsonCreator public MotionProfileData(@NotNull @JsonProperty(required = true) String filename, @JsonProperty(required = true) boolean inverted, - boolean velocityOnly) { + boolean velocityOnly, + boolean resetPosition) { this.inverted = inverted; this.velocityOnly = velocityOnly; + this.resetPosition = resetPosition; try { readFile(Robot.RESOURCES_PATH + filename); @@ -114,4 +122,11 @@ public boolean isVelocityOnly() { public boolean isInverted(){ return inverted; } + + /** + * @return Whether or not to reset the talon position when this profile starts. + */ + public boolean resetPosition() { + return resetPosition; + } } diff --git a/RoboRIO/src/main/resources/nate_map.yml b/RoboRIO/src/main/resources/nate_map.yml index 1528a809..31fbb402 100644 --- a/RoboRIO/src/main/resources/nate_map.yml +++ b/RoboRIO/src/main/resources/nate_map.yml @@ -6,6 +6,7 @@ leftTestProfile: '@id': leftTest filename: "forward100InProfile.csv" inverted: false + resetPosition: true rightTestProfile: <<: *leftTest '@id': rightTest @@ -14,51 +15,63 @@ leftProfiles: !!map '@id': blueLeftCenter filename: "calciferLeftBlueMidProfile.csv" inverted: false + resetPosition: true "blue_left" : '@id': blueLeftLeft filename: "calciferLeftBlueLeftProfile.csv" inverted: false + resetPosition: true "blue_right" : '@id': blueLeftRight filename: "calciferLeftBlueRightProfile.csv" inverted: false + resetPosition: true "red_center" : '@id': redLeftCenter filename: "calciferLeftRedMidProfile.csv" inverted: false + resetPosition: true "red_left" : '@id': redLeftLeft filename: "calciferLeftRedLeftProfile.csv" inverted: false + resetPosition: true "red_right" : '@id': redLeftRight filename: "calciferLeftRedRightProfile.csv" inverted: false + resetPosition: true rightProfiles: !!map "blue_center" : '@id': blueRightCenter filename: "calciferRightBlueMidProfile.csv" inverted: false + resetPosition: true "blue_left" : '@id': blueRightLeft filename: "calciferRightBlueLeftProfile.csv" inverted: false + resetPosition: true "blue_right" : '@id': blueRightRight filename: "calciferRightBlueRightProfile.csv" inverted: false + resetPosition: true "red_center" : '@id': redRightCenter filename: "calciferRightRedMidProfile.csv" inverted: false + resetPosition: true "red_left" : '@id': redRightLeft filename: "calciferRightRedLeftProfile.csv" inverted: false + resetPosition: true "red_right" : '@id': redRightRight filename: "calciferRightRedRightProfile.csv" inverted: false + resetPosition: true allianceSwitch: '@id': allianceSwitch ports: [3] diff --git a/RoboRIO/src/main/resources/naveen_map.yml b/RoboRIO/src/main/resources/naveen_map.yml index f664073f..c1c7240b 100644 --- a/RoboRIO/src/main/resources/naveen_map.yml +++ b/RoboRIO/src/main/resources/naveen_map.yml @@ -6,6 +6,7 @@ leftTestProfile: '@id': leftTest filename: "forward100InProfile.csv" inverted: false + resetPosition: true rightTestProfile: <<: *leftTest '@id': rightTest @@ -14,51 +15,63 @@ leftProfiles: !!map '@id': blueLeftCenter filename: "calciferLeftBlueMidProfile.csv" inverted: false + resetPosition: true "blue_left" : '@id': blueLeftLeft filename: "calciferLeftBlueLeftProfile.csv" inverted: false + resetPosition: true "blue_right" : '@id': blueLeftRight filename: "calciferLeftBlueRightProfile.csv" inverted: false + resetPosition: true "red_center" : '@id': redLeftCenter filename: "calciferLeftRedMidProfile.csv" inverted: false + resetPosition: true "red_left" : '@id': redLeftLeft filename: "calciferLeftRedLeftProfile.csv" inverted: false + resetPosition: true "red_right" : '@id': redLeftRight filename: "calciferLeftRedRightProfile.csv" inverted: false + resetPosition: true rightProfiles: !!map "blue_center" : '@id': blueRightCenter filename: "calciferRightBlueMidProfile.csv" inverted: false + resetPosition: true "blue_left" : '@id': blueRightLeft filename: "calciferRightBlueLeftProfile.csv" inverted: false + resetPosition: true "blue_right" : '@id': blueRightRight filename: "calciferRightBlueRightProfile.csv" inverted: false + resetPosition: true "red_center" : '@id': redRightCenter filename: "calciferRightRedMidProfile.csv" inverted: false + resetPosition: true "red_left" : '@id': redRightLeft filename: "calciferRightRedLeftProfile.csv" inverted: false + resetPosition: true "red_right" : '@id': redRightRight filename: "calciferRightRedRightProfile.csv" inverted: false + resetPosition: true allianceSwitch: '@id': allianceSwitch ports: [3]