From cb621a043237016207366be14c8d001e609ef447 Mon Sep 17 00:00:00 2001 From: doawelul Date: Sat, 17 Mar 2018 14:55:21 -0700 Subject: [PATCH 1/3] added quick clamp to movespeed when in auto and lift is up --- .../src/org/usfirst/frc/team199/Robot2018/Robot.java | 4 +++- .../usfirst/frc/team199/Robot2018/commands/PIDMove.java | 9 ++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index 80897aa..4e0272b 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; Command autonomousCommand; SendableChooser posChooser = new SendableChooser(); Map> stratChoosers = new HashMap>(); @@ -208,6 +208,7 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { + auto = true; dt.resetAHRS(); AutoUtils.state = new State(0, 0, 0); Scheduler.getInstance().add(new ShiftLowGear()); @@ -237,6 +238,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/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index b2897cb..5dab6eb 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; @@ -131,7 +132,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); From dfa7f506be09fb58fa11d3c3e9befc4292c61cfd Mon Sep 17 00:00:00 2001 From: doawelul Date: Sat, 17 Mar 2018 15:48:27 -0700 Subject: [PATCH 2/3] added file for motion profiling --- .../commands/MotionProfilingMove.java | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java 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..6c25206 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java @@ -0,0 +1,36 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class MotionProfilingMove extends Command { + + public MotionProfilingMove() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // 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() { + } +} From 5403e15956e8f67bac229d747f9756059156ccab Mon Sep 17 00:00:00 2001 From: doawelul Date: Thu, 22 Mar 2018 10:59:26 -0700 Subject: [PATCH 3/3] first commit of motion profiling (probably everything is wrong) --- .../frc/team199/Robot2018/RobotMap.java | 1 + .../commands/MotionProfilingMove.java | 125 +++++++++++++++--- 2 files changed, 105 insertions(+), 21 deletions(-) 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 index 6c25206..51cb76f 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/MotionProfilingMove.java @@ -1,36 +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 { - public MotionProfilingMove() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } + 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 just before this Command runs the first time - protected void initialize() { - } + // 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++; + } - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return i >= traj.length(); + } - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } + // Called once after isFinished returns true + protected void end() { + } - // 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(); + } - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } + 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; + } + } }