diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index bfb4bef..b85b57d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java @@ -53,7 +53,7 @@ public class Robot extends IterativeRobot { public static OI oi; public static Map> autoScripts; - + public static boolean auto; public static boolean stopIntake = false; Command autonomousCommand; @@ -210,6 +210,7 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { + auto = true; dt.resetAHRS(); AutoUtils.state = new State(0, 0, 0); Scheduler.getInstance().add(new ShiftLowGear()); @@ -239,6 +240,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + auto = false; System.out.println("In teleopInit()"); dt.resetAHRS(); AutoUtils.state = new State(0, 0, 0); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index bb50606..08648a8 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -73,6 +73,7 @@ public class RobotMap { public static DoubleSolenoid dtGear; private final double DIST_PER_PULSE_RATIO = (5.0 * Math.PI) * (17.0 / 25) / (3.0 * 256); + public final double gravity = 9.80665; /** * This function takes in a TalonSRX motorController and sets nominal and peak diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java new file mode 100644 index 0000000..51cb76f --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java @@ -0,0 +1,119 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; +import org.usfirst.frc.team199.Robot2018.RobotMap; +import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; +import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; +import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight; + +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Scheduler; +import jaci.pathfinder.Pathfinder; +import jaci.pathfinder.Trajectory; +import jaci.pathfinder.Waypoint; + +/** + * + */ +public class MotionProfilingMove extends Command { + + private double target; + private DrivetrainInterface dt; + private PIDSource avg; + private SmartDashboardInterface sd; + private double[] point; + private boolean usePoint; + Trajectory traj; + int i; + + public MotionProfilingMove(double target, double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, + PIDSource avg, boolean usePoint) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + this.point = point; + this.target = target; + this.usePoint = usePoint; + this.dt = dt; + this.avg = avg; + this.sd = sd; + if (Robot.dt != null) { + requires(Robot.dt); + } + } + + public MotionProfilingMove(double target, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) { + this(target, null, dt, sd, avg, false); + } + + public MotionProfilingMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) { + this(0, point, dt, sd, avg, true); + } + + // Called just before this Command runs the first time + protected void initialize() { + if (!usePoint) { + // use point because easier for motion profiling + point[0] = 0; + point[1] = target; + } + Waypoint[] pt = new Waypoint[1]; + pt[0] = new Waypoint(point[0], point[1], 0); + Trajectory.FitMethod fit = Robot.getBool("MP_CubicTraj", true) ? Trajectory.FitMethod.HERMITE_CUBIC : Trajectory.FitMethod.HERMITE_QUINTIC; + //0 samples is fast (1k), 1 is low (10k), and 2 is fast (100k) + int samples = findSamples(Robot.getConst("MP_AmtSamples", 0)); +// int samples = (int) Robot.getConst("MP_AmtSamples", 1000); + double timestep = Robot.rmap.getCycleTime(); + double maxVelocity = Robot.dt.getCurrentMaxSpeed(); + LiftHeight currPos = Robot.lift.getCurrPos(); + if (Robot.auto && currPos != LiftHeight.GROUND && currPos != LiftHeight.HOLD_CUBE) maxVelocity *= Robot.getConst("Auto Max Speed", 0.5); + double maxAccel = Robot.rmap.gravity; + //the acceleration = F/m, and F = coefficient of friction (1) * weight (mg), so a = mg/m = g + double maxJerk = Robot.getConst("MP_MaxJerk", 4); + //Kevin said that a good random small number was 4 + Trajectory.Config config = new Trajectory.Config(fit, samples, timestep, maxVelocity, maxAccel, maxJerk); + traj = Pathfinder.generate(pt, config); + i = 0; + //TODO Use the built-in modifications to make everything better + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Trajectory.Segment seg = traj.get(i); + double prevX = i == 0 ? 0 : traj.get(i-1).x; + double prevY = i == 0 ? 0 : traj.get(i-1).y; + double[] newPoint = new double[2]; + newPoint[0] = seg.x - prevX; + newPoint[1] = seg.y - prevY; + Scheduler.getInstance().add(new PIDMove(newPoint, dt, sd, avg)); + i++; + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return i >= traj.length(); + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } + + private int findSamples(double key) { + switch ((int)key) { + case 0: + return Trajectory.Config.SAMPLES_FAST; + case 1: + return Trajectory.Config.SAMPLES_LOW; + case 2: + return Trajectory.Config.SAMPLES_HIGH; + default: + return Trajectory.Config.SAMPLES_FAST; + } + } +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index 5bf87a0..10cbd9e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -4,6 +4,7 @@ import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; +import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; @@ -133,7 +134,13 @@ public void initialize() { dt.resetDistEncs(); moveController.disable(); // output in "motor units" (arcade and tank only accept values [-1, 1] - moveController.setOutputRange(-1.0, 1.0); + LiftHeight currPos = Robot.lift.getCurrPos(); + if (Robot.auto && currPos != LiftHeight.GROUND && currPos != LiftHeight.HOLD_CUBE) { + moveController.setOutputRange(Robot.getConst("Auto Min Speed", -0.5), + Robot.getConst("Auto Max Speed", 0.5)); + } else { + moveController.setOutputRange(-1.0, 1.0); + } moveController.setContinuous(false); moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.5)); System.out.println("move target = " + target);