Skip to content

Commit

Permalink
Add option to not reset position
Browse files Browse the repository at this point in the history
  • Loading branch information
Noah Gleason committed Oct 23, 2017
1 parent 3e68a8e commit bba6226
Show file tree
Hide file tree
Showing 6 changed files with 61 additions and 27 deletions.
20 changes: 14 additions & 6 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 @@ -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[]{
Expand Down
23 changes: 4 additions & 19 deletions R scripts/drawMP.R
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -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]),])
Expand Down
2 changes: 1 addition & 1 deletion RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
17 changes: 16 additions & 1 deletion RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/MotionProfileData.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
*/
Expand All @@ -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);
Expand Down Expand Up @@ -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;
}
}
13 changes: 13 additions & 0 deletions RoboRIO/src/main/resources/nate_map.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ leftTestProfile:
'@id': leftTest
filename: "forward100InProfile.csv"
inverted: false
resetPosition: true
rightTestProfile:
<<: *leftTest
'@id': rightTest
Expand All @@ -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]
Expand Down
13 changes: 13 additions & 0 deletions RoboRIO/src/main/resources/naveen_map.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ leftTestProfile:
'@id': leftTest
filename: "forward100InProfile.csv"
inverted: false
resetPosition: true
rightTestProfile:
<<: *leftTest
'@id': rightTest
Expand All @@ -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]
Expand Down

0 comments on commit bba6226

Please sign in to comment.