Skip to content

Commit

Permalink
Merge in master
Browse files Browse the repository at this point in the history
  • Loading branch information
Noah Gleason committed Nov 4, 2017
2 parents dd1d21f + 465ce86 commit d900242
Show file tree
Hide file tree
Showing 444 changed files with 14,597 additions and 6,911 deletions.
Binary file modified Pathgen/lib/Pathfinder-Java.jar
View file
Binary file not shown.
93 changes: 49 additions & 44 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 @@ -14,25 +14,27 @@
* Generates a motion profile that hits any number of waypoints.
*/
public class Pathgen {

public static void main(String[] args) throws IOException {

final double CENTER_TO_FRONT = 27./2.;
final double CENTER_TO_BACK = 27./2. + 3.25;
final double CENTER_TO_SIDE = 29./2. + 3.25;
final double CENTER_TO_FRONT = 27. / 2.;
final double CENTER_TO_BACK = 27. / 2. + 3.25;
final double CENTER_TO_SIDE = 29. / 2. + 3.25;
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,86 +45,86 @@ 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[]{
new Waypoint(0, 0, 0),
new Waypoint((BLUE_WALL_TO_CENTER_PEG - CENTER_TO_BACK - PEG_BASE_TO_CENTER)/12., 0, 0)
new Waypoint((BLUE_WALL_TO_CENTER_PEG - CENTER_TO_BACK - PEG_BASE_TO_CENTER) / 12., 0, 0)
};

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[]{
new Waypoint(0, 0, 0),
new Waypoint((RED_WALL_TO_CENTER_PEG - CENTER_TO_BACK - PEG_BASE_TO_CENTER)/12., 0, 0)
new Waypoint((RED_WALL_TO_CENTER_PEG - CENTER_TO_BACK - PEG_BASE_TO_CENTER) / 12., 0, 0)
};

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.,
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))
};

Waypoint[] bluePegToKey = new Waypoint[]{
new Waypoint(0, 0, 0),
new Waypoint((PEG_BASE_TO_CENTER*Math.cos(Math.toRadians(180)) + BLUE_WALL_TO_SIDE_PEG*Math.cos(Math.toRadians(60)) + BLUE_KEY_CORNER_TO_SIDE_PEG*Math.cos(Math.toRadians(-30))
+ BLUE_HALF_KEY_LENGTH*Math.cos(Math.toRadians(-75)) + CENTER_TO_BACK*Math.cos(Math.toRadians(-165)))/12.,
(BLUE_WALL_TO_SIDE_PEG*Math.sin(Math.toRadians(60)) + BLUE_KEY_CORNER_TO_SIDE_PEG*Math.sin(Math.toRadians(-30))
+ BLUE_HALF_KEY_LENGTH*Math.sin(Math.toRadians(-75)) + CENTER_TO_BACK*Math.sin(Math.toRadians(-165)))/12.,
new Waypoint((PEG_BASE_TO_CENTER * Math.cos(Math.toRadians(180)) + BLUE_WALL_TO_SIDE_PEG * Math.cos(Math.toRadians(60)) + BLUE_KEY_CORNER_TO_SIDE_PEG * Math.cos(Math.toRadians(-30))
+ BLUE_HALF_KEY_LENGTH * Math.cos(Math.toRadians(-75)) + CENTER_TO_BACK * Math.cos(Math.toRadians(-165))) / 12.,
(BLUE_WALL_TO_SIDE_PEG * Math.sin(Math.toRadians(60)) + BLUE_KEY_CORNER_TO_SIDE_PEG * Math.sin(Math.toRadians(-30))
+ BLUE_HALF_KEY_LENGTH * Math.sin(Math.toRadians(-75)) + CENTER_TO_BACK * Math.sin(Math.toRadians(-165))) / 12.,
Math.toRadians(16))
};

Waypoint[] backupRed = new Waypoint[]{
new Waypoint(0, 0, 0),
new Waypoint(3, 1, Math.PI/3)
new Waypoint(3, 1, Math.PI / 3)
};

Waypoint[] backupBlue = new Waypoint[]{
new Waypoint(0, 0, 0),
new Waypoint(3, -1, -Math.PI/3)
new Waypoint(3, -1, -Math.PI / 3)
};

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 All @@ -136,25 +138,28 @@ public static void main(String[] args) throws IOException {
profiles.put("BlueShoot", bluePegToKey);
profiles.put("RedBackup", backupRed);
profiles.put("BlueBackup", backupBlue);
profiles.put("forward100In", points);
profiles.put("BlueBackup", backupBlue);
profiles.put("BlueLoadingToLoading", blueLoadingToLoading);
profiles.put("BlueBoilerToLoading", blueBoilerToLoading);
profiles.put("RedLoadingToLoading", redLoadingToLoading);
profiles.put("RedBoilerToLoading", redBoilerToLoading);
// profiles.put("forward100In", points);

final String ROBOT_NAME = "calcifer";

//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

//433.415
double calciferWheelbase = 26./12.;
double calciferWheelbase = 26. / 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 All @@ -168,14 +173,14 @@ public static void main(String[] args) throws IOException {
lfw.write(tm.getLeftTrajectory().length() + "\n");
for (int i = 0; i < tm.getLeftTrajectory().length(); i++) {
lfw.write(tm.getLeftTrajectory().get(i).position + ",\t" + tm.getLeftTrajectory().get(i).velocity + ",\t"
+ tm.getLeftTrajectory().get(i).dt + ",");
+ tm.getLeftTrajectory().get(i).acceleration + ",\t" + tm.getLeftTrajectory().get(i).dt);
lfw.write("\n");
}

rfw.write(tm.getRightTrajectory().length() + "\n");
for (int i = 0; i < tm.getRightTrajectory().length(); i++) {
rfw.write(tm.getRightTrajectory().get(i).position + ",\t" + tm.getRightTrajectory().get(i).velocity + "," +
"\t" + tm.getRightTrajectory().get(i).dt + ",");
rfw.write(tm.getRightTrajectory().get(i).position + ",\t" + tm.getRightTrajectory().get(i).velocity +
",\t" + tm.getLeftTrajectory().get(i).acceleration + ",\t" + tm.getRightTrajectory().get(i).dt);
rfw.write("\n");
}

Expand Down
8 changes: 4 additions & 4 deletions Pathgen/src/main/R/drawMP.R → R scripts/drawMP.R
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
plotProfile <- function(profileName, inverted = FALSE, wheelbaseDiameter, centerToFront, centerToBack, centerToSide, startY = 0, startPos = c(-1,-1,-1,-1,-1), usePosition = TRUE){
left <- read.csv(paste("../../../../calciferLeft",profileName,"Profile.csv",sep=""), header=FALSE)
right <- read.csv(paste("../../../../calciferRight",profileName,"Profile.csv",sep=""), header=FALSE)
left <- read.csv(paste("../calciferLeft",profileName,"Profile.csv",sep=""), header=FALSE)
right <- read.csv(paste("../calciferRight",profileName,"Profile.csv",sep=""), header=FALSE)
startingCenter <- c(startY, centerToBack)
left$V1[1] <- 0
left$V2[1] <- 0
Expand Down Expand Up @@ -73,11 +73,11 @@ 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 <- (pi - theta)/2 - (pi/2 - perpendicular)
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.
vectorDistanceWithoutR <- sin(theta)/sin((pi-theta)/2)
vectorDistanceWithoutR <- 2*sin(theta/2)

#If inverted, swap which wheel gets which input
if(inverted){
Expand Down
0 Pathgen/src/main/R/field.csv → R scripts/field.csv
View file
File renamed without changes.
24 changes: 24 additions & 0 deletions R scripts/findVelVsVoltage.R
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
# Title : findVelVsVoltage
# Objective : Find the line or curve describing how
# Created by: Noah Gleason
# Created on: 9/30/17

trials = 1
voltages = seq(1.5,5,by=0.5)
#voltages = c(0.6, 2, 2.5)
raw <- read.csv("eventLog-2017.10.01.13.58.02.csv")
raw$class <- as.character(raw$class)
maxes <- raw[grep("class org.usfirst.frc.team449.robot.drive.unidirectional.commands.DetermineVelVsVoltage", raw$class), 3]
maxes <- as.numeric(as.vector(maxes))

model <- lm(maxes ~ voltages)
coeffs <- c(coefficients(model))
View(coeffs)
xInt <- -coeffs[1]/coeffs[2]

res <- resid(model)

plot(x=voltages, y=maxes, main = "Max Velocity vs. Voltage", xlab="Voltage (volts)", ylab="Max speed (feet/sec)", xlim=c(0,12), ylim=c(0,12))
abline(model)
plot(x=voltages, y=res, main="Residuals", xlab="Voltage (volts)", ylab="Residuals (feet/sec)", xlim=c(0,12))
abline(0,0)
Loading

0 comments on commit d900242

Please sign in to comment.