Team4388/2019-Hit-or-Miss
1
0
Fork 0
mirror of https://github.com/Team4388/2019-Hit-or-Miss.git synced 2026-06-09 00:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

added in parent code structure

Browse Source 
utility and drive changes, removed elevator, 2019 code worked on lukes computer, modeled off of parent code
This commit is contained in:
lukesta182
2019-02-16 17:14:32 -07:00
parent 9bca7665ce
commit 1381a6504c
62 changed files with 3937 additions and 2919 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java
+1 -1
View File
@@ -8,7 +8,7 @@ package org.usfirst.frc4388.robot;
*/
public class Constants {
public static double kLooperDt = 0.01;
public static double kDriveWheelDiameterInches = 6.04;
public static double kTrackLengthInches = 10;
public static double kTrackWidthInches = 26.5;
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
-3
View File
@@ -32,7 +32,6 @@ public class Robot extends IterativeRobot
public static final Elevator elevator = new Elevator();
public static final BallIntake ballIntake = new BallIntake();
@@ -58,7 +57,6 @@ public class Robot extends IterativeRobot
oi = OI.getInstance();
controlLoop.addLoopable(drive);
controlLoop.addLoopable(elevator);
operationModeChooser = new SendableChooser<OperationMode>();
@@ -144,7 +142,6 @@ public class Robot extends IterativeRobot
public void updateStatus() {
drive.updateStatus(operationMode);
elevator.updateStatus(operationMode);
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java
-60
View File
@@ -1,60 +0,0 @@
// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
import org.usfirst.frc4388.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
public class DriveStraightMP extends Command {
private double m_distanceInches;
private double m_maxVelocityInchesPerSec;
private boolean m_useGyroLock;
private boolean m_useAbsolute;
private double m_desiredAbsoluteAngle;
public DriveStraightMP(double distanceInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
requires(Robot.drive);
m_distanceInches = distanceInches;
m_maxVelocityInchesPerSec = maxVelocityInchesPerSec;
m_useGyroLock = useGyroLock;
m_useAbsolute = useAbsolute;
m_desiredAbsoluteAngle = desiredAbsoluteAngle;
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.drive.setStraightMP(m_distanceInches, m_maxVelocityInchesPerSec, m_useGyroLock, m_useAbsolute, m_desiredAbsoluteAngle);
}
// 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 Robot.drive.isFinished();
}
// Called once after isFinished returns true
protected void end() {
Robot.drive.setControlMode(DriveControlMode.JOYSTICK);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java
+1 -1
View File
@@ -4,7 +4,7 @@ import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
import org.usfirst.frc4388.utility.BHRMathUtils;
import org.usfirst.frc4388.utility.CANTalonEncoder;
import org.usfirst.frc4388.utility.TalonSRXEncoder;
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
import com.ctre.phoenix.motorcontrol.ControlMode;
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java
-99
View File
@@ -1,99 +0,0 @@
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.Robot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class ElevatorBasic extends Command {
private double m_targetHeightInchesAboveFloor;
private double m_speed;
private boolean m_goingUp;
private double m_commandInitTimestamp;
private double m_lastCommandExecuteTimestamp = 0.0;
private double m_lastCommandExecutePeriod = 0.0;
public static boolean isfinishedElevatorBasic;
public ElevatorBasic(double targetHeightInchesAboveFloor) {
requires(Robot.elevator);
m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor;
}
// Called just before this Command runs the first time
protected void initialize()
{
Robot.elevator.setControlMode(DriveControlMode.RAW);
double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
// start out at half speed, as crude way to reduce slippage
m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight);
System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN"));
if (m_goingUp) {
m_speed = Constants.kElevatorBasicPercentOutputUp;
}
else {
m_speed = Constants.kElevatorBasicPercentOutputDown;
}
m_commandInitTimestamp = Timer.getFPGATimestamp();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
// Measure *actual* update period
double currentTimestamp = Timer.getFPGATimestamp();
if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time
m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp;
}
m_lastCommandExecuteTimestamp = currentTimestamp;
Robot.elevator.rawSetOutput(m_speed);
//SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
boolean finished=false;
double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0);
System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor);
if (remaining < 0.0) {
finished = true;
}
/*} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms
velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed
} else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms
velocity = m_maxVelocityInchesPerSec / 2.0; // half speed
}*/
if (!finished) {
SmartDashboard.putNumber("EB Dist", currentHeight);
} else {
SmartDashboard.putNumber("EB finDist", currentHeight);
}
return finished;
}
// Called once after isFinished returns true
protected void end() {
double currentTimestamp = Timer.getFPGATimestamp();
SmartDashboard.putNumber("EB Runtime", currentTimestamp - m_commandInitTimestamp);
isfinishedElevatorBasic = isFinished();
Robot.elevator.rawSetOutput(0.0);
Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java
+2 -2
View File
@@ -3,8 +3,8 @@ package org.usfirst.frc4388.robot.subsystems;
import org.usfirst.frc4388.robot.RobotMap;
import org.usfirst.frc4388.robot.commands.*;
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
import org.usfirst.frc4388.utility.CANTalonEncoder;
import org.usfirst.frc4388.utility.ControlLoopable;
import org.usfirst.frc4388.utility.TalonSRXEncoder;
import org.usfirst.frc4388.utility.Looper;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.usfirst.frc4388.robot.OI;
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java
+3 -3
View File
@@ -12,10 +12,10 @@ import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.RobotMap;
import org.usfirst.frc4388.robot.commands.*;
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
import org.usfirst.frc4388.utility.CANTalonEncoder;
import org.usfirst.frc4388.utility.ControlLoopable;
import org.usfirst.frc4388.utility.TalonSRXEncoder;
import org.usfirst.frc4388.utility.Looper;
import org.usfirst.frc4388.utility.PIDParams;
import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
import org.usfirst.frc4388.utility.SoftwarePIDController;
import com.ctre.phoenix.motorcontrol.ControlMode;
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
+47 -14
View File
@@ -8,11 +8,11 @@ import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.OI;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.RobotMap;
import org.usfirst.frc4388.utility.AdaptivePurePursuitController;
import org.usfirst.frc4388.utility.control.AdaptivePurePursuitController;
import org.usfirst.frc4388.utility.BHRMathUtils;
import org.usfirst.frc4388.utility.CANTalonEncoder;
//import org.usfirst.frc4388.utility.CANTalonEncoder;
import org.usfirst.frc4388.utility.ControlLoopable;
import org.usfirst.frc4388.utility.Kinematics;
import org.usfirst.frc4388.utility.control.Kinematics;
import org.usfirst.frc4388.utility.MPSoftwarePIDController;
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
import org.usfirst.frc4388.utility.MPTalonPIDController;
@@ -22,12 +22,12 @@ import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController;
import org.usfirst.frc4388.utility.MotionProfilePoint;
//import org.usfirst.frc4388.utility.MotionProfileBoxCar;
import org.usfirst.frc4388.utility.PIDParams;
import org.usfirst.frc4388.utility.Path;
import org.usfirst.frc4388.utility.PathGenerator;
import org.usfirst.frc4388.utility.RigidTransform2d;
import org.usfirst.frc4388.utility.Rotation2d;
//import org.usfirst.frc4388.utility.Path;
//import org.usfirst.frc4388.utility.PathGenerator;
import org.usfirst.frc4388.utility.math.RigidTransform2d;
import org.usfirst.frc4388.utility.math.Rotation2d;
import org.usfirst.frc4388.utility.SoftwarePIDController;
import org.usfirst.frc4388.utility.Translation2d;
import org.usfirst.frc4388.utility.math.Translation2d;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.ControlMode;
@@ -373,12 +373,33 @@ public class Drive extends Subsystem implements ControlLoopable
// }
//}
/*
public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
mpStraightController.setPID(mpStraightPIDParams);
mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true);
setControlMode(DriveControlMode.MP_STRAIGHT);
}
*/
//public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
// double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
// mpStraightController.setPID(mpStraightPIDParams);
@@ -416,7 +437,7 @@ public class Drive extends Subsystem implements ControlLoopable
pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType);
setControlMode(DriveControlMode.PID_TURN);
}
/*
public void setPathMP(PathGenerator path) {
mpPathController.setPID(mpPathPIDParams);
mpPathController.setMPPathTarget(path);
@@ -438,7 +459,7 @@ public class Drive extends Subsystem implements ControlLoopable
adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25);
setControlMode(DriveControlMode.ADAPTIVE_PURSUIT);
}
*/
public void setDriveHold(boolean status) {
if (status) {
setControlMode(DriveControlMode.HOLD);
@@ -469,13 +490,15 @@ public class Drive extends Subsystem implements ControlLoopable
*
* @return Set of Strings with Path Markers that the robot has crossed.
*/
/*
public synchronized Set<String> getPathMarkersCrossed() {
if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) {
return null;
} else {
return adaptivePursuitController.getMarkersCrossed();
}
}
}*/
public synchronized void setControlMode(DriveControlMode controlMode) {
this.controlMode = controlMode;
@@ -497,6 +520,16 @@ public class Drive extends Subsystem implements ControlLoopable
leftDrive1.set(0); //TODO URGENT: make sure not called when robot moving
rightDrive1.set(0); //TODO URGENT: make sure not called when robot moving
}
/*
else if (controlMode == DriveControlMode.HOLD) {
mpStraightController.setPID(mpHoldPIDParams);
//leftDrive1.changeControlMode(TalonControlMode.Position);
@@ -509,7 +542,7 @@ public class Drive extends Subsystem implements ControlLoopable
leftDrive1.set(0);
//rightDrive1.setSelectedSensorPosition(0, 0, 0); //not needed for spark? TODO: verify want 0="Primary closed-loop", with no timeout
rightDrive1.set(0);
}
}*/
isFinished = false;
}
@@ -538,13 +571,13 @@ public class Drive extends Subsystem implements ControlLoopable
else if (controlMode == DriveControlMode.MP_PATH) {
isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg());
}
else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) {
/*else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) {
isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
}
else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) {
updatePose();
isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
}
}*/
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java
-450
View File
@@ -1,450 +0,0 @@
package org.usfirst.frc4388.robot.subsystems;
import java.util.ArrayList;
import org.usfirst.frc4388.controller.XboxController;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.RobotMap;
import org.usfirst.frc4388.robot.commands.*;
import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
import org.usfirst.frc4388.utility.CANTalonEncoder;
import org.usfirst.frc4388.utility.ControlLoopable;
import org.usfirst.frc4388.utility.PIDParams;
import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
import com.ctre.phoenix.motorcontrol.ControlMode;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.SensorCollection;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
public class Elevator extends Subsystem implements ControlLoopable
{
//PID encoder and motor
private CANTalonEncoder elevatorRight;
private WPI_TalonSRX elevatorLeft;
//PID controller Max Scale
private SoftwarePIDPositionController pidPositionControllerMaxScale;
//private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0);
private PIDParams PositionPMaxScale;
//PID controller Max Scale
private SoftwarePIDPositionController pidPositionControllerLowScale;
//private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0);
private PIDParams PositionPLowScale;
//PID controller Max Scale
private SoftwarePIDPositionController pidPositionControllerSwitch;
//private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0);
private PIDParams PositionPSwitch;
//PID controller Max Scale
private SoftwarePIDPositionController pidPositionControllerLowest;
//private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0);
private PIDParams PositionPLowest;
//PID target
private double targetPPosition;
//Encoder ticks to inches for encoders
public static final double ENCODER_TICKS_TO_INCHES = Constants.kElevatorEncoderTicksPerInch;
//control mode for joystick control
private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
private double periodMs;
//Non Linear Joystick
public static final double STICK_DEADBAND = 0.02;
public static final double MOVE_NON_LINEARITY = 1.0;
private int moveNonLinear = 0;
private double moveScale = 1.0;
private double moveTrim = 0.0;
private boolean isFinished;
private DifferentialDrive m_drive;
//private LimitSwitchSource limitSwitch = new DigitalInput(1);
LimitSwitchSource limitSwitchSource;
public boolean pressed;
SensorCollection isPressed;
public boolean elevatorControlMode = false;
// Motor controllers
//private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
public Elevator()
{
try
{
//PID elevator encoder and talon
elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID);
elevatorRight.setInverted(false);
//Setting left elevator motor as follower
elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID());
elevatorLeft.setInverted(false);
elevatorLeft.setNeutralMode(NeutralMode.Brake);
elevatorRight.setNeutralMode(NeutralMode.Brake);
elevatorRight.setSensorPhase(true);
//Limit Switch Left
//elevatorLeft.overrideLimitSwitchesEnable(true);
elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
//Limit Switch Right
//elevatorRight.overrideLimitSwitchesEnable(true);
//elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
//elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
//Change This boi
// elevatorRight.configForwardSoftLimitThreshold(10000, 0); //right here
//elevatorRight.configReverseSoftLimitThreshold(5, 0);
//elevatorRight.configForwardSoftLimitEnable(true, 0);
//elevatorRight.configReverseSoftLimitEnable(true, 0);
//sos
//elevatorRight.enableLimitSwitch(true, true);
}
catch(Exception e)
{
System.err.println("You thought the code would work, but it was me, error. An error occurred in the Elevator Construtor");
}
}
private double nonlinearStickCalcPositive(double move, double moveNonLinearity)
{
return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity);
}
private double nonlinearStickCalcNegative(double move, double moveNonLinearity)
{
return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity);
}
private boolean inDeadZone(double input)
{
boolean inDeadZone;
if (Math.abs(input) < STICK_DEADBAND)
{
inDeadZone = true;
}
else
{
inDeadZone = false;
}
return inDeadZone;
}
private double limitValue(double value)
{
if (value > 1.0)
{
value = 1.0;
}
else if (value < -1.0)
{
value = -1.0;
}
return value;
}
public double adjustJoystickSensitivity(double scale, double trim, double move, int nonLinearFactor, double wheelNonLinearity)
{
if (inDeadZone(move))
{
return 0;
}
move += trim;
move *= scale;
move = limitValue(move);
int iterations = Math.abs(nonLinearFactor);
for (int i = 0; i < iterations; i++)
{
if (nonLinearFactor > 0)
{
move = nonlinearStickCalcPositive(move, wheelNonLinearity);
}
else
{
move = nonlinearStickCalcNegative(move, wheelNonLinearity);
}
}
return move;
}
public void setElevatorMode()
{
setControlMode(DriveControlMode.JOYSTICK);
}
public void resetElevatorEncoder()
{
elevatorRight.setSelectedSensorPosition(0, 0, 0);
}
public void moveElevatorXbox()
{
double moveElevatorInput;
double elevatorSafeZone = 15;
double elevatorPos = getEncoderElevatorPosition();
//moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis();
//double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY);
boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON);
boolean elevatorTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON);
if(elevatorTuningPressed == true)
{
//elevatorRight.set(moveElevatorInput * 0.5);
}
else if(elevatorTuningPressed == false)
{
//elevatorRight.set(moveElevatorInput);
}
/*
if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0)
{
elevatorRight.set(moveElevatorInput);
}
else if(elevatorPos > elevatorSafeZone)
{
elevatorRight.set(moveElevatorInput * 0.65);
if(holdButtonPressed == true)
{
elevatorRight.set(-0.43 * (0.2));
}
else if(holdButtonPressed == false)
{
elevatorRight.set(moveElevatorInput * 0.75);
}
}
else if(elevatorPos < 0)
{
elevatorRight.set(moveElevatorInput * 0.75);
}
*/
}
// System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range
//PID encoder position
public double getEncoderElevatorPosition()
{
return elevatorRight.getPositionWorld();
}
public double getElevatorHeightInchesAboveFloor()
{
return elevatorRight.getPositionWorld();
}
public synchronized void setControlMode(DriveControlMode controlMode)
{
this.controlMode = controlMode;
isFinished = false;
}
/*
public void setElevatorPIDMaxScale(double ElevatorPosition, double maxError, double minError)
{
double elevatorTargetPos = 0;
this.targetPPosition = elevatorTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); ///////TARGET POSITION WHERE??
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE);
}
public void setElevatorPIDLowScale(double ElevatorPosition, double maxError, double minError)
{
double elevatorTargetPos = 0;
this.targetPPosition = elevatorTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
}
public void setElevatorPIDSwitch(double ElevatorPosition, double maxError, double minError)
{
double elevatorTargetPos = 0;
this.targetPPosition = elevatorTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
}
public void setElevatorPIDLowest(double ElevatorPosition, double maxError, double minError)
{
double elevatorTargetPos = 0;
this.targetPPosition = elevatorTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError);
Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
}
*/
public void rawSetOutput(double output){
elevatorRight.set(/*ControlMode.PercentOutput,*/ output);
}
public void holdInPos()
{
elevatorRight.set(-0.43 * 0.2);
}
public void stopMotors()
{
elevatorRight.set(0);
}
public void isSwitchPressed()
{
pressed = false;
isPressed = elevatorRight.getSensorCollection();
if(isPressed.isFwdLimitSwitchClosed() == true)
{
if (controlMode == DriveControlMode.JOYSTICK) {
Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS);
}
pressed = true;
}
else
{
if (controlMode == DriveControlMode.STOP_MOTORS){
{
Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
}
pressed = false;
}
}
}
//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
@Override
public void controlLoopUpdate()
{
if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB)
{
moveElevatorXbox();
}
else if (!isFinished)
{
//PID control mode
if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE)
{
isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderElevatorPosition());
}
else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE)
{
isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderElevatorPosition());
}
else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH)
{
isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderElevatorPosition());
}
else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST)
{
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
}
/*
else if(controlMode == DriveControlMode.RAW)
{
isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
}
*/
}
}
@Override
public void initDefaultCommand()
{
}
@Override
public void periodic()
{
// isSwitchPressed();
}
@Override
public void setPeriodMs(long periodMs)
{
//PID controller
pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, elevatorRight);
pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, elevatorRight);
pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, elevatorRight);
pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, elevatorRight);
this.periodMs = periodMs;
}
public synchronized boolean isFinished()
{
return isFinished;
}
public double getPeriodMs()
{
return periodMs;
}
public void updateStatus(Robot.OperationMode operationMode)
{
if (operationMode == Robot.OperationMode.TEST)
{
try
{
SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0));
SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor());
//SmartDashboard.putData(pressed);
}
catch (Exception e)
{
System.err.println("Drivetrain update status error");
}
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java
-228
View File
@@ -1,228 +0,0 @@
package org.usfirst.frc4388.utility;
import java.util.ArrayList;
import java.util.Optional;
import java.util.Set;
import org.usfirst.frc4388.robot.Constants;
import com.ctre.phoenix.motorcontrol.ControlMode;
import edu.wpi.first.wpilibj.Timer;
/**
* Implements an adaptive pure pursuit controller. See:
* https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4
* .pdf
*
* Basically, we find a spot on the path we'd like to follow and calculate the
* wheel speeds necessary to make us land on that spot. The target spot is a
* specified distance ahead of us, and we look further ahead the greater our
* tracking error.
*/
public class AdaptivePurePursuitController {
private static final double kEpsilon = 1E-9;
double mFixedLookahead;
Path mPath;
RigidTransform2d.Delta mLastCommand;
double mLastTime;
double mMaxAccel;
double mDt;
boolean mReversed;
double mPathCompletionTolerance;
protected ArrayList<CANTalonEncoder> motorControllers;
protected long periodMs;
protected PIDParams pidParams;
protected double startGyroAngle;
protected double targetGyroAngle;
protected double trackDistance;
public AdaptivePurePursuitController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers)
{
this.motorControllers = motorControllers;
this.periodMs = periodMs;
setPID(pidParams);
}
public void setPID(PIDParams pidParams) {
this.pidParams = pidParams;
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
//motorController.setF(pidParams.kF);
//motorController.configNominalOutputVoltage(+0.0f, -0.0f);
//motorController.configPeakOutputVoltage(+12.0f, -12.0f);
//motorController.setProfile(0);
motorController.config_kP(0, pidParams.kP, 0);
motorController.config_kI(0, pidParams.kI, 0);
motorController.config_kD(0, pidParams.kD, 0);
motorController.config_kF(0, pidParams.kF, 0);
motorController.configNominalOutputForward(+0.0f, 0);
motorController.configNominalOutputReverse(-0.0f, 0);
motorController.configPeakOutputForward(+1.0f, 0);
motorController.configPeakOutputReverse(-1.0f, 0);
motorController.selectProfileSlot(0, 0);
}
}
public void setPath(double fixed_lookahead, double max_accel, Path path,
boolean reversed, double path_completion_tolerance) {
mFixedLookahead = fixed_lookahead;
mMaxAccel = max_accel;
mPath = path;
mDt = periodMs;
mLastCommand = null;
mReversed = reversed;
mPathCompletionTolerance = path_completion_tolerance;
// Set up the motion profile
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
//motorController.set(0);
//motorController.changeControlMode(TalonControlMode.Speed);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
motorController.set(ControlMode.Velocity, 0);
}
}
public boolean isDone() {
double remainingLength = mPath.getRemainingLength();
return remainingLength <= mPathCompletionTolerance;
}
public boolean controlLoopUpdate(RigidTransform2d robot_pose) {
RigidTransform2d.Delta command = update(robot_pose, Timer.getFPGATimestamp());
Kinematics.DriveVelocity setpoint = Kinematics.inverseKinematics(command);
// Scale the command to respect the max velocity limits
double max_vel = 0.0;
max_vel = Math.max(max_vel, Math.abs(setpoint.left));
max_vel = Math.max(max_vel, Math.abs(setpoint.right));
if (max_vel > Constants.kPathFollowingMaxVel) {
double scaling = Constants.kPathFollowingMaxVel / max_vel;
setpoint = new Kinematics.DriveVelocity(setpoint.left * scaling, setpoint.right * scaling);
}
// Update the controllers Kf and set point.
for (CANTalonEncoder motorController : motorControllers) {
if (motorController.isRight()) {
motorController.setVelocityWorld(-setpoint.right);
}
else {
motorController.setVelocityWorld(-setpoint.left);
}
}
return isDone();
}
public RigidTransform2d.Delta update(RigidTransform2d robot_pose, double now) {
RigidTransform2d pose = robot_pose;
if (mReversed) {
pose = new RigidTransform2d(robot_pose.getTranslation(),
robot_pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI)));
}
double distance_from_path = mPath.update(robot_pose.getTranslation());
if (this.isDone()) {
return new RigidTransform2d.Delta(0, 0, 0);
}
PathSegment.Sample lookahead_point = mPath.getLookaheadPoint(robot_pose.getTranslation(),
distance_from_path + mFixedLookahead);
Optional<Circle> circle = joinPath(pose, lookahead_point.translation);
double speed = lookahead_point.speed;
if (mReversed) {
speed *= -1;
}
// Ensure we don't accelerate too fast from the previous command
double dt = now - mLastTime;
if (mLastCommand == null) {
mLastCommand = new RigidTransform2d.Delta(0, 0, 0);
dt = mDt;
}
double accel = (speed - mLastCommand.dx) / dt;
if (accel < -mMaxAccel) {
speed = mLastCommand.dx - mMaxAccel * dt;
} else if (accel > mMaxAccel) {
speed = mLastCommand.dx + mMaxAccel * dt;
}
// Ensure we slow down in time to stop
// vf^2 = v^2 + 2*a*d
// 0 = v^2 + 2*a*d
double remaining_distance = mPath.getRemainingLength();
double max_allowed_speed = Math.sqrt(2 * mMaxAccel * remaining_distance);
if (Math.abs(speed) > max_allowed_speed) {
speed = max_allowed_speed * Math.signum(speed);
}
final double kMinSpeed = 4.0;
if (Math.abs(speed) < kMinSpeed) {
// Hack for dealing with problems tracking very low speeds with
// Talons
speed = kMinSpeed * Math.signum(speed);
}
RigidTransform2d.Delta rv;
if (circle.isPresent()) {
rv = new RigidTransform2d.Delta(speed, 0,
(circle.get().turn_right ? -1 : 1) * Math.abs(speed) / circle.get().radius);
} else {
rv = new RigidTransform2d.Delta(speed, 0, 0);
}
mLastTime = now;
mLastCommand = rv;
return rv;
}
public Set<String> getMarkersCrossed() {
return mPath.getMarkersCrossed();
}
public static class Circle {
public final Translation2d center;
public final double radius;
public final boolean turn_right;
public Circle(Translation2d center, double radius, boolean turn_right) {
this.center = center;
this.radius = radius;
this.turn_right = turn_right;
}
}
public static Optional<Circle> joinPath(RigidTransform2d robot_pose, Translation2d lookahead_point) {
double x1 = robot_pose.getTranslation().getX();
double y1 = robot_pose.getTranslation().getY();
double x2 = lookahead_point.getX();
double y2 = lookahead_point.getY();
Translation2d pose_to_lookahead = robot_pose.getTranslation().inverse().translateBy(lookahead_point);
double cross_product = pose_to_lookahead.getX() * robot_pose.getRotation().sin()
- pose_to_lookahead.getY() * robot_pose.getRotation().cos();
if (Math.abs(cross_product) < kEpsilon) {
return Optional.empty();
}
double dx = x1 - x2;
double dy = y1 - y2;
double my = (cross_product > 0 ? -1 : 1) * robot_pose.getRotation().cos();
double mx = (cross_product > 0 ? 1 : -1) * robot_pose.getRotation().sin();
double cross_term = mx * dx + my * dy;
if (Math.abs(cross_term) < kEpsilon) {
// Points are colinear
return Optional.empty();
}
return Optional.of(new Circle(
new Translation2d((mx * (x1 * x1 - x2 * x2 - dy * dy) + 2 * my * x1 * dy) / (2 * cross_term),
(-my * (-y1 * y1 + y2 * y2 + dx * dx) + 2 * mx * y1 * dx) / (2 * cross_term)),
.5 * Math.abs((dx * dx + dy * dy) / cross_term), cross_product > 0));
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java
-65
View File
@@ -1,65 +0,0 @@
package org.usfirst.frc4388.utility;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
public class CANTalonEncoder extends WPI_TalonSRX
{
private double encoderTicksToWorld;
private boolean isRight = true;
public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
this(deviceNumber, encoderTicksToWorld, false, feedbackDevice);
}
public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) {
super(deviceNumber);
//this.setFeedbackDevice(feedbackDevice);
this.configSelectedFeedbackSensor(feedbackDevice, 0, 0);
this.encoderTicksToWorld = encoderTicksToWorld;
this.isRight = isRight;
}
public boolean isRight() {
return isRight;
}
public void setRight(boolean isRight) {
this.isRight = isRight;
}
public double convertEncoderTicksToWorld(double encoderTicks) {
return encoderTicks / encoderTicksToWorld;
}
public double convertEncoderWorldToTicks(double worldValue) {
return worldValue * encoderTicksToWorld;
}
public void setWorld(double worldValue) {
//this.set(convertEncoderWorldToTicks(worldValue));
this.set(getControlMode(), convertEncoderWorldToTicks(worldValue)); //TODO: change to explicit mode set?
}
public void setPositionWorld(double worldValue) {
//this.setPosition(convertEncoderWorldToTicks(worldValue));
this.setSelectedSensorPosition((int)convertEncoderWorldToTicks(worldValue), 0, 0); //TODO: verify
}
public double getPositionWorld() {
//return convertEncoderTicksToWorld(this.getPosition());
return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0)); //TODO: verify want 0="Primary closed-loop"
}
public void setVelocityWorld(double worldValue) {
//this.set(convertEncoderWorldToTicks(worldValue) * 0.1);
this.set(getControlMode(), convertEncoderWorldToTicks(worldValue) * 0.1); //TODO: change to explicit mode set?
}
public double getVelocityWorld() {
//return convertEncoderTicksToWorld(this.getSpeed() / 0.1);
return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1); //TODO: verify want 0="Primary closed-loop"
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java
+20 -3
View File
@@ -6,6 +6,8 @@ import java.io.FileWriter;
import java.io.IOException;
import java.lang.reflect.Field;
import java.math.BigDecimal;
import java.nio.file.Files;
import java.nio.file.StandardOpenOption;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
@@ -19,8 +21,8 @@ import org.json.simple.parser.ParseException;
/**
* ConstantsBase
*
* Base class for storing robot constants. Anything stored as a public static
* field will be reflected and be able to set externally
* Base class for storing robot constants. Anything stored as a public static field will be reflected and be able to set
* externally
*/
public abstract class ConstantsBase {
HashMap<String, Boolean> modifiedKeys = new HashMap<String, Boolean>();
@@ -54,6 +56,18 @@ public abstract class ConstantsBase {
return new File(filePath);
}
public boolean truncateUserConstants() {
try {
Files.write(getFile().toPath(), new byte[0], StandardOpenOption.TRUNCATE_EXISTING);
loadFromFile();
return true;
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
return false;
}
}
public boolean setConstant(String name, Double value) {
return setConstantRaw(name, value);
}
@@ -77,6 +91,9 @@ public abstract class ConstantsBase {
success = true;
if (!value.equals(current)) {
modifiedKeys.put(name, true);
System.out.println("Constant Modified:" + field.getName());
} else {
System.out.println("Constant Not Modified:" + field.getName());
}
} catch (IllegalArgumentException | IllegalAccessException e) {
System.out.println("Could not set field: " + name);
@@ -116,7 +133,7 @@ public abstract class ConstantsBase {
public Collection<Constant> getConstants() {
List<Constant> constants = (List<Constant>) getAllConstants();
int stop = constants.size() - 1;
int stop = constants.size();
for (int i = 0; i < constants.size(); ++i) {
Constant c = constants.get(i);
if ("kEndEditableArea".equals(c.name)) {
2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTracker.java
+67
View File
@@ -0,0 +1,67 @@
package org.usfirst.frc4388.utility;
import java.io.FileWriter;
import java.io.IOException;
import java.io.PrintWriter;
import java.util.Date;
import java.util.UUID;
/**
* Tracks start-up and caught crash events, logging them to a file which dosn't roll over
*/
public class CrashTracker {
private static final UUID RUN_INSTANCE_UUID = UUID.randomUUID();
public static void logRobotStartup() {
logMarker("robot startup");
}
public static void logRobotConstruction() {
logMarker("robot startup");
}
public static void logRobotInit() {
logMarker("robot init");
}
public static void logTeleopInit() {
logMarker("teleop init");
}
public static void logAutoInit() {
logMarker("auto init");
}
public static void logDisabledInit() {
logMarker("disabled init");
}
public static void logThrowableCrash(Throwable throwable) {
logMarker("Exception", throwable);
}
private static void logMarker(String mark) {
logMarker(mark, null);
}
private static void logMarker(String mark, Throwable nullableException) {
try (PrintWriter writer = new PrintWriter(new FileWriter("/home/lvuser/crash_tracking.txt", true))) {
writer.print(RUN_INSTANCE_UUID.toString());
writer.print(", ");
writer.print(mark);
writer.print(", ");
writer.print(new Date().toString());
if (nullableException != null) {
writer.print(", ");
nullableException.printStackTrace(writer);
}
writer.println();
} catch (IOException e) {
e.printStackTrace();
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/CrashTrackingRunnable.java
+19
View File
@@ -0,0 +1,19 @@
package org.usfirst.frc4388.utility;
/**
* Runnable class with reports all uncaught throws to CrashTracker
*/
public abstract class CrashTrackingRunnable implements Runnable {
@Override
public final void run() {
try {
runCrashTracked();
} catch (Throwable t) {
CrashTracker.logThrowableCrash(t);
throw t;
}
}
public abstract void runCrashTracked();
}
2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java
-894
View File
@@ -1,894 +0,0 @@
package org.usfirst.frc4388.utility;
import java.awt.AWTException;
import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.FontMetrics;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Image;
import java.awt.Rectangle;
import java.awt.RenderingHints;
import java.awt.Robot;
import java.awt.Stroke;
import java.awt.Toolkit;
import java.awt.datatransfer.Clipboard;
import java.awt.datatransfer.ClipboardOwner;
import java.awt.datatransfer.DataFlavor;
import java.awt.datatransfer.Transferable;
import java.awt.datatransfer.UnsupportedFlavorException;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.MouseAdapter;
import java.awt.event.MouseEvent;
import java.awt.geom.AffineTransform;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Line2D;
import java.awt.image.BufferedImage;
import java.io.IOException;
import java.text.DecimalFormat;
import java.util.LinkedList;
import javax.swing.JFrame;
import javax.swing.JMenuItem;
import javax.swing.JPanel;
import javax.swing.JPopupMenu;
/**
* This class is a basic plotting class using the Java AWT interface. It has basic features which allow the user
* to plot multiple graphs on one figure, control axis dimensions, and specify colors.
*
* This is by all means not an extensive plotter, but it will help visualize data very quickly and accurately. If
* a more robust plotting function is required, the user is encouraged to use Excel or Matlab. The purpose of this
* class is to be easy to use with enought automation to have nice graphs with minimal effort, but give the user
* control over as much as possible, so they can generate the perfect chart.
*
* The plotter also features the ability to capture screen shots directly from the right-click menu, this allows
* the user to copy and paste plots into reports or other documents rather quickly.
*
* This class holds an interface similar to that of Matlab.
*
* This class currently only supports scatterd line charts.
*
* @author Kevin Harrilal
* @email kevin@team2168.org
* @version 1
* @date 9 Sept 2014
*
*/
class FalconLinePlot extends JPanel implements ClipboardOwner{
private static final long serialVersionUID = 3205256608145459434L;
private final int yPAD = 60; //controls how far the X- and Y- axis lines are away from the window edge
private final int xPAD = 70; //controls how far the X- and Y- axis lines are away from the window edge
private double upperXtic;
private double lowerXtic;
private double upperYtic;
private double lowerYtic;
private boolean yGrid;
private boolean xGrid;
private double yMax;
private double yMin;
private double xMax;
private double xMin;
private int yticCount;
private int xticCount;
private double xTicStepSize;
private double yTicStepSize;
boolean userSetYTic;
boolean userSetXTic;
private String xLabel;
private String yLabel;
private String titleLabel;
protected static int count = 0;
JPopupMenu menu = new JPopupMenu("Popup");
//Link List to hold all different plots on one graph.
private LinkedList<xyNode> link;
/**
* Constructor which Plots only Y-axis data.
* @param yData is a array of doubles representing the Y-axis values of the data to be plotted.
*/
public FalconLinePlot(double[] yData)
{
this(null,yData,Color.red);
}
public FalconLinePlot(double[] yData,Color lineColor, Color marker)
{
this(null,yData,lineColor,marker);
}
/**
* Constructor which Plots chart based on provided x and y data. X and Y arrays must be of the same length.
* @param xData is an array of doubles representing the X-axis values of the data to be plotted.
* @param yData is an array of double representing the Y-axis values of the data to be plotted.
*/
public FalconLinePlot(double[] xData, double[] yData)
{
this(xData,yData,Color.red,null);
}
/**
* Constructor which Plots chart based on provided x and y axis data.
* @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data
* is the second dimension.
*/
public FalconLinePlot(double[][] data)
{
this(getXVector(data),getYVector(data),Color.red,null);
}
/**
* Constructor which plots charts based on provided x and y axis data in a single two dimensional array.
* @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data
* is the second dimension.
* @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
* @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to
* not have datapoint markers.
*/
public FalconLinePlot(double[][] data, Color lineColor, Color markerColor)
{
this(getXVector(data),getYVector(data),lineColor,markerColor);
}
/**
* Constructor which plots charts based on provided x and y axis data provided as separate arrays. The user can also specify the color of the adjoining line.
* Data markers are not displayed.
* @param xData is an array of doubles representing the X-axis values of the data to be plotted.
* @param yData is an array of double representing the Y-axis values of the data to be plotted.
* @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
*/
public FalconLinePlot(double[] xData, double[] yData,Color lineColor)
{
this(xData,yData,lineColor,null);
}
/**
* Constructor which plots charts based on provided x and y axis data, provided as separate arrays. The user
* can also specify the color of the adjoining line and the color of the datapoint maker.
* @param xData is an array of doubles representing the X-axis values of the data to be plotted.
* @param yData is an array of double representing the Y-axis values of the data to be plotted.
* @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint
* @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to
* not have datapoint markers.
*/
public FalconLinePlot(double[] xData, double[] yData,Color lineColor, Color markerColor)
{
xLabel = "X axis";
yLabel = "Y axis";
titleLabel = "Title";
upperXtic = -Double.MAX_VALUE;
lowerXtic = Double.MAX_VALUE;
upperYtic = -Double.MAX_VALUE;
lowerYtic = Double.MAX_VALUE;
xticCount = -Integer.MAX_VALUE;
this.userSetXTic = false;
this.userSetYTic = false;
link = new LinkedList<xyNode>();
addData(xData, yData, lineColor,markerColor);
count ++;
JFrame g = new JFrame("Figure " + count);
g.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
g.add(this);
g.setSize(600,400);
g.setLocationByPlatform(true);
g.setVisible(true);
menu(g,this);
}
/**
* Adds a plot to an existing figure.
* @param yData is a array of doubles representing the Y-axis values of the data to be plotted.
* @param color is the color the user wishes to be displayed for the line connecting each datapoint
*/
public void addData(double[] y, Color lineColor)
{
addData(y, lineColor, null);
}
public void addData(double[] y, Color lineColor, Color marker)
{
//cant add y only data unless all other data is y only data
for(xyNode data: link)
if(data.x != null)
throw new Error ("All previous chart series need to have only Y data arrays");
addData(null,y,lineColor, marker);
}
public void addData(double[] x, double[] y, Color lineColor)
{
addData(x,y,lineColor,null);
}
public void addData(double[][] data, Color lineColor)
{
addData(getXVector(data),getYVector(data),lineColor,null);
}
public void addData(double[][] data, Color lineColor, Color marker)
{
addData(getXVector(data),getYVector(data),lineColor,marker);
}
public void addData(double[] x, double[] y, Color lineColor, Color marker)
{
xyNode Data = new xyNode();
//copy y array into node
Data.y = new double[y.length];
Data.lineColor = lineColor;
if(marker == null)
Data.lineMarker = false;
else
{
Data.lineMarker = true;
Data.markerColor = marker;
}
for(int i=0; i<y.length; i++)
Data.y[i] = y[i];
//if X is not null, copy x
if(x != null)
{
//cant add x, and y data unless all other data has x and y data
for(xyNode data: link)
if(data.x == null)
throw new Error ("All previous chart series need to have both X and Y data arrays");
if(x.length != y.length)
throw new Error("X dimension must match Y dimension");
Data.x = new double[x.length];
for(int i=0; i<x.length; i++)
Data.x[i] = x[i];
}
link.add(Data);
}
/**
* Main method which paints the panel and shows the figure.
*
*/
@Override
protected void paintComponent(Graphics g) {
super.paintComponent(g);
Graphics2D g2 = (Graphics2D)g;
g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
int w = getWidth();
int h = getHeight();
// Draw X and Y lines axis.
Line2D yaxis = new Line2D.Double(xPAD, yPAD, xPAD, h-yPAD);
Line2D.Double xaxis = new Line2D.Double(xPAD, h-yPAD, w-xPAD, h-yPAD);
g2.draw(yaxis);
g2.draw(xaxis);
//find Max Y limits
getMinMax(link);
//draw ticks
drawYTickRange(g2, yaxis, 15, yMax, yMin);
drawXTickRange(g2, xaxis, 15, xMax, xMin);
//plot all data
plot(g2);
//draw x and y labels
setXLabel(g2, xLabel);
setYLabel(g2, yLabel);
setTitle(g2, titleLabel);
}
void setXTic(double lowerBound, double upperBound, double stepSize)
{
this.userSetXTic = true;
this.upperXtic=upperBound;
this.lowerXtic=lowerBound;
this.xTicStepSize = stepSize;
}
public void setYTic(double lowerBound, double upperBound, double stepSize)
{
this.userSetYTic = true;
this.upperYtic=upperBound;
this.lowerYtic=lowerBound;
this.yTicStepSize = stepSize;
}
private void plot(Graphics2D g2)
{
int w = super.getWidth();
int h = super.getHeight();
Color tempC = g2.getColor();
//loop through list and plot each
for(int i=0; i<link.size(); i++)
{
// Draw lines.
double xScale = (double)(w - 2*xPAD)/(upperXtic-lowerXtic);
double yScale = (double)(h - 2*yPAD)/(upperYtic-lowerYtic);
for(int j = 0; j < link.get(i).y.length-1; j++)
{
double x1;
double x2;
if(link.get(i).x==null)
{
x1 = xPAD + j*xScale;
x2 = xPAD + (j+1)*xScale;
}
else
{
x1 = xPAD + xScale*link.get(i).x[j] + lowerXtic*xScale;
x2 = xPAD + xScale*link.get(i).x[j+1] + lowerXtic*xScale;
}
double y1 = h - yPAD - yScale*link.get(i).y[j] + lowerYtic*yScale;
double y2 = h - yPAD - yScale*link.get(i).y[j+1] + lowerYtic*yScale;
g2.setPaint(link.get(i).lineColor);
g2.draw(new Line2D.Double(x1, y1, x2, y2));
if(link.get(i).lineMarker)
{
g2.setPaint(link.get(i).markerColor);
g2.fill(new Ellipse2D.Double(x1-2, y1-2, 4, 4));
g2.fill(new Ellipse2D.Double(x2-2, y2-2, 4, 4));
}
}
}
g2.setColor(tempC);
}
/**
* need to optimize for loops
* @param list
*/
private void getMinMax(LinkedList<xyNode> list)
{
for(xyNode node: list)
{
double tempYMax = getMax(node.y);
double tempYMin = getMin(node.y);
if(tempYMin<yMin)
yMin = tempYMin;
if(tempYMax>yMax)
yMax=tempYMax;
if(xticCount < node.y.length)
xticCount = node.y.length;
if(node.x != null)
{
double tempXMax = getMax(node.x);
double tempXMin = getMin(node.x);
if(tempXMin<xMin)
xMin = tempXMin;
if(tempXMax>xMax)
xMax=tempXMax;
}
else
{
xMax=node.y.length-1;
xMin=0;
}
}
}
private double getMax(double[] data) {
double max = -Double.MAX_VALUE;
for(int i = 0; i < data.length; i++) {
if(data[i] > max)
max = data[i];
}
return max;
}
private double getMin(double[] data) {
double min = Double.MAX_VALUE;
for(int i = 0; i < data.length; i++) {
if(data[i] < min)
min = data[i];
}
return min;
}
public void setYLabel(String s)
{
yLabel = s;
}
public void setXLabel(String s)
{
xLabel = s;
}
public void setTitle(String s)
{
titleLabel = s;
}
private void setYLabel(Graphics2D g2, String s)
{
FontMetrics fm = getFontMetrics(getFont());
int width = fm.stringWidth(s);
AffineTransform temp = g2.getTransform();
AffineTransform at = new AffineTransform();
at.setToRotation(-Math.PI /2, 10, getHeight()/2+width/2);
g2.setTransform(at);
//draw string in center of y axis
g2.drawString(s, 10, 7+getHeight()/2+width/2);
g2.setTransform(temp);
}
private void setXLabel(Graphics2D g2, String s)
{
FontMetrics fm = getFontMetrics(getFont());
int width = fm.stringWidth(s);
g2.drawString(s, getWidth()/2-(width/2), getHeight()-10);
}
private void setTitle(Graphics2D g2, String s)
{
FontMetrics fm = getFontMetrics(getFont());
String[] line = s.split("\n");
int height = xPAD/2 - ((line.length-1) * fm.getHeight()/2);
for (int i=0; i<line.length; i++)
{
int width = fm.stringWidth(line[i]);
g2.drawString(line[i], getWidth()/2-(width/2), height);
height +=fm.getHeight();
}
}
public void yGridOn()
{
yGrid=true;
//super.repaint();
}
public void yGridOff()
{
yGrid=false;
//super.repaint();
}
public void xGridOn()
{
xGrid=true;
//super.repaint();
}
public void xGridOff()
{
xGrid=false;
//super.repaint();
}
private void drawYTickRange(Graphics2D g2, Line2D yaxis, int tickCount, double Max, double Min)
{
if(!userSetYTic)
{
double range = Max - Min;
//calculate max Y and min Y tic Range
double unroundedTickSize = range/(tickCount-1);
double x = Math.ceil(Math.log10(unroundedTickSize)-1);
double pow10x = Math.pow(10, x);
yTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
//determine min and max tick label
if(Min<0)
lowerYtic = yTicStepSize * Math.floor(Min/yTicStepSize);
else
lowerYtic = yTicStepSize * Math.ceil(Min/yTicStepSize);
if(Max<0)
upperYtic = yTicStepSize * Math.floor(1+Max/yTicStepSize);
else
upperYtic = yTicStepSize * Math.ceil(1+Max/yTicStepSize);
}
double x0 = yaxis.getX1();
double y0 = yaxis.getY1();
double xf = yaxis.getX2();
double yf = yaxis.getY2();
//calculate stepsize between ticks and length of Y axis using distance formula
int roundedTicks = (int) ((upperYtic - lowerYtic) / yTicStepSize);
double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
double upper = upperYtic;
for (int i = 0; i<=roundedTicks; i++)
{
double newY = y0;
//calculate width of number for proper drawing
String number = new DecimalFormat("#.#").format(upper);
FontMetrics fm = getFontMetrics(getFont());
int width = fm.stringWidth(number);
g2.draw(new Line2D.Double(x0,newY, x0-10,newY));
g2.drawString(number, (float)x0-15-width, (float)newY+5);
//add grid lines to chart
if(yGrid && i!=roundedTicks)
{
Stroke tempS = g2.getStroke();
Color tempC = g2.getColor();
g2.setColor (Color.lightGray);
g2.setStroke (new BasicStroke(
1f,
BasicStroke.CAP_ROUND,
BasicStroke.JOIN_ROUND,
1f,
new float[] {5f},
0f));
g2.draw(new Line2D.Double(xPAD, newY, getWidth()-xPAD, newY));
g2.setColor(tempC);
g2.setStroke(tempS);
}
upper = upper - yTicStepSize;
y0 = newY + distance;
}
}
private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min)
{
drawXTickRange(g2, xaxis, tickCount, Max, Min, 1);
}
private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min, double skip)
{
if(!userSetXTic)
{
double range = Max - Min;
//calculate max Y and min Y tic Range
double unroundedTickSize = range/(tickCount-1);
double x = Math.ceil(Math.log10(unroundedTickSize)-1);
double pow10x = Math.pow(10, x);
xTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
//determine min and max tick label
if(Min<0)
lowerXtic = xTicStepSize * Math.floor(Min/xTicStepSize);
else
lowerXtic = xTicStepSize * Math.ceil(Min/xTicStepSize);
if(Max<0)
upperXtic = xTicStepSize * Math.floor(1+Max/xTicStepSize);
else
upperXtic = xTicStepSize * Math.ceil(1+Max/xTicStepSize);
}
double x0 = xaxis.getX1();
double y0 = xaxis.getY1();
double xf = xaxis.getX2();
double yf = xaxis.getY2();
//calculate stepsize between ticks and length of Y axis using distance formula
int roundedTicks = (int) ((upperXtic - lowerXtic) / xTicStepSize);
double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
double lower = lowerXtic;
for (int i = 0; i<=roundedTicks; i++)
{
double newX = x0;
//calculate width of number for proper drawing
String number = new DecimalFormat("#.#").format(lower);
FontMetrics fm = getFontMetrics( getFont() );
int width = fm.stringWidth(number);
g2.draw(new Line2D.Double(newX,yf, newX,yf+10));
//dont label every x tic to prevent clutter
if(i%skip==0)
g2.drawString(number, (float)(newX-(width/2.0)), (float)yf+25);
//add grid lines to chart
if(xGrid && i!=0)
{
Stroke tempS = g2.getStroke();
Color tempC = g2.getColor();
g2.setColor (Color.lightGray);
g2.setStroke (new BasicStroke(
1f,
BasicStroke.CAP_ROUND,
BasicStroke.JOIN_ROUND,
1f,
new float[] {5f},
0f));
g2.draw(new Line2D.Double(newX, yPAD, newX, getHeight()-yPAD));
g2.setColor(tempC);
g2.setStroke(tempS);
}
lower = lower + xTicStepSize;
x0 = newX + distance;
}
}
public void updateData(int series, double[][] data)
{
//add Data to link list
addData(data,null,null);
//copy data from new to old and line styles from list to new list.
link.get(series).x = link.getLast().x.clone();
link.get(series).y = link.getLast().y.clone();
//remove last data
link.removeLast();
}
public static double[] getXVector(double[][] arr)
{
double[] temp = new double[arr.length];
for(int i=0; i<temp.length; i++)
temp[i] = arr[i][0];
return temp;
}
public static double[] getYVector(double[][] arr)
{
double[] temp = new double[arr.length];
for(int i=0; i<temp.length; i++)
temp[i] = arr[i][1];
return temp;
}
/**********Class for Linked List************/
private class xyNode
{
double[] x;
double[] y;
Color lineColor;
boolean lineMarker;
Color markerColor;
public xyNode()
{
x=null;
y=null;
lineMarker = false;
}
}
/****Methods to Support Right Click Menu****/
@Override
public void lostOwnership(Clipboard clip, Transferable transferable)
{
//We must keep the object we placed on the system clipboard
//until this method is called.
}
private void menu(JFrame g, final FalconLinePlot p )
{
g.addMouseListener(new PopupTriggerListener());
JMenuItem item = new JMenuItem("Copy Figure");
item.addActionListener(new ActionListener()
{
public void actionPerformed(ActionEvent e)
{
BufferedImage i = new BufferedImage(p.getSize().width, p.getSize().height,BufferedImage.TRANSLUCENT);
p.setOpaque(false);
p.paint(i.createGraphics());
TransferableImage trans = new TransferableImage( i );
Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
c.setContents( trans, p);
}
});
menu.add(item);
item = new JMenuItem("Desktop ScreenShot");
item.addActionListener(new ActionListener()
{
public void actionPerformed(ActionEvent e)
{
System.out.println("Copy files to clipboard");
try {
Robot robot = new Robot();
Dimension screenSize = Toolkit.getDefaultToolkit().getScreenSize();
Rectangle screen = new Rectangle( screenSize );
BufferedImage i = robot.createScreenCapture( screen );
TransferableImage trans = new TransferableImage( i );
Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
c.setContents( trans, p);
}
catch ( AWTException x ) {
x.printStackTrace();
System.exit( 1 );
}
}
});
menu.add(item);
}
class PopupTriggerListener extends MouseAdapter {
public void mousePressed(MouseEvent ev) {
if (ev.isPopupTrigger()) {
menu.show(ev.getComponent(), ev.getX(), ev.getY());
}
}
public void mouseReleased(MouseEvent ev) {
if (ev.isPopupTrigger()) {
menu.show(ev.getComponent(), ev.getX(), ev.getY());
}
}
public void mouseClicked(MouseEvent ev) {
}
}
private class TransferableImage implements Transferable {
Image i;
public TransferableImage( Image i ) {
this.i = i;
}
public Object getTransferData( DataFlavor flavor )
throws UnsupportedFlavorException, IOException {
if ( flavor.equals( DataFlavor.imageFlavor ) && i != null ) {
return i;
}
else {
throw new UnsupportedFlavorException( flavor );
}
}
public DataFlavor[] getTransferDataFlavors() {
DataFlavor[] flavors = new DataFlavor[ 1 ];
flavors[ 0 ] = DataFlavor.imageFlavor;
return flavors;
}
public boolean isDataFlavorSupported( DataFlavor flavor ) {
DataFlavor[] flavors = getTransferDataFlavors();
for ( int i = 0; i < flavors.length; i++ ) {
if ( flavor.equals( flavors[ i ] ) ) {
return true;
}
}
return false;
}
}
/******TEST MAIN METHOD*******/
public static void main(String[] args) {
double[] data = {
-235, 14, 18, 03, 60, 150, 74, 87, 54, 77,
61, 55, 48, 60, 49, 36, 38, 27, 20, 18,5
};
double[] data2 = {
-4, 124, 128, 33, -1, 1, 74, 87, 54, 77,
61, 55, 48, 60, 40, 36, 38, 27, 20, 18,5
};
double[] test = {0.1, 0.3, 0.5, 0.7, 0.9, 1.1, 1.3, 1.5, 1.7, 2.0,
2.2,2.4,2.6,2.8,3.0,3.2,3.4,3.6,3.8,4.0,4.2
};
FalconLinePlot fig2 = new FalconLinePlot(data,Color.red, Color.blue);
fig2.yGridOn();
fig2.xGridOn();
fig2.setYLabel("This is a new");
fig2.addData(data2, Color.blue);
FalconLinePlot fig1 = new FalconLinePlot(test,data);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java
-59
View File
@@ -1,59 +0,0 @@
package org.usfirst.frc4388.utility;
import org.usfirst.frc4388.robot.Constants;
/**
* Provides forward and inverse kinematics equations for the robot modeling the
* wheelbase as a differential drive (with a corrective factor to account for
* the inherent skidding of the center 4 wheels quasi-kinematically).
*/
public class Kinematics {
private static final double kEpsilon = 1E-9;
/**
* Forward kinematics using only encoders, rotation is implicit (less
* accurate than below, but useful for predicting motion)
*/
public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta) {
double linear_velocity = (left_wheel_delta + right_wheel_delta) / 2;
double delta_v = (right_wheel_delta - left_wheel_delta) / 2;
double delta_rotation = delta_v * 2 * Constants.kTrackScrubFactor / Constants.kTrackEffectiveDiameter;
return new RigidTransform2d.Delta(linear_velocity, 0, delta_rotation);
}
/**
* Forward kinematics using encoders and explicitly measured rotation (ex.
* from gyro)
*/
public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta,
double delta_rotation_rads) {
return new RigidTransform2d.Delta((left_wheel_delta + right_wheel_delta) / 2, 0, delta_rotation_rads);
}
/** Append the result of forward kinematics to a previous pose. */
public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, double left_wheel_delta,
double right_wheel_delta, Rotation2d current_heading) {
RigidTransform2d.Delta with_gyro = forwardKinematics(left_wheel_delta, right_wheel_delta,
current_pose.getRotation().inverse().rotateBy(current_heading).getRadians());
return current_pose.transformBy(RigidTransform2d.fromVelocity(with_gyro));
}
public static class DriveVelocity {
public final double left;
public final double right;
public DriveVelocity(double left, double right) {
this.left = left;
this.right = right;
}
}
public static DriveVelocity inverseKinematics(RigidTransform2d.Delta velocity) {
if (Math.abs(velocity.dtheta) < kEpsilon) {
return new DriveVelocity(velocity.dx, velocity.dx);
}
double delta_v = Constants.kTrackEffectiveDiameter * velocity.dtheta / (2 * Constants.kTrackScrubFactor);
return new DriveVelocity(velocity.dx - delta_v, velocity.dx + delta_v);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/Loop.java
+14
View File
@@ -0,0 +1,14 @@
package org.usfirst.frc4388.utility;
/**
* Interface for loops, which are routine that run periodically in the robot code (such as periodic gyroscope
* calibration, etc.)
*/
public interface Loop {
public void onStart(double timestamp);
public void onLoop(double timestamp);
public void onStop(double timestamp);
}
2019robot/src/main/java/org/usfirst/frc4388/utility/Looper.java
+89
View File
@@ -0,0 +1,89 @@
package org.usfirst.frc4388.utility;
import java.util.ArrayList;
import java.util.List;
import org.usfirst.frc4388.robot.Constants;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* This code runs all of the robot's loops. Loop objects are stored in a List object. They are started when the robot
* powers up and stopped after the match.
*/
public class Looper {
public final double kPeriod = Constants.kLooperDt;
private boolean running_;
private final Notifier notifier_;
private final List<Loop> loops_;
private final Object taskRunningLock_ = new Object();
private double timestamp_ = 0;
private double dt_ = 0;
private final CrashTrackingRunnable runnable_ = new CrashTrackingRunnable() {
@Override
public void runCrashTracked() {
synchronized (taskRunningLock_) {
if (running_) {
double now = Timer.getFPGATimestamp();
for (Loop loop : loops_) {
loop.onLoop(now);
}
dt_ = now - timestamp_;
timestamp_ = now;
}
}
}
};
public Looper() {
notifier_ = new Notifier(runnable_);
running_ = false;
loops_ = new ArrayList<>();
}
public synchronized void register(Loop loop) {
synchronized (taskRunningLock_) {
loops_.add(loop);
}
}
public synchronized void start() {
if (!running_) {
System.out.println("Starting loops");
synchronized (taskRunningLock_) {
timestamp_ = Timer.getFPGATimestamp();
for (Loop loop : loops_) {
loop.onStart(timestamp_);
}
running_ = true;
}
notifier_.startPeriodic(kPeriod);
}
}
public synchronized void stop() {
if (running_) {
System.out.println("Stopping loops");
notifier_.stop();
synchronized (taskRunningLock_) {
running_ = false;
timestamp_ = Timer.getFPGATimestamp();
for (Loop loop : loops_) {
System.out.println("Stopping " + loop);
loop.onStop(timestamp_);
}
}
}
}
public void outputToSmartDashboard() {
SmartDashboard.putNumber("looper_dt", dt_);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java
+20 -41
View File
@@ -2,9 +2,9 @@ package org.usfirst.frc4388.utility;
import java.util.ArrayList;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.subsystems.Drive;
//import com.ctre.CANTalon.TalonControlMode;
import com.ctre.phoenix.motorcontrol.ControlMode;
public class MMTalonPIDController
@@ -12,7 +12,7 @@ public class MMTalonPIDController
protected static enum MMControlMode { STRAIGHT, TURN };
public static enum MMTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY };
protected ArrayList<CANTalonEncoder> motorControllers;
protected ArrayList<TalonSRXEncoder> motorControllers;
protected long periodMs;
protected PIDParams pidParams;
protected boolean useGyroLock;
@@ -23,73 +23,54 @@ public class MMTalonPIDController
protected MMTalonTurnType turnType;
protected double targetValue;
public MMTalonPIDController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers)
public MMTalonPIDController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers)
{
this.motorControllers = motorControllers;
this.periodMs = periodMs;
setPID(pidParams);
}
private int convertInchesPerSecToTicksPer100ms(double inchesPerSec) {
return (int)Math.round(Constants.kDriveEncoderTicksPerInch * inchesPerSec / 10);
}
public void setPID(PIDParams pidParams) {
this.pidParams = pidParams;
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
//motorController.setF(pidParams.kF);
motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout
motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout
motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout
motorController.config_kF(0, pidParams.kF, 0); //TODO: verify want parameter slot 0, with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPIDF(0, pidParams.kP, pidParams.kI, pidParams.kD, pidParams.kF);
}
}
public void setMMStraightTarget(double startValue, double targetValue, double maxVelocityInchesPerSec, double maxAccelerationInchesPerSecPerSec, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
public void setMMStraightTarget(double startValue, double targetValue, double maxVelocity, double maxAcceleration, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
controlMode = MMControlMode.STRAIGHT;
this.startGyroAngle = desiredAngle;
this.useGyroLock = useGyroLock;
this.targetValue = targetValue;
// Set up the motion profile
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (resetEncoder) {
//motorController.setPosition(0);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
motorController.setPosition(0);
}
//motorController.setMotionMagicCruiseVelocity(maxVelocity);
//motorController.setMotionMagicAcceleration(maxAcceleration);
//motorController.set(targetValue);
//motorController.changeControlMode(TalonControlMode.MotionMagic);
motorController.configMotionCruiseVelocity(convertInchesPerSecToTicksPer100ms(maxVelocityInchesPerSec), 0);
motorController.configMotionAcceleration(convertInchesPerSecToTicksPer100ms(maxAccelerationInchesPerSecPerSec), 0);
motorController.configMotionCruiseVelocity((int)maxVelocity, TalonSRXEncoder.TIMEOUT_MS);
motorController.configMotionAcceleration((int)maxAcceleration, TalonSRXEncoder.TIMEOUT_MS);
motorController.set(ControlMode.MotionMagic, targetValue);
}
}
public void setZeroPosition() {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
//motorController.set(0);
//motorController.changeControlMode(TalonControlMode.Position);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
motorController.set(ControlMode.Position, 0);
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPosition(0);
motorController.set(ControlMode.MotionMagic, targetValue);
}
}
public void resetZeroPosition() {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPosition(0);
}
}
public void resetZeroPosition(double angle) {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(angle);
motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPositionWorld(angle);
}
}
@@ -120,14 +101,12 @@ public class MMTalonPIDController
leftTarget = targetValue - deltaDistance;
// Update the controllers with updated set points.
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
//motorController.set(rightTarget);
motorController.set(motorController.getControlMode(), rightTarget); //TODO: change to explicit mode set?
motorController.set(ControlMode.MotionMagic, rightTarget);
}
else {
//motorController.set(leftTarget);
motorController.set(motorController.getControlMode(), leftTarget); //TODO: change to explicit mode set?
motorController.set(ControlMode.MotionMagic, leftTarget);
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java
+26 -38
View File
@@ -2,13 +2,14 @@ package org.usfirst.frc4388.utility;
import java.util.ArrayList;
//import com.ctre.CANTalon.TalonControlMode;
import com.ctre.phoenix.motorcontrol.ControlMode;
public class MPSoftwarePIDController
{
public static enum MPSoftwareTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY, LEFT_ARC, RIGHT_ARC };
protected ArrayList<CANTalonEncoder> motorControllers;
protected ArrayList<TalonSRXEncoder> motorControllers;
protected long periodMs;
protected PIDParams pidParams;
protected MotionProfileBoxCar mp;
@@ -21,7 +22,7 @@ public class MPSoftwarePIDController
protected double prevError = 0.0; // the prior error (used to compute velocity)
protected double totalError = 0.0; // the sum of the errors for use in the integral calc
public MPSoftwarePIDController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers)
public MPSoftwarePIDController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers)
{
this.motorControllers = motorControllers;
this.periodMs = periodMs;
@@ -40,31 +41,22 @@ public class MPSoftwarePIDController
// Set up the motion profile
mp = new MotionProfileBoxCar(startAngleDeg, targetAngleDeg, maxTurnRateDegPerSec, periodMs, t1, t2);
// NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below
// for (CANTalonEncoder motorController : motorControllers) {
// motorController.changeControlMode(TalonControlMode.PercentVbus);
// }
prevError = 0.0;
totalError = 0.0;
}
//public void setMPTurnTarget(String key, MPSoftwareTurnType turnType, double trackWidth) {
// this.turnType = turnType;
// this.useGyroLock = true;
//
// // Set up the motion profile
// mp = MotionProfileCache.getInstance().getMP(key);
// this.startGyroAngle = mp.getStartDistance();
// this.targetGyroAngle = mp.getTargetDistance();
//
// for (CANTalonEncoder motorController : motorControllers) {
// motorController.changeControlMode(TalonControlMode.PercentVbus);
// }
//
// prevError = 0.0;
// totalError = 0.0;
//}
public void setMPTurnTarget(String key, MPSoftwareTurnType turnType, double trackWidth) {
this.turnType = turnType;
this.useGyroLock = true;
// Set up the motion profile
mp = MotionProfileCache.getInstance().getMP(key);
this.startGyroAngle = mp.getStartDistance();
this.targetGyroAngle = mp.getTargetDistance();
prevError = 0.0;
totalError = 0.0;
}
public boolean controlLoopUpdate() {
return controlLoopUpdate(0);
@@ -93,55 +85,51 @@ public class MPSoftwarePIDController
// Update the controllers set point.
if (turnType == MPSoftwareTurnType.TANK) {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.set(output);
motorController.set(ControlMode.PercentOutput, output);
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
motorController.set(ControlMode.PercentOutput, -output);
}
else {
motorController.set(ControlMode.PercentOutput, output);
}
}
}
else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) {
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
//motorController.set(0);
motorController.set(ControlMode.PercentOutput, 0);
}
else {
//motorController.set(2.0 * output);
motorController.set(ControlMode.PercentOutput, 2.0 * output);
}
}
}
else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) {
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
//motorController.set(2.0 * output);
motorController.set(ControlMode.PercentOutput, 2.0 * output);
}
else {
//motorController.set(0);
motorController.set(ControlMode.PercentOutput, 0);
}
}
}
else if (turnType == MPSoftwareTurnType.LEFT_ARC) {
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
//motorController.set(1.0 * output);
motorController.set(ControlMode.PercentOutput, 1.0 * output);
}
else {
//motorController.set(2.0 * output);
motorController.set(ControlMode.PercentOutput, 2.0 * output);
}
}
}
else if (turnType == MPSoftwareTurnType.RIGHT_ARC) {
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
//motorController.set(2.0 * output);
motorController.set(ControlMode.PercentOutput, 2.0 * output);
}
else {
//motorController.set(1.0 * output);
motorController.set(ControlMode.PercentOutput, 1.0 * output);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java
+73 -73
View File
@@ -1,16 +1,15 @@
package org.usfirst.frc4388.utility;
import java.util.ArrayList;
//import com.ctre.CANTalon.TalonControlMode;
import com.ctre.phoenix.motorcontrol.ControlMode;
public class MPTalonPIDController
{
protected static enum MPControlMode { STRAIGHT, TURN };
public static enum MPTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY };
protected ArrayList<CANTalonEncoder> motorControllers;
protected ArrayList<TalonSRXEncoder> motorControllers;
protected long periodMs;
protected PIDParams pidParams;
protected MotionProfileBoxCar mp;
@@ -21,22 +20,26 @@ public class MPTalonPIDController
protected double trackDistance;
protected MPControlMode controlMode;
protected MPTalonTurnType turnType;
protected int pidSlot;
public MPTalonPIDController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers)
public MPTalonPIDController(long periodMs, ArrayList<TalonSRXEncoder> motorControllers)
{
this.motorControllers = motorControllers;
this.periodMs = periodMs;
setPID(pidParams);
}
public void setPID(PIDParams pidParams) {
public void setPID(PIDParams pidParams, int slot) {
this.pidParams = pidParams;
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout
motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout
motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPID(slot, pidParams.kP, pidParams.kI, pidParams.kD);
}
}
public void setPIDSlot(int slot) {
this.pidSlot = slot;
for (TalonSRXEncoder motorController : motorControllers) {
motorController.selectProfileSlot(slot, 0);
}
}
@@ -55,32 +58,36 @@ public class MPTalonPIDController
// Set up the motion profile
mp = new MotionProfileBoxCar(startValue, targetValue, maxVelocity, periodMs, t1, t2);
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (resetEncoder) {
//motorController.setPosition(0);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
motorController.setPosition(0);
}
//motorController.set(mp.getStartDistance());
//motorController.changeControlMode(TalonControlMode.Position);
motorController.set(ControlMode.Position, mp.getStartDistance());
motorController.setWorld(ControlMode.Position, mp.getStartDistance());
}
}
//public void setMPStraightTarget(String key, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
// controlMode = MPControlMode.STRAIGHT;
// this.startGyroAngle = desiredAngle;
// this.useGyroLock = useGyroLock;
//
// // Set up the motion profile
// mp = MotionProfileCache.getInstance().getMP(key);
// for (CANTalonEncoder motorController : motorControllers) {
// if (resetEncoder) {
// motorController.setPosition(0);
// }
// motorController.set(mp.getStartDistance());
// motorController.changeControlMode(TalonControlMode.Position);
// }
//}
public double getStartPosition() {
return mp != null ? mp.getStartDistance() : 0;
}
public double getTargetPosition() {
return mp != null ? mp.getTargetDistance() : 0;
}
public void setMPStraightTarget(String key, boolean useGyroLock, double desiredAngle, boolean resetEncoder) {
controlMode = MPControlMode.STRAIGHT;
this.startGyroAngle = desiredAngle;
this.useGyroLock = useGyroLock;
// Set up the motion profile
mp = MotionProfileCache.getInstance().getMP(key);
for (TalonSRXEncoder motorController : motorControllers) {
if (resetEncoder) {
motorController.setPosition(0);
}
motorController.setWorld(ControlMode.Position, mp.getStartDistance());
}
}
public void setMPTurnTarget(double startAngleDeg, double targetAngleDeg, double maxTurnRateDegPerSec, double t1, double t2, MPTalonTurnType turnType, double trackWidth) {
controlMode = MPControlMode.TURN;
@@ -93,11 +100,8 @@ public class MPTalonPIDController
// Set up the motion profile
mp = new MotionProfileBoxCar(0, trackDistance, maxTurnRateDegPerSec, periodMs, t1, t2);
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
//motorController.set(0);
//motorController.changeControlMode(TalonControlMode.Position);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPosition(0);
motorController.set(ControlMode.Position, 0);
}
@@ -121,26 +125,21 @@ public class MPTalonPIDController
}
public void setZeroPosition() {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
//motorController.set(0);
//motorController.changeControlMode(TalonControlMode.Position);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPosition(0);
motorController.set(ControlMode.Position, 0);
}
}
public void resetZeroPosition() {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPosition(0);
}
}
public void resetZeroPosition(double angle) {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(angle);
motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position
public void resetZeroPosition(double position) {
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPositionWorld(position);
}
}
@@ -164,24 +163,19 @@ public class MPTalonPIDController
if (controlMode == MPControlMode.STRAIGHT) {
double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0;
if (Math.abs(mpPoint.position) > 0.001) {
//KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position;
//KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position;
KfLeft = -(pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position;
KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position;
KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position;
}
// Update the controllers Kf and set point.
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
//motorController.setF(KfRight);
motorController.config_kF(0, KfRight, 0); //TODO: verify
motorController.setWorld(-mpPoint.position);
motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
motorController.setWorld(ControlMode.Position, mpPoint.position);
}
else {
//motorController.setF(KfLeft);
motorController.config_kF(0, KfLeft, 0); //TODO: verify
motorController.setWorld(mpPoint.position);
motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
motorController.setWorld(ControlMode.Position, mpPoint.position);
}
}
}
@@ -194,31 +188,27 @@ public class MPTalonPIDController
KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position;
}
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (turnType == MPTalonTurnType.TANK) {
if (motorController.isRight()) {
//motorController.setF(KfRight);
motorController.config_kF(0, KfRight, 0); //TODO: verify
motorController.setWorld(-mpPoint.position);
motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
motorController.setWorld(ControlMode.Position, -mpPoint.position);
}
else {
//motorController.setF(KfLeft);
motorController.config_kF(0, KfLeft, 0); //TODO: verify
motorController.setWorld(mpPoint.position);
motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
motorController.setWorld(ControlMode.Position, mpPoint.position);
}
}
else if (turnType == MPTalonTurnType.LEFT_SIDE_ONLY) {
if (!motorController.isRight()) {
//motorController.setF(KfLeft);
motorController.config_kF(0, KfLeft, 0); //TODO: verify
motorController.setWorld(mpPoint.position);
motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
motorController.setWorld(ControlMode.Position, mpPoint.position);
}
}
else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) {
if (motorController.isRight()) {
//motorController.setF(KfRight);
motorController.config_kF(0, KfRight, 0); //TODO: verify
motorController.setWorld(-mpPoint.position);
motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
motorController.setWorld(ControlMode.Position, -mpPoint.position);
}
}
}
@@ -227,6 +217,16 @@ public class MPTalonPIDController
return false;
}
public void setTarget(double position, double Kf) {
// Kf gets multipled by position in the Talon
double KfPerPosition = Math.abs(position) > 0.001 ? Kf / position : 0;
for (TalonSRXEncoder motorController : motorControllers) {
motorController.config_kF(0, KfPerPosition, TalonSRXEncoder.TIMEOUT_MS);
motorController.setWorld(ControlMode.Position, position);
}
}
public MotionProfilePoint getCurrentPoint() {
return mpPoint;
}
2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java
+15 -54
View File
@@ -2,21 +2,20 @@ package org.usfirst.frc4388.utility;
import java.util.ArrayList;
//import com.ctre.CANTalon.TalonControlMode;
import com.ctre.phoenix.motorcontrol.ControlMode;
import jaci.pathfinder.Trajectory.Segment;
public class MPTalonPIDPathController
{
protected ArrayList<CANTalonEncoder> motorControllers;
protected ArrayList<TalonSRXEncoder> motorControllers;
protected long periodMs;
protected PIDParams pidParams;
protected PathGenerator path;
protected double startGyroAngle;
protected double targetGyroAngle;
protected double trackDistance;
public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers)
public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers)
{
this.motorControllers = motorControllers;
this.periodMs = periodMs;
@@ -26,87 +25,49 @@ public class MPTalonPIDPathController
public void setPID(PIDParams pidParams) {
this.pidParams = pidParams;
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout
motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout
motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout
}
}
public void setMPPathTarget(PathGenerator path) {
this.path = path;
path.resetCounter();
// Set up the motion profile
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
//motorController.set(0);
//motorController.changeControlMode(TalonControlMode.Position);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
motorController.set(ControlMode.Position, 0);
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPID(0, pidParams.kP, pidParams.kI, pidParams.kD);
}
}
public void setZeroPosition() {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
//motorController.set(0);
//motorController.changeControlMode(TalonControlMode.Position);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPosition(0);
motorController.set(ControlMode.Position, 0);
}
}
public void resetZeroPosition() {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPosition(0);
}
}
public void resetZeroPosition(double angle) {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(angle);
motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPositionWorld(angle);
}
}
public boolean controlLoopUpdate(double currentGyroAngle) {
Segment leftPoint = path.getLeftPoint();
Segment rightPoint = path.getRightPoint();
// Check if we are finished
if (leftPoint == null) {
return true;
}
// Calculate the motion profile feed forward and gyro feedback terms
double KfLeft = 0.0;
double KfRight = 0.0;
// Update the set points and Kf gains
double gyroDelta = -path.getHeading() - currentGyroAngle;
if (Math.abs(leftPoint.position) > 0.001) {
KfLeft = (pidParams.kA * leftPoint.acceleration + pidParams.kV * leftPoint.velocity + pidParams.kG * gyroDelta) / leftPoint.position;
KfRight = (pidParams.kA * rightPoint.acceleration + pidParams.kV * rightPoint.velocity - pidParams.kG * gyroDelta) / rightPoint.position;
}
// Update the controllers Kf and set point.
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
//motorController.setF(KfRight);
motorController.config_kF(0, KfRight, 0); //TODO: verify
motorController.setWorld(rightPoint.position);
motorController.config_kF(0, KfRight, TalonSRXEncoder.TIMEOUT_MS);
// motorController.setWorld(ControlMode.Position, rightPoint.position);
}
else {
//motorController.setF(KfLeft);
motorController.config_kF(0, KfLeft, 0); //TODO: verify
motorController.setWorld(leftPoint.position);
motorController.config_kF(0, KfLeft, TalonSRXEncoder.TIMEOUT_MS);
}
}
path.incrementCounter();
return false;
}
2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java
+21 -61
View File
@@ -2,21 +2,21 @@ package org.usfirst.frc4388.utility;
import java.util.ArrayList;
//import com.ctre.CANTalon.TalonControlMode;
import com.ctre.phoenix.motorcontrol.ControlMode;
import jaci.pathfinder.Trajectory.Segment;
public class MPTalonPIDPathVelocityController
{
protected ArrayList<CANTalonEncoder> motorControllers;
protected ArrayList<TalonSRXEncoder> motorControllers;
protected long periodMs;
protected PIDParams pidParams;
protected PathGenerator path;
// protected PathGenerator path;
protected double startGyroAngle;
protected double targetGyroAngle;
protected double trackDistance;
public MPTalonPIDPathVelocityController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers)
public MPTalonPIDPathVelocityController(long periodMs, PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers)
{
this.motorControllers = motorControllers;
this.periodMs = periodMs;
@@ -26,86 +26,46 @@ public class MPTalonPIDPathVelocityController
public void setPID(PIDParams pidParams) {
this.pidParams = pidParams;
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD);
//motorController.setF(pidParams.kF);
//motorController.configNominalOutputVoltage(+0.0f, -0.0f);
//motorController.configPeakOutputVoltage(+12.0f, -12.0f);
//motorController.setProfile(0);
motorController.config_kP(0, pidParams.kP, 0);
motorController.config_kI(0, pidParams.kI, 0);
motorController.config_kD(0, pidParams.kD, 0);
motorController.config_kF(0, pidParams.kF, 0);
motorController.configNominalOutputForward(+0.0f, 0);
motorController.configNominalOutputReverse(-0.0f, 0);
motorController.configPeakOutputForward(+1.0f, 0);
motorController.configPeakOutputReverse(-1.0f, 0);
motorController.selectProfileSlot(0, 0);
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPIDF(0, pidParams.kP, pidParams.kI, pidParams.kD, pidParams.kF);
motorController.configVoltageCompSaturation(12.0, TalonSRXEncoder.TIMEOUT_MS);
motorController.enableVoltageCompensation(true);
motorController.configNominalOutputForward(0.0, TalonSRXEncoder.TIMEOUT_MS);
motorController.configNominalOutputReverse(0.0, TalonSRXEncoder.TIMEOUT_MS);
motorController.configPeakOutputForward(+1.0f, TalonSRXEncoder.TIMEOUT_MS);
motorController.configPeakOutputReverse(-1.0f, TalonSRXEncoder.TIMEOUT_MS);
motorController.selectProfileSlot(0, TalonSRXEncoder.PID_IDX);
}
}
public void setMPPathTarget(PathGenerator path) {
this.path = path;
path.resetCounter();
// Set up the motion profile
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
//motorController.set(0);
//motorController.changeControlMode(TalonControlMode.Speed);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
motorController.set(ControlMode.Velocity, 0);
}
}
public void setZeroPosition() {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(0);
//motorController.set(0);
//motorController.changeControlMode(TalonControlMode.Position);
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPosition(0);
motorController.set(ControlMode.Position, 0);
}
}
public void resetZeroPosition() {
for (CANTalonEncoder motorController : motorControllers) {
motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPosition(0);
}
}
public void resetZeroPosition(double angle) {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.setPosition(angle);
motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position
for (TalonSRXEncoder motorController : motorControllers) {
motorController.setPositionWorld(angle);
}
}
// }
public boolean controlLoopUpdate(double currentGyroAngle) {
Segment leftPoint = path.getLeftPoint();
Segment rightPoint = path.getRightPoint();
// Check if we are finished
if (leftPoint == null) {
return true;
}
// Calculate the motion profile feed forward and gyro feedback terms
double rightVelocity = rightPoint.velocity;
double leftVelocity = leftPoint.velocity;
// Update the controllers Kf and set point.
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
motorController.setVelocityWorld(rightVelocity);
}
else {
motorController.setVelocityWorld(leftVelocity);
}
}
path.incrementCounter();
return false;
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java
+1 -1
View File
@@ -171,7 +171,7 @@ public class MotionProfileBoxCar
public static void main(String[] args) {
long startTime = System.nanoTime();
MotionProfileBoxCar mp = new MotionProfileBoxCar(0, 96, 120, 10, 600, 300);
MotionProfileBoxCar mp = new MotionProfileBoxCar(0, 70, 120, 10, 600, 300);
System.out.println("Time, Position, Velocity, Acceleration");
MotionProfilePoint point = new MotionProfilePoint();
while(mp.getNextPoint(point) != null) {
2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileCache.java
+49
View File
@@ -0,0 +1,49 @@
package org.usfirst.frc4388.utility;
import java.text.DecimalFormat;
import java.util.Hashtable;
public class MotionProfileCache {
private Hashtable<String, MotionProfileBoxCar> cache;
private DecimalFormat df = new DecimalFormat("#.000");
private static MotionProfileCache instance;
private MotionProfileCache() {
cache = new Hashtable<String, MotionProfileBoxCar>();
}
public String addMP(double startDistance, double targetDistance, double maxVelocity, double itp, double t1, double t2) {
String key = generateKey(startDistance, targetDistance, maxVelocity, itp, t1, t2);
if (!cache.containsKey(key)) {
MotionProfileBoxCar mp = new MotionProfileBoxCar(startDistance, targetDistance, maxVelocity, itp, t1, t2);
this.addMP(key, mp);
}
return key;
}
public void addMP(String key, MotionProfileBoxCar mp) {
cache.put(key, mp);
}
public MotionProfileBoxCar getMP(String key) {
return cache.get(key);
}
public static MotionProfileCache getInstance() {
if(instance == null) {
instance = new MotionProfileCache();
}
return instance;
}
public void release() {
instance = null;
}
private String generateKey(double startDistance, double targetDistance, double maxVelocity, double itp, double t1, double t2) {
return df.format(startDistance) + df.format(targetDistance) + df.format(maxVelocity) + df.format(itp) + df.format(t1) + df.format(t2);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java
-5
View File
@@ -15,11 +15,6 @@ public class PIDParams
public PIDParams() {
}
public PIDParams(double kP)
{
this.kP = kP;
}
public PIDParams(double kP, double kI, double kD) {
this.kP = kP;
this.kI = kI;
2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java
-260
View File
@@ -1,260 +0,0 @@
package org.usfirst.frc4388.utility;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.Iterator;
import java.util.List;
import java.util.Optional;
import java.util.Set;
import org.usfirst.frc4388.utility.Path.Waypoint;
/**
* A Path is a recording of the path that the robot takes. Path objects consist
* of a List of Waypoints that the robot passes by. Using multiple Waypoints in
* a Path object and the robot's current speed, the code can extrapolate future
* Waypoints and predict the robot's motion. It can also dictate the robot's
* motion along the set path.
*/
public class Path {
protected static final double kSegmentCompletePercentage = .99;
protected List<Waypoint> mWaypoints;
protected List<PathSegment> mSegments;
protected Set<String> mMarkersCrossed;
/**
* A point along the Path, which consists of the location, the speed, and a
* string marker (that future code can identify). Paths consist of a List of
* Waypoints.
*/
public static class Waypoint {
public final Translation2d position;
public final double speed;
public final Optional<String> marker;
public Waypoint(Translation2d position, double speed) {
this.position = position;
this.speed = speed;
this.marker = Optional.empty();
}
public Waypoint(Translation2d position, double speed, String marker) {
this.position = position;
this.speed = speed;
this.marker = Optional.of(marker);
}
}
public Path(List<Waypoint> waypoints) {
mMarkersCrossed = new HashSet<String>();
mWaypoints = waypoints;
mSegments = new ArrayList<PathSegment>();
for (int i = 0; i < waypoints.size() - 1; ++i) {
mSegments.add(
new PathSegment(waypoints.get(i).position, waypoints.get(i + 1).position, waypoints.get(i).speed));
}
// The first waypoint is already complete
if (mWaypoints.size() > 0) {
Waypoint first_waypoint = mWaypoints.get(0);
if (first_waypoint.marker.isPresent()) {
mMarkersCrossed.add(first_waypoint.marker.get());
}
mWaypoints.remove(0);
}
}
/**
* @param An
* initial position
* @return Returns the distance from the position to the first point on the
* path
*/
public double update(Translation2d position) {
double rv = 0.0;
for (Iterator<PathSegment> it = mSegments.iterator(); it.hasNext();) {
PathSegment segment = it.next();
PathSegment.ClosestPointReport closest_point_report = segment.getClosestPoint(position);
if (closest_point_report.index >= kSegmentCompletePercentage) {
it.remove();
if (mWaypoints.size() > 0) {
Waypoint waypoint = mWaypoints.get(0);
if (waypoint.marker.isPresent()) {
mMarkersCrossed.add(waypoint.marker.get());
}
mWaypoints.remove(0);
}
} else {
if (closest_point_report.index > 0.0) {
// Can shorten this segment
segment.updateStart(closest_point_report.closest_point);
}
// We are done
rv = closest_point_report.distance;
// ...unless the next segment is closer now
if (it.hasNext()) {
PathSegment next = it.next();
PathSegment.ClosestPointReport next_closest_point_report = next.getClosestPoint(position);
if (next_closest_point_report.index > 0
&& next_closest_point_report.index < kSegmentCompletePercentage
&& next_closest_point_report.distance < rv) {
next.updateStart(next_closest_point_report.closest_point);
rv = next_closest_point_report.distance;
mSegments.remove(0);
if (mWaypoints.size() > 0) {
Waypoint waypoint = mWaypoints.get(0);
if (waypoint.marker.isPresent()) {
mMarkersCrossed.add(waypoint.marker.get());
}
mWaypoints.remove(0);
}
}
}
break;
}
}
return rv;
}
public Set<String> getMarkersCrossed() {
return mMarkersCrossed;
}
public double getRemainingLength() {
double length = 0.0;
for (int i = 0; i < mSegments.size(); ++i) {
length += mSegments.get(i).getLength();
}
return length;
}
/**
* @param The
* robot's current position
* @param A
* specified distance to predict a future waypoint
* @return A segment of the robot's predicted motion with start/end points
* and speed.
*/
public PathSegment.Sample getLookaheadPoint(Translation2d position, double lookahead_distance) {
if (mSegments.size() == 0) {
return new PathSegment.Sample(new Translation2d(), 0);
}
// Check the distances to the start and end of each segment. As soon as
// we find a point > lookahead_distance away, we know the right point
// lies somewhere on that segment.
Translation2d position_inverse = position.inverse();
if (position_inverse.translateBy(mSegments.get(0).getStart()).norm() >= lookahead_distance) {
// Special case: Before the first point, so just return the first
// point.
return new PathSegment.Sample(mSegments.get(0).getStart(), mSegments.get(0).getSpeed());
}
for (int i = 0; i < mSegments.size(); ++i) {
PathSegment segment = mSegments.get(i);
double distance = position_inverse.translateBy(segment.getEnd()).norm();
if (distance >= lookahead_distance) {
// This segment contains the lookahead point
Optional<Translation2d> intersection_point = getFirstCircleSegmentIntersection(segment, position,
lookahead_distance);
if (intersection_point.isPresent()) {
return new PathSegment.Sample(intersection_point.get(), segment.getSpeed());
} else {
System.out.println("ERROR: No intersection point?");
}
}
}
// Special case: After the last point, so extrapolate forward.
PathSegment last_segment = mSegments.get(mSegments.size() - 1);
PathSegment new_last_segment = new PathSegment(last_segment.getStart(), last_segment.interpolate(10000),
last_segment.getSpeed());
Optional<Translation2d> intersection_point = getFirstCircleSegmentIntersection(new_last_segment, position,
lookahead_distance);
if (intersection_point.isPresent()) {
return new PathSegment.Sample(intersection_point.get(), last_segment.getSpeed());
} else {
System.out.println("ERROR: No intersection point anywhere on line?");
return new PathSegment.Sample(last_segment.getEnd(), last_segment.getSpeed());
}
}
static Optional<Translation2d> getFirstCircleSegmentIntersection(PathSegment segment, Translation2d center,
double radius) {
double x1 = segment.getStart().getX() - center.getX();
double y1 = segment.getStart().getY() - center.getY();
double x2 = segment.getEnd().getX() - center.getX();
double y2 = segment.getEnd().getY() - center.getY();
double dx = x2 - x1;
double dy = y2 - y1;
double dr_squared = dx * dx + dy * dy;
double det = x1 * y2 - x2 * y1;
double discriminant = dr_squared * radius * radius - det * det;
if (discriminant < 0) {
// No intersection
return Optional.empty();
}
double sqrt_discriminant = Math.sqrt(discriminant);
Translation2d pos_solution = new Translation2d(
(det * dy + (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(),
(-det * dx + Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY());
Translation2d neg_solution = new Translation2d(
(det * dy - (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(),
(-det * dx - Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY());
// Choose the one between start and end that is closest to start
double pos_dot_product = segment.dotProduct(pos_solution);
double neg_dot_product = segment.dotProduct(neg_solution);
if (pos_dot_product < 0 && neg_dot_product >= 0) {
return Optional.of(neg_solution);
} else if (pos_dot_product >= 0 && neg_dot_product < 0) {
return Optional.of(pos_solution);
} else {
if (Math.abs(pos_dot_product) <= Math.abs(neg_dot_product)) {
return Optional.of(pos_solution);
} else {
return Optional.of(neg_solution);
}
}
}
public static void addCircleArc(List<Waypoint> waypoints, double radius, double angleDeg, int numPoints, String endMarker ) {
Waypoint last = waypoints.get(waypoints.size() - 1);
double centerX = last.position.x_;
double centerY = radius;
double deltaAngle = angleDeg / numPoints;
double currentAngle = deltaAngle;
for (int i = 0; i < numPoints; i++ ) {
double x = radius * Math.sin(Math.toRadians(currentAngle)) + centerX;
double y = centerY - radius * Math.cos(Math.toRadians(currentAngle));
if (i == numPoints - 1 && endMarker != null) {
Waypoint point = new Waypoint(new Translation2d(x, y), last.speed, endMarker);
waypoints.add(point);
}
else {
Waypoint point = new Waypoint(new Translation2d(x, y), last.speed);
waypoints.add(point);
}
currentAngle += deltaAngle;
}
}
public static void main(String[] args) {
List<Waypoint> waypoints = new ArrayList<>();
waypoints.add(new Waypoint(new Translation2d(0, 0), 40.0));
waypoints.add(new Waypoint(new Translation2d(-35, 0), 40.0));
Path.addCircleArc(waypoints, 30.0, -45.0, 10, "hopperSensorOn");
waypoints.add(new Waypoint(new Translation2d(-85, 30), 40.0));
for (int i = 0; i < waypoints.size(); i++) {
Waypoint curPoint = waypoints.get(i);
System.out.println("x = " + curPoint.position.x_ + ", y = " + curPoint.position.y_);
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java
-232
View File
@@ -1,232 +0,0 @@
package org.usfirst.frc4388.utility;
import java.awt.Color;
import org.usfirst.frc4388.robot.subsystems.Drive;
import jaci.pathfinder.Pathfinder;
import jaci.pathfinder.Trajectory;
import jaci.pathfinder.Trajectory.Segment;
import jaci.pathfinder.Waypoint;
import jaci.pathfinder.modifiers.TankModifier;
public class PathGenerator {
private Segment[] centerPoints;
private Segment[] leftPoints;
private Segment[] rightPoints;
private int curIndex = 0;
public PathGenerator(Waypoint[] points, double timeStep, double maxVelocity, double maxAccel, double maxJerk) {
boolean negativeFlag = false;
for (int i = 0; i < points.length; i++) {
if (points[i].x < 0) {
negativeFlag = true;
}
}
if (negativeFlag == true) {
for (int i = 0; i < points.length; i++) {
points[i].x = -(points[i].x);
}
}
Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_LOW, timeStep, maxVelocity, maxAccel, maxJerk);
Trajectory trajectory = Pathfinder.generate(points, config);
centerPoints = trajectory.segments;
TankModifier modifier = new TankModifier(trajectory).modify(Drive.TRACK_WIDTH_INCHES);
leftPoints = modifier.getLeftTrajectory().segments;
rightPoints = modifier.getRightTrajectory().segments;
if (negativeFlag == true) {
for (int i = 0; i < centerPoints.length; i++) {
Segment curSeg = centerPoints[i];
curSeg.x = -(curSeg.x);
curSeg.y = -(curSeg.y);
curSeg.jerk = -(curSeg.jerk);
curSeg.acceleration = -(curSeg.acceleration);
curSeg.velocity = -(curSeg.velocity);
curSeg.position = -(curSeg.position);
curSeg.heading = -(curSeg.heading);
curSeg = leftPoints[i];
curSeg.x = -(curSeg.x);
curSeg.y = -(curSeg.y);
curSeg.jerk = -(curSeg.jerk);
curSeg.acceleration = -(curSeg.acceleration);
curSeg.velocity = -(curSeg.velocity);
curSeg.position = -(curSeg.position);
curSeg.heading = -(curSeg.heading);
curSeg = rightPoints[i];
curSeg.x = -(curSeg.x);
curSeg.y = -(curSeg.y);
curSeg.jerk = -(curSeg.jerk);
curSeg.acceleration = -(curSeg.acceleration);
curSeg.velocity = -(curSeg.velocity);
curSeg.position = -(curSeg.position);
curSeg.heading = -(curSeg.heading);
}
}
// TankModifier2 modifier2 = new TankModifier2();
// modifier2.modify(centerPoints, Drive.TRACK_WIDTH_INCHES);
// leftPoints = modifier2.getLeftSegments();
// rightPoints = modifier2.getRightSegments();
}
public Segment getLeftPoint() {
return (curIndex < leftPoints.length) ? leftPoints[curIndex] : null;
}
public Segment getRightPoint() {
return (curIndex < rightPoints.length) ? rightPoints[curIndex] : null;
}
public Segment getCenterPoint() {
return (curIndex < centerPoints.length) ? centerPoints[curIndex] : null;
}
public Double getHeading() {
if (curIndex < centerPoints.length) {
double heading = Math.toDegrees(centerPoints[curIndex].heading);
if (heading > 180) {
heading -= 360;
}
else if (heading < -180) {
heading += 360;
}
return heading;
}
return null;
}
public void incrementCounter() {
curIndex++;
}
public void resetCounter() {
curIndex =0;
}
public Segment[] getCenterPoints() {
return centerPoints;
}
public Segment[] getLeftPoints() {
return leftPoints;
}
public Segment[] getRightPoints() {
return rightPoints;
}
public static void main(String[] args) {
Waypoint[] points = new Waypoint[] {
new Waypoint(0, 0, 0),
new Waypoint(78, 20, Pathfinder.d2r(32))
};
PathGenerator path = new PathGenerator(points, 0.01, 120, 200.0, 700.0);
Segment[] centerSegments = path.getCenterPoints();
Segment[] leftSegments = path.getLeftPoints();
Segment[] rightSegments = path.getRightPoints();
double[] heading = new double[centerSegments.length];
double[] centerX = new double[centerSegments.length];
double[] centerY = new double[centerSegments.length];
double[] centerAccel = new double[centerSegments.length];
double[] centerVelocity = new double[centerSegments.length];
double[] centerPosition = new double[centerSegments.length];
double[] leftX = new double[leftSegments.length];
double[] leftY = new double[leftSegments.length];
double[] leftAccel = new double[leftSegments.length];
double[] leftVelocity = new double[leftSegments.length];
double[] leftPosition = new double[leftSegments.length];
double[] rightX = new double[rightSegments.length];
double[] rightY = new double[rightSegments.length];
double[] rightAccel = new double[rightSegments.length];
double[] rightVelocity = new double[rightSegments.length];
double[] rightPosition = new double[rightSegments.length];
path.resetCounter();
for (int i = 0; i < centerSegments.length; i++) {
heading[i] = path.getHeading();
centerX[i] = path.getCenterPoint().x;
centerY[i] = path.getCenterPoint().y;
centerAccel[i] = path.getCenterPoint().acceleration;
centerVelocity[i] = path.getCenterPoint().velocity;
centerPosition[i] = path.getCenterPoint().position;
leftX[i] = path.getLeftPoint().x;
leftY[i] = path.getLeftPoint().y;
leftAccel[i] = path.getLeftPoint().acceleration;
leftVelocity[i] = path.getLeftPoint().velocity;
leftPosition[i] = path.getLeftPoint().position;
rightX[i] = path.getRightPoint().x;
rightY[i] = path.getRightPoint().y;
rightAccel[i] = path.getRightPoint().acceleration;
rightVelocity[i] = path.getRightPoint().velocity;
rightPosition[i] = path.getRightPoint().position;
path.incrementCounter();
}
FalconLinePlot fig4 = new FalconLinePlot(centerX);
fig4.yGridOn();
fig4.xGridOn();
fig4.setYLabel("X (in)");
fig4.setXLabel("time");
fig4.setTitle("Position Profile X");
fig4.addData(rightX, Color.magenta);
fig4.addData(leftX, Color.blue);
FalconLinePlot fig0 = new FalconLinePlot(centerY);
fig0.yGridOn();
fig0.xGridOn();
fig0.setYLabel("Y (in)");
fig0.setXLabel("time");
fig0.setTitle("Position Profile Y");
fig0.addData(rightY, Color.magenta);
fig0.addData(leftY, Color.blue);
FalconLinePlot fig1 = new FalconLinePlot(centerPosition);
fig1.yGridOn();
fig1.xGridOn();
fig1.setYLabel("Position (in)");
fig1.setXLabel("time (seconds)");
fig1.setTitle("Position Profile for Left and Right Wheels \n Left = Blue, Right = Magenta");
fig1.addData(rightPosition, Color.magenta);
fig1.addData(leftPosition, Color.blue);
FalconLinePlot fig2 = new FalconLinePlot(centerVelocity, Color.green, Color.green);
fig2.yGridOn();
fig2.xGridOn();
fig2.setYLabel("Velocity (in/sec)");
fig2.setXLabel("time (seconds)");
fig2.setTitle("Velocity Profile for Left and Right Wheels \n Left = Blue, Right = Magenta");
fig2.addData(rightVelocity, Color.magenta);
fig2.addData(leftVelocity, Color.blue);
FalconLinePlot fig3 = new FalconLinePlot(centerAccel, Color.green, Color.green);
fig3.yGridOn();
fig3.xGridOn();
fig3.setYLabel("Accel (in/sec^2)");
fig3.setXLabel("time (seconds)");
fig3.setTitle("Accel Profile for Left and Right Wheels \n Left = Blue, Right = Magenta");
fig3.addData(rightAccel, Color.magenta);
fig3.addData(leftAccel, Color.blue);
FalconLinePlot fig5 = new FalconLinePlot(heading);
fig5.yGridOn();
fig5.xGridOn();
fig5.setYLabel("Heading");
fig5.setXLabel("time");
fig5.setTitle("Heading Profile");
FalconLinePlot fig6 = new FalconLinePlot(centerX, centerY);
fig6.yGridOn();
fig6.xGridOn();
fig6.setYLabel("Y");
fig6.setXLabel("X");
fig6.setTitle("XY Profile");
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java
-89
View File
@@ -1,89 +0,0 @@
package org.usfirst.frc4388.utility;
/**
* A PathSegment consists of two Translation2d objects (the start and end
* points) as well as the speed of the robot.
*
*/
public class PathSegment {
protected static final double kEpsilon = 1E-9;
public static class Sample {
public final Translation2d translation;
public final double speed;
public Sample(Translation2d translation, double speed) {
this.translation = translation;
this.speed = speed;
}
}
protected double mSpeed;
protected Translation2d mStart;
protected Translation2d mEnd;
protected Translation2d mStartToEnd; // pre-computed for efficiency
protected double mLength; // pre-computed for efficiency
public static class ClosestPointReport {
public double index; // Index of the point on the path segment (not
// clamped to [0, 1])
public double clamped_index; // As above, but clamped to [0, 1]
public Translation2d closest_point; // The result of
// interpolate(clamped_index)
public double distance; // The distance from closest_point to the query
// point
}
public PathSegment(Translation2d start, Translation2d end, double speed) {
mEnd = end;
mSpeed = speed;
updateStart(start);
}
public void updateStart(Translation2d new_start) {
mStart = new_start;
mStartToEnd = mStart.inverse().translateBy(mEnd);
mLength = mStartToEnd.norm();
}
public double getSpeed() {
return mSpeed;
}
public Translation2d getStart() {
return mStart;
}
public Translation2d getEnd() {
return mEnd;
}
public double getLength() {
return mLength;
}
// Index is on [0, 1]
public Translation2d interpolate(double index) {
return mStart.interpolate(mEnd, index);
}
public double dotProduct(Translation2d other) {
Translation2d start_to_other = mStart.inverse().translateBy(other);
return mStartToEnd.getX() * start_to_other.getX() + mStartToEnd.getY() * start_to_other.getY();
}
public ClosestPointReport getClosestPoint(Translation2d query_point) {
ClosestPointReport rv = new ClosestPointReport();
if (mLength > kEpsilon) {
double dot_product = dotProduct(query_point);
rv.index = dot_product / (mLength * mLength);
rv.clamped_index = Math.min(1.0, Math.max(0.0, rv.index));
rv.closest_point = interpolate(rv.index);
} else {
rv.index = rv.clamped_index = 0.0;
rv.closest_point = new Translation2d(mStart);
}
rv.distance = rv.closest_point.inverse().translateBy(query_point).norm();
return rv;
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/ReflectingCSVWriter.java
+74
View File
@@ -0,0 +1,74 @@
package org.usfirst.frc4388.utility;
import java.io.FileNotFoundException;
import java.io.PrintWriter;
import java.lang.reflect.Field;
import java.util.concurrent.ConcurrentLinkedDeque;
/**
* Writes data to a CSV file
*/
public class ReflectingCSVWriter<T> {
ConcurrentLinkedDeque<String> mLinesToWrite = new ConcurrentLinkedDeque<>();
PrintWriter mOutput = null;
Field[] mFields;
public ReflectingCSVWriter(String fileName, Class<T> typeClass) {
mFields = typeClass.getFields();
try {
mOutput = new PrintWriter(fileName);
} catch (FileNotFoundException e) {
e.printStackTrace();
}
// Write field names.
StringBuffer line = new StringBuffer();
for (Field field : mFields) {
if (line.length() != 0) {
line.append(", ");
}
line.append(field.getName());
}
writeLine(line.toString());
}
public void add(T value) {
StringBuffer line = new StringBuffer();
for (Field field : mFields) {
if (line.length() != 0) {
line.append(", ");
}
try {
line.append(field.get(value).toString());
} catch (IllegalArgumentException e) {
e.printStackTrace();
} catch (IllegalAccessException e) {
e.printStackTrace();
}
}
mLinesToWrite.add(line.toString());
}
protected synchronized void writeLine(String line) {
if (mOutput != null) {
mOutput.println(line);
}
}
// Call this periodically from any thread to write to disk.
public void write() {
while (true) {
String val = mLinesToWrite.pollFirst();
if (val == null) {
break;
}
writeLine(val);
}
}
public synchronized void flush() {
if (mOutput != null) {
write();
mOutput.flush();
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java
+6 -19
View File
@@ -4,12 +4,12 @@ import java.util.ArrayList;
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
//import com.ctre.CANTalon.TalonControlMode;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
public class SoftwarePIDController
{
protected ArrayList<CANTalonEncoder> motorControllers;
protected ArrayList<TalonSRXEncoder> motorControllers;
protected PIDParams pidParams;
protected boolean useGyroLock;
protected double targetGyroAngle;
@@ -21,7 +21,7 @@ public class SoftwarePIDController
protected double prevError = 0.0; // the prior error (used to compute velocity)
protected double totalError = 0.0; // the sum of the errors for use in the integral calc
public SoftwarePIDController( PIDParams pidParams, ArrayList<CANTalonEncoder> motorControllers)
public SoftwarePIDController( PIDParams pidParams, ArrayList<TalonSRXEncoder> motorControllers)
{
this.motorControllers = motorControllers;
setPID(pidParams);
@@ -31,7 +31,6 @@ public class SoftwarePIDController
this.pidParams = pidParams;
}
public void setPIDTurnTarget(double targetAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) {
this.turnType = turnType;
this.targetGyroAngle = targetAngleDeg;
@@ -39,14 +38,8 @@ public class SoftwarePIDController
this.maxError = maxError;
this.maxPrevError = maxPrevError;
// NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below
// for (CANTalonEncoder motorController : motorControllers) {
// motorController.changeControlMode(TalonControlMode.PercentVbus);
// }
prevError = 0.0;
totalError = 0.0;
}
public boolean controlLoopUpdate() {
@@ -63,7 +56,6 @@ public class SoftwarePIDController
return true;
}
if (Math.abs(targetGyroAngle - currentGyroAngle) < pidParams.iZone) {
totalError += error;
}
@@ -79,31 +71,26 @@ public class SoftwarePIDController
// Update the controllers set point.
if (turnType == MPSoftwareTurnType.TANK) {
for (CANTalonEncoder motorController : motorControllers) {
//motorController.set(output);
for (TalonSRXEncoder motorController : motorControllers) {
motorController.set(ControlMode.PercentOutput, output);
}
}
else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) {
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
//motorController.set(0);
motorController.set(ControlMode.PercentOutput, 0);
}
else {
//motorController.set(2.0 * output);
motorController.set(ControlMode.PercentOutput, 2.0 * output);
}
}
}
else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) {
for (CANTalonEncoder motorController : motorControllers) {
for (TalonSRXEncoder motorController : motorControllers) {
if (motorController.isRight()) {
//motorController.set(2.0 * output);
motorController.set(ControlMode.PercentOutput, 2.0 * output);
}
else {
//motorController.set(0);
motorController.set(ControlMode.PercentOutput, 0);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java
-85
View File
@@ -1,85 +0,0 @@
package org.usfirst.frc4388.utility;
import java.util.ArrayList;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj.Timer;
public class SoftwarePIDPositionController
{
//protected ArrayList<CANTalonEncoder> motorControllers;
protected WPI_TalonSRX motorController;
protected PIDParams pidParams;
protected PIDParams PValue;
protected double targetEncoderPosition;
protected double minTurnOutput = 0.002;
protected double maxError;
protected double minError;
protected double maxPrevError; ///??
protected double prevError = 0.0; // the prior error (used to compute velocity)
protected double totalError = 0.0; // the sum of the errors for use in the integral calc
public SoftwarePIDPositionController(PIDParams PValue, WPI_TalonSRX elevatorLeft)
{
this.motorController = elevatorLeft;
setP(PValue);
}
public void setP(PIDParams PValue)
{
this.PValue = PValue;
}
public void setPIDPositionTarget(double targetPosition, double maxError, double minError)
{
this.targetEncoderPosition = targetPosition;
this.maxError = maxError;
this.minError = minError;
//this.maxPrevError = maxPrevError;
prevError = 0.0;
totalError = 0.0;
}
public boolean controlLoopUpdate()
{
return controlLoopUpdate(0);
}
public boolean controlLoopUpdate(double currentEncoderPosition)
{
// Calculate the motion profile feed forward and error feedback terms
double error = targetEncoderPosition - currentEncoderPosition;
//double deltaLastError = (error - prevError);
//Updating the error
//totalError += error;
// Check if we are finished
if (Math.abs(error) < maxError && Math.abs(error) > minError)
{
//Robot.elevator.holdInPos();
return true;
}
double output = pidParams.kP * error; // + pidParams.kI * totalError + pidParams.kD * deltaLastError;
prevError = error;
// Update the controllers set point.
motorController.set(ControlMode.PercentOutput, output);
return false;
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java
+84
View File
@@ -0,0 +1,84 @@
package org.usfirst.frc4388.utility;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
public class TalonSRXEncoder extends WPI_TalonSRX
{
public static int TIMEOUT_MS = 0;
public static int PID_IDX = 0;
private double encoderTicksToWorld;
private boolean isRight = true;
public TalonSRXEncoder(int deviceId, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
this(deviceId, encoderTicksToWorld, false, feedbackDevice);
}
public TalonSRXEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) {
super(deviceNumber);
this.configSelectedFeedbackSensor(feedbackDevice, PID_IDX, TIMEOUT_MS);
this.encoderTicksToWorld = encoderTicksToWorld;
this.isRight = isRight;
}
public boolean isRight() {
return isRight;
}
public void setRight(boolean isRight) {
this.isRight = isRight;
}
public void setPID(int slotId, double kP, double kI, double kD) {
this.setPIDF(slotId, kP, kI, kD, 0);
}
public void setPIDF(int slotId, double kP, double kI, double kD, double kF) {
this.config_kP(slotId, kP, TIMEOUT_MS);
this.config_kI(slotId, kI, TIMEOUT_MS);
this.config_kD(slotId, kD, TIMEOUT_MS);
this.config_kF(slotId, kF, TIMEOUT_MS);
}
public void setPIDFIZone(int slotId, double kP, double kI, double kD, double kF, int iZone) {
this.config_kP(slotId, kP, TIMEOUT_MS);
this.config_kI(slotId, kI, TIMEOUT_MS);
this.config_kD(slotId, kD, TIMEOUT_MS);
this.config_kF(slotId, kF, TIMEOUT_MS);
this.config_IntegralZone(slotId, iZone, TIMEOUT_MS);
}
public double convertEncoderTicksToWorld(double encoderTicks) {
return encoderTicks / encoderTicksToWorld;
}
public int convertEncoderWorldToTicks(double worldValue) {
return (int)(worldValue * encoderTicksToWorld);
}
public void setWorld(ControlMode mode, double worldValue) {
this.set(mode, convertEncoderWorldToTicks(worldValue));
}
public void setPosition(int value) {
this.setSelectedSensorPosition(value, PID_IDX, TIMEOUT_MS);
}
public void setPositionWorld(double worldValue) {
this.setSelectedSensorPosition(convertEncoderWorldToTicks(worldValue), PID_IDX, TIMEOUT_MS);
}
public double getPositionWorld() {
return convertEncoderTicksToWorld(this.getSelectedSensorPosition(PID_IDX));
}
public void setVelocityWorld(double worldValue) {
this.set(ControlMode.Velocity, convertEncoderWorldToTicks(worldValue) * 0.1);
}
public double getVelocityWorld() {
return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(PID_IDX) / 0.1);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXFactory.java
+208
View File
@@ -0,0 +1,208 @@
package org.usfirst.frc4388.utility;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj.MotorSafety;
/**
* Creates TalonSRX objects and configures all the parameters we care about to factory defaults. Closed-loop and sensor
* parameters are not set, as these are expected to be set by the application.
*/
public class TalonSRXFactory {
public static class Configuration {
public boolean LIMIT_SWITCH_NORMALLY_OPEN = true;
public double MAX_OUTPUT = 1;
public double NOMINAL_OUTPUT = 0;
public double PEAK_OUTPUT = 12;
public NeutralMode ENABLE_BRAKE = NeutralMode.Brake;
public boolean ENABLE_CURRENT_LIMIT = false;
public boolean ENABLE_SOFT_LIMIT = false;
public boolean ENABLE_LIMIT_SWITCH = false;
public int CURRENT_LIMIT = 0;
// public double EXPIRATION_TIMEOUT_SECONDS = MotorSafety.DEFAULT_SAFETY_EXPIRATION;
public double FORWARD_SOFT_LIMIT = 0;
public boolean INVERTED = false;
public double NOMINAL_CLOSED_LOOP_VOLTAGE = 12;
public double REVERSE_SOFT_LIMIT = 0;
public boolean SAFETY_ENABLED = false;
public int CONTROL_FRAME_PERIOD_MS = 5;
public int MOTION_CONTROL_FRAME_PERIOD_MS = 100;
public int GENERAL_STATUS_FRAME_RATE_MS = 5;
public int FEEDBACK_STATUS_FRAME_RATE_MS = 100;
public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = 100;
public int ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 100;
public int PULSE_WIDTH_STATUS_FRAME_RATE_MS = 100;
public VelocityMeasPeriod VELOCITY_MEASUREMENT_PERIOD = VelocityMeasPeriod.Period_100Ms;
public int VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW = 64;
public double VOLTAGE_COMPENSATION_RAMP_RATE = 0;
public double VOLTAGE_RAMP_RATE = 0;
}
private static final Configuration kDefaultConfiguration = new Configuration();
private static final Configuration kSlaveConfiguration = new Configuration();
static {
kSlaveConfiguration.CONTROL_FRAME_PERIOD_MS = 1000;
kSlaveConfiguration.MOTION_CONTROL_FRAME_PERIOD_MS = 1000;
kSlaveConfiguration.GENERAL_STATUS_FRAME_RATE_MS = 1000;
kSlaveConfiguration.FEEDBACK_STATUS_FRAME_RATE_MS = 1000;
kSlaveConfiguration.QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000;
kSlaveConfiguration.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000;
kSlaveConfiguration.PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000;
}
// Create a TalonSRX with the default (out of the box) configuration.
public static TalonSRX createDefaultTalon(int id) {
return createTalon(id, kDefaultConfiguration);
}
public static TalonSRXEncoder createTalonEncoder(int id, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
TalonSRXEncoder talon = new TalonSRXEncoder(id, encoderTicksToWorld, feedbackDevice);
// initializeTalon(talon, kDefaultConfiguration);
return talon;
}
public static TalonSRXEncoder createTalonEncoder(int id, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) {
TalonSRXEncoder talon = new TalonSRXEncoder(id, encoderTicksToWorld, isRight, feedbackDevice);
// initializeTalon(talon, kDefaultConfiguration);
return talon;
}
public static TalonSRX createPermanentSlaveTalon(int id, int master_id) {
final TalonSRX talon = createTalon(id, kSlaveConfiguration);
talon.set(ControlMode.Follower, master_id);
return talon;
}
public static TalonSRX createTalon(int id, Configuration config) {
TalonSRX talon = new TalonSRX(id);
// initializeTalon(talon, config);
return talon;
}
public static void initializeTalon(TalonSRX talon, Configuration config) {
// talon.setControlFramePeriod(config.CONTROL_FRAME_PERIOD_MS, TalonSRXEncoder.TIMEOUT_MS);
talon.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS);
talon.setIntegralAccumulator(0, TalonSRXEncoder.PID_IDX, TalonSRXEncoder.TIMEOUT_MS);
talon.clearMotionProfileHasUnderrun(TalonSRXEncoder.TIMEOUT_MS);
talon.clearMotionProfileTrajectories();
talon.clearStickyFaults(TalonSRXEncoder.TIMEOUT_MS);
talon.configPeakOutputForward(config.MAX_OUTPUT, TalonSRXEncoder.TIMEOUT_MS);
talon.configPeakOutputReverse(-config.MAX_OUTPUT, TalonSRXEncoder.TIMEOUT_MS);
talon.configNominalOutputForward(config.NOMINAL_OUTPUT, TalonSRXEncoder.TIMEOUT_MS);
talon.configNominalOutputReverse(-config.NOMINAL_OUTPUT, TalonSRXEncoder.TIMEOUT_MS);
talon.setNeutralMode(config.ENABLE_BRAKE);
talon.enableCurrentLimit(config.ENABLE_CURRENT_LIMIT);
talon.configForwardSoftLimitEnable(config.ENABLE_SOFT_LIMIT, TalonSRXEncoder.TIMEOUT_MS);
talon.configReverseSoftLimitEnable(config.ENABLE_SOFT_LIMIT, TalonSRXEncoder.TIMEOUT_MS);
talon.setInverted(config.INVERTED);
talon.setSensorPhase(false);
talon.setSelectedSensorPosition(0, TalonSRXEncoder.PID_IDX, TalonSRXEncoder.TIMEOUT_MS);
talon.selectProfileSlot(0, TalonSRXEncoder.TIMEOUT_MS);
talon.configVelocityMeasurementPeriod(config.VELOCITY_MEASUREMENT_PERIOD, TalonSRXEncoder.TIMEOUT_MS);
talon.configVelocityMeasurementWindow(config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW, TalonSRXEncoder.TIMEOUT_MS);
talon.setStatusFramePeriod(StatusFrame.Status_1_General, config.GENERAL_STATUS_FRAME_RATE_MS, TalonSRXEncoder.TIMEOUT_MS);
talon.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, config.FEEDBACK_STATUS_FRAME_RATE_MS, TalonSRXEncoder.TIMEOUT_MS);
talon.setStatusFramePeriod(StatusFrame.Status_4_AinTempVbat, config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, TalonSRXEncoder.TIMEOUT_MS);
// talon.setStatusFrameRateMs(TalonSRX.StatusFrameRate.QuadEncoder, config.QUAD_ENCODER_STATUS_FRAME_RATE_MS);
// talon.setStatusFrameRateMs(TalonSRX.StatusFrameRate.PulseWidth, config.PULSE_WIDTH_STATUS_FRAME_RATE_MS);
}
/**
* Run this on a fresh talon to produce good values for the defaults.
*/
public static String getFullTalonInfo(TalonSRX talon) {
StringBuilder sb = new StringBuilder().append("isRevLimitSwitchClosed = ");
// .append(talon.isRevLimitSwitchClosed()).append("\n").append("getBusVoltage = ")
// .append(talon.getBusVoltage()).append("\n").append("isForwardSoftLimitEnabled = ")
// .append(talon.isForwardSoftLimitEnabled()).append("\n").append("getFaultRevSoftLim = ")
// .append(talon.getFaultRevSoftLim()).append("\n").append("getStickyFaultOverTemp = ")
// .append(talon.getStickyFaultOverTemp()).append("\n").append("isZeroSensorPosOnFwdLimitEnabled = ")
// .append(talon.isZeroSensorPosOnFwdLimitEnabled()).append("\n")
// .append("getMotionProfileTopLevelBufferCount = ").append(talon.getMotionProfileTopLevelBufferCount())
// .append("\n").append("getNumberOfQuadIdxRises = ").append(talon.getNumberOfQuadIdxRises()).append("\n")
// .append("getInverted = ").append(talon.getInverted()).append("\n")
// .append("getPulseWidthRiseToRiseUs = ").append(talon.getPulseWidthRiseToRiseUs()).append("\n")
// .append("getError = ").append(talon.getError()).append("\n").append("isSensorPresent = ")
// .append(talon.isSensorPresent(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)).append("\n")
// .append("isControlEnabled = ").append(talon.isControlEnabled()).append("\n").append("getTable = ")
// .append(talon.getTable()).append("\n").append("isEnabled = ").append(talon.isEnabled()).append("\n")
// .append("isZeroSensorPosOnRevLimitEnabled = ").append(talon.isZeroSensorPosOnRevLimitEnabled())
// .append("\n").append("isSafetyEnabled = ").append(talon.isSafetyEnabled()).append("\n")
// .append("getOutputVoltage = ").append(talon.getOutputVoltage()).append("\n").append("getTemperature = ")
// .append(talon.getTemperature()).append("\n").append("getSmartDashboardType = ")
// .append(talon.getSmartDashboardType()).append("\n").append("getPulseWidthPosition = ")
// .append(talon.getPulseWidthPosition()).append("\n").append("getOutputCurrent = ")
// .append(talon.getOutputCurrent()).append("\n").append("get = ").append(talon.get()).append("\n")
// .append("isZeroSensorPosOnIndexEnabled = ").append(talon.isZeroSensorPosOnIndexEnabled()).append("\n")
// .append("getMotionMagicCruiseVelocity = ").append(talon.getMotionMagicCruiseVelocity()).append("\n")
// .append("getStickyFaultRevSoftLim = ").append(talon.getStickyFaultRevSoftLim()).append("\n")
// .append("getFaultRevLim = ").append(talon.getFaultRevLim()).append("\n").append("getEncPosition = ")
// .append(talon.getEncPosition()).append("\n").append("getIZone = ").append(talon.getIZone()).append("\n")
// .append("getAnalogInPosition = ").append(talon.getAnalogInPosition()).append("\n")
// .append("getFaultUnderVoltage = ").append(talon.getFaultUnderVoltage()).append("\n")
// .append("getCloseLoopRampRate = ").append(talon.getCloseLoopRampRate()).append("\n")
// .append("toString = ").append(talon.toString()).append("\n")
// // .append("getMotionMagicActTrajPosition =
// // ").append(talon.getMotionMagicActTrajPosition()).append("\n")
// .append("getF = ").append(talon.getF()).append("\n").append("getClass = ").append(talon.getClass())
// .append("\n").append("getAnalogInVelocity = ").append(talon.getAnalogInVelocity()).append("\n")
// .append("getI = ").append(talon.getI()).append("\n").append("isReverseSoftLimitEnabled = ")
// .append(talon.isReverseSoftLimitEnabled()).append("\n").append("getPIDSourceType = ")
// .append(talon.getPIDSourceType()).append("\n").append("getEncVelocity = ")
// .append(talon.getEncVelocity()).append("\n").append("GetVelocityMeasurementPeriod = ")
// .append(talon.GetVelocityMeasurementPeriod()).append("\n").append("getP = ").append(talon.getP())
// .append("\n").append("GetVelocityMeasurementWindow = ").append(talon.GetVelocityMeasurementWindow())
// .append("\n").append("getDeviceID = ").append(talon.getDeviceID()).append("\n")
// .append("getStickyFaultRevLim = ").append(talon.getStickyFaultRevLim()).append("\n")
// // .append("getMotionMagicActTrajVelocity =
// // ").append(talon.getMotionMagicActTrajVelocity()).append("\n")
// .append("getReverseSoftLimit = ").append(talon.getReverseSoftLimit()).append("\n").append("getD = ")
// .append(talon.getD()).append("\n").append("getFaultOverTemp = ").append(talon.getFaultOverTemp())
// .append("\n").append("getForwardSoftLimit = ").append(talon.getForwardSoftLimit()).append("\n")
// .append("GetFirmwareVersion = ").append(talon.GetFirmwareVersion()).append("\n")
// .append("getLastError = ").append(talon.getLastError()).append("\n").append("isAlive = ")
// .append(talon.isAlive()).append("\n").append("getPinStateQuadIdx = ").append(talon.getPinStateQuadIdx())
// .append("\n").append("getAnalogInRaw = ").append(talon.getAnalogInRaw()).append("\n")
// .append("getFaultForLim = ").append(talon.getFaultForLim()).append("\n").append("getSpeed = ")
// .append(talon.getSpeed()).append("\n").append("getStickyFaultForLim = ")
// .append(talon.getStickyFaultForLim()).append("\n").append("getFaultForSoftLim = ")
// .append(talon.getFaultForSoftLim()).append("\n").append("getStickyFaultForSoftLim = ")
// .append(talon.getStickyFaultForSoftLim()).append("\n").append("getClosedLoopError = ")
// .append(talon.getClosedLoopError()).append("\n").append("getSetpoint = ").append(talon.getSetpoint())
// .append("\n").append("isMotionProfileTopLevelBufferFull = ")
// .append(talon.isMotionProfileTopLevelBufferFull()).append("\n").append("getDescription = ")
// .append(talon.getDescription()).append("\n").append("hashCode = ").append(talon.hashCode()).append("\n")
// .append("isFwdLimitSwitchClosed = ").append(talon.isFwdLimitSwitchClosed()).append("\n")
// .append("getPinStateQuadA = ").append(talon.getPinStateQuadA()).append("\n")
// .append("getPinStateQuadB = ").append(talon.getPinStateQuadB()).append("\n").append("GetIaccum = ")
// .append(talon.GetIaccum()).append("\n").append("getFaultHardwareFailure = ")
// .append(talon.getFaultHardwareFailure()).append("\n").append("pidGet = ").append(talon.pidGet())
// .append("\n").append("getBrakeEnableDuringNeutral = ").append(talon.getBrakeEnableDuringNeutral())
// .append("\n").append("getStickyFaultUnderVoltage = ").append(talon.getStickyFaultUnderVoltage())
// .append("\n").append("getPulseWidthVelocity = ").append(talon.getPulseWidthVelocity()).append("\n")
// .append("GetNominalClosedLoopVoltage = ").append(talon.GetNominalClosedLoopVoltage()).append("\n")
// .append("getPosition = ").append(talon.getPosition()).append("\n").append("getExpiration = ")
// .append(talon.getExpiration()).append("\n").append("getPulseWidthRiseToFallUs = ")
// .append(talon.getPulseWidthRiseToFallUs()).append("\n")
// // .append("createTableListener = ").append(talon.createTableListener()).append("\n")
// .append("getControlMode = ").append(talon.getControlMode()).append("\n")
// .append("getMotionMagicAcceleration = ").append(talon.getMotionMagicAcceleration()).append("\n")
// .append("getControlMode = ").append(talon.getControlMode());
return sb.toString();
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/Util.java
+46
View File
@@ -0,0 +1,46 @@
package org.usfirst.frc4388.utility;
import java.util.List;
/**
* Contains basic functions that are used often.
*/
public class Util {
/** Prevent this class from being instantiated. */
private Util() {
}
/**
* Limits the given input to the given magnitude.
*/
public static double limit(double v, double maxMagnitude) {
return limit(v, -maxMagnitude, maxMagnitude);
}
public static double limit(double v, double min, double max) {
return Math.min(max, Math.max(min, v));
}
public static String joinStrings(String delim, List<?> strings) {
StringBuilder sb = new StringBuilder();
for (int i = 0; i < strings.size(); ++i) {
sb.append(strings.get(i).toString());
if (i < strings.size() - 1) {
sb.append(delim);
}
}
return sb.toString();
}
public static boolean epsilonEquals(double a, double b, double epsilon) {
return (a - epsilon <= b) && (a + epsilon >= b);
}
public static boolean allCloseTo(List<Double> list, double value, double epsilon) {
boolean result = true;
for (Double value_in : list) {
result &= epsilonEquals(value_in, value, epsilon);
}
return result;
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/control/AdaptivePurePursuitController.java
+200
View File
@@ -0,0 +1,200 @@
package org.usfirst.frc4388.utility.control;
import org.usfirst.frc4388.utility.math.RigidTransform2d;
import org.usfirst.frc4388.utility.math.Rotation2d;
import org.usfirst.frc4388.utility.math.Translation2d;
import org.usfirst.frc4388.utility.math.Twist2d;
/**
* Implements an adaptive pure pursuit controller. See:
* https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4 .pdf
*
* Basically, we find a spot on the path we'd like to follow and calculate the arc necessary to make us land on that
* spot. The target spot is a specified distance ahead of us, and we look further ahead the greater our tracking error.
* We also return the maximum speed we'd like to be going when we reach the target spot.
*/
public class AdaptivePurePursuitController {
private static final double kReallyBigNumber = 1E6;
public static class Command {
public Twist2d delta = Twist2d.identity();
public double cross_track_error;
public double max_velocity;
public double end_velocity;
public Translation2d lookahead_point;
public double remaining_path_length;
public Command() {
}
public Command(Twist2d delta, double cross_track_error, double max_velocity, double end_velocity,
Translation2d lookahead_point, double remaining_path_length) {
this.delta = delta;
this.cross_track_error = cross_track_error;
this.max_velocity = max_velocity;
this.end_velocity = end_velocity;
this.lookahead_point = lookahead_point;
this.remaining_path_length = remaining_path_length;
}
}
Path mPath;
boolean mAtEndOfPath = false;
final boolean mReversed;
final Lookahead mLookahead;
public AdaptivePurePursuitController(Path path, boolean reversed, Lookahead lookahead) {
mPath = path;
mReversed = reversed;
mLookahead = lookahead;
}
/**
* Gives the RigidTransform2d.Delta that the robot should take to follow the path
*
* @param pose
* robot pose
* @return movement command for the robot to follow
*/
public Command update(RigidTransform2d pose) {
if (mReversed) {
pose = new RigidTransform2d(pose.getTranslation(),
pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI)));
}
final Path.TargetPointReport report = mPath.getTargetPoint(pose.getTranslation(), mLookahead);
if (isFinished()) {
// Stop.
return new Command(Twist2d.identity(), report.closest_point_distance, report.max_speed, 0.0,
report.lookahead_point, report.remaining_path_distance);
}
final Arc arc = new Arc(pose, report.lookahead_point);
double scale_factor = 1.0;
// Ensure we don't overshoot the end of the path (once the lookahead speed drops to zero).
if (report.lookahead_point_speed < 1E-6 && report.remaining_path_distance < arc.length) {
scale_factor = Math.max(0.0, report.remaining_path_distance / arc.length);
mAtEndOfPath = true;
} else {
mAtEndOfPath = false;
}
if (mReversed) {
scale_factor *= -1;
}
return new Command(
new Twist2d(scale_factor * arc.length, 0.0,
arc.length * getDirection(pose, report.lookahead_point) * Math.abs(scale_factor) / arc.radius),
report.closest_point_distance, report.max_speed,
report.lookahead_point_speed * Math.signum(scale_factor), report.lookahead_point,
report.remaining_path_distance);
}
public boolean hasPassedMarker(String marker) {
return mPath.hasPassedMarker(marker);
}
public static class Arc {
public Translation2d center;
public double radius;
public double length;
public Arc(RigidTransform2d pose, Translation2d point) {
center = getCenter(pose, point);
radius = new Translation2d(center, point).norm();
length = getLength(pose, point, center, radius);
}
}
/**
* Gives the center of the circle joining the lookahead point and robot pose
*
* @param pose
* robot pose
* @param point
* lookahead point
* @return center of the circle joining the lookahead point and robot pose
*/
public static Translation2d getCenter(RigidTransform2d pose, Translation2d point) {
final Translation2d poseToPointHalfway = pose.getTranslation().interpolate(point, 0.5);
final Rotation2d normal = pose.getTranslation().inverse().translateBy(poseToPointHalfway).direction().normal();
final RigidTransform2d perpendicularBisector = new RigidTransform2d(poseToPointHalfway, normal);
final RigidTransform2d normalFromPose = new RigidTransform2d(pose.getTranslation(),
pose.getRotation().normal());
if (normalFromPose.isColinear(perpendicularBisector.normal())) {
// Special case: center is poseToPointHalfway.
return poseToPointHalfway;
}
return normalFromPose.intersection(perpendicularBisector);
}
/**
* Gives the radius of the circle joining the lookahead point and robot pose
*
* @param pose
* robot pose
* @param point
* lookahead point
* @return radius of the circle joining the lookahead point and robot pose
*/
public static double getRadius(RigidTransform2d pose, Translation2d point) {
Translation2d center = getCenter(pose, point);
return new Translation2d(center, point).norm();
}
/**
* Gives the length of the arc joining the lookahead point and robot pose (assuming forward motion).
*
* @param pose
* robot pose
* @param point
* lookahead point
* @return the length of the arc joining the lookahead point and robot pose
*/
public static double getLength(RigidTransform2d pose, Translation2d point) {
final double radius = getRadius(pose, point);
final Translation2d center = getCenter(pose, point);
return getLength(pose, point, center, radius);
}
public static double getLength(RigidTransform2d pose, Translation2d point, Translation2d center, double radius) {
if (radius < kReallyBigNumber) {
final Translation2d centerToPoint = new Translation2d(center, point);
final Translation2d centerToPose = new Translation2d(center, pose.getTranslation());
// If the point is behind pose, we want the opposite of this angle. To determine if the point is behind,
// check the sign of the cross-product between the normal vector and the vector from pose to point.
final boolean behind = Math.signum(
Translation2d.cross(pose.getRotation().normal().toTranslation(),
new Translation2d(pose.getTranslation(), point))) > 0.0;
final Rotation2d angle = Translation2d.getAngle(centerToPose, centerToPoint);
return radius * (behind ? 2.0 * Math.PI - Math.abs(angle.getRadians()) : Math.abs(angle.getRadians()));
} else {
return new Translation2d(pose.getTranslation(), point).norm();
}
}
/**
* Gives the direction the robot should turn to stay on the path
*
* @param pose
* robot pose
* @param point
* lookahead point
* @return the direction the robot should turn: -1 is left, +1 is right
*/
public static int getDirection(RigidTransform2d pose, Translation2d point) {
Translation2d poseToPoint = new Translation2d(pose.getTranslation(), point);
Translation2d robot = pose.getRotation().toTranslation();
double cross = robot.x() * poseToPoint.y() - robot.y() * poseToPoint.x();
return (cross < 0) ? -1 : 1; // if robot < pose turn left
}
/**
* @return has the robot reached the end of the path
*/
public boolean isFinished() {
return mAtEndOfPath;
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/control/Kinematics.java
+83
View File
@@ -0,0 +1,83 @@
package org.usfirst.frc4388.utility.control;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.utility.math.RigidTransform2d;
import org.usfirst.frc4388.utility.math.Rotation2d;
import org.usfirst.frc4388.utility.math.Twist2d;
/**
* Provides forward and inverse kinematics equations for the robot modeling the wheelbase as a differential drive (with
* a corrective factor to account for skidding).
*/
public class Kinematics {
private static final double kEpsilon = 1E-9;
/**
* Forward kinematics using only encoders, rotation is implicit (less accurate than below, but useful for predicting
* motion)
*/
public static Twist2d forwardKinematics(double left_wheel_delta, double right_wheel_delta) {
double delta_v = (right_wheel_delta - left_wheel_delta) / 2 * Constants.kTrackScrubFactor;
double delta_rotation = delta_v * 2 / Constants.kTrackWidthInches;
return forwardKinematics(left_wheel_delta, right_wheel_delta, delta_rotation);
}
/**
* Forward kinematics using encoders and explicitly measured rotation (ex. from gyro)
*/
public static Twist2d forwardKinematics(double left_wheel_delta, double right_wheel_delta,
double delta_rotation_rads) {
final double dx = (left_wheel_delta + right_wheel_delta) / 2.0;
return new Twist2d(dx, 0, delta_rotation_rads);
}
/**
* For convenience, forward kinematic with an absolute rotation and previous rotation.
*/
public static Twist2d forwardKinematics(Rotation2d prev_heading, double left_wheel_delta, double right_wheel_delta,
Rotation2d current_heading) {
return forwardKinematics(left_wheel_delta, right_wheel_delta,
prev_heading.inverse().rotateBy(current_heading).getRadians());
}
/** Append the result of forward kinematics to a previous pose. */
public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, double left_wheel_delta,
double right_wheel_delta, Rotation2d current_heading) {
Twist2d with_gyro = forwardKinematics(current_pose.getRotation(), left_wheel_delta, right_wheel_delta,
current_heading);
return integrateForwardKinematics(current_pose, with_gyro);
}
/**
* For convenience, integrate forward kinematics with a Twist2d and previous rotation.
*/
public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose,
Twist2d forward_kinematics) {
return current_pose.transformBy(RigidTransform2d.exp(forward_kinematics));
}
/**
* Class that contains left and right wheel velocities
*/
public static class DriveVelocity {
public final double left;
public final double right;
public DriveVelocity(double left, double right) {
this.left = left;
this.right = right;
}
}
/**
* Uses inverse kinematics to convert a Twist2d into left and right wheel velocities
*/
public static DriveVelocity inverseKinematics(Twist2d velocity) {
if (Math.abs(velocity.dtheta) < kEpsilon) {
return new DriveVelocity(velocity.dx, velocity.dx);
}
double delta_v = Constants.kTrackWidthInches * velocity.dtheta / (2 * Constants.kTrackScrubFactor);
return new DriveVelocity(velocity.dx - delta_v, velocity.dx + delta_v);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/control/Lookahead.java
+28
View File
@@ -0,0 +1,28 @@
package org.usfirst.frc4388.utility.control;
/**
* A utility class for interpolating lookahead distance based on current speed.
*/
public class Lookahead {
public final double min_distance;
public final double max_distance;
public final double min_speed;
public final double max_speed;
protected final double delta_distance;
protected final double delta_speed;
public Lookahead(double min_distance, double max_distance, double min_speed, double max_speed) {
this.min_distance = min_distance;
this.max_distance = max_distance;
this.min_speed = min_speed;
this.max_speed = max_speed;
delta_distance = max_distance - min_distance;
delta_speed = max_speed - min_speed;
}
public double getLookaheadForSpeed(double speed) {
double lookahead = delta_distance * (speed - min_speed) / delta_speed + min_distance;
return Double.isNaN(lookahead) ? min_distance : Math.max(min_distance, Math.min(max_distance, lookahead));
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/control/Path.java
+235
View File
@@ -0,0 +1,235 @@
package org.usfirst.frc4388.utility.control;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.utility.math.Translation2d;
import org.usfirst.frc4388.utility.motion.MotionState;
/**
* Class representing the robot's autonomous path.
*
* Field Coordinate System: Uses a right hand coordinate system. Positive x is right, positive y is up, and the origin
* is at the bottom left corner of the field. For angles, 0 degrees is facing right (1, 0) and angles increase as you
* turn counter clockwise.
*
* +Y
* ^
* |
* |------------------------------
* | | <--- FRC Field Boundary
* | |
* | |
* | ----- |
* | |robot|--> 0 deg |
* | ----- |
* ----------------------------------> +X
* (0,0)
*
* Notes:
* 1) The starting point needs to match the first point (x,y) with the proper starting rotation
* 2) When chaining together paths, keep all coordinates for both paths in the reference frame above.
* The second path should start where the first stopped.
* 3) When moving in reverse, you need to set the isReversed flag on the path.
*
*/
public class Path {
List<PathSegment> segments;
PathSegment prevSegment;
HashSet<String> mMarkersCrossed = new HashSet<String>();
public void extrapolateLast() {
PathSegment last = segments.get(segments.size() - 1);
last.extrapolateLookahead(true);
}
public Translation2d getEndPosition() {
return segments.get(segments.size() - 1).getEnd();
}
public Path() {
segments = new ArrayList<PathSegment>();
}
/**
* add a segment to the Path
*
* @param segment
* the segment to add
*/
public void addSegment(PathSegment segment) {
segments.add(segment);
}
/**
* @return the last MotionState in the path
*/
public MotionState getLastMotionState() {
if (segments.size() > 0) {
MotionState endState = segments.get(segments.size() - 1).getEndState();
return new MotionState(0.0, 0.0, endState.vel(), endState.acc());
} else {
return new MotionState(0, 0, 0, 0);
}
}
/**
* get the remaining distance left for the robot to travel on the current segment
*
* @param robotPos
* robot position
* @return remaining distance on current segment
*/
public double getSegmentRemainingDist(Translation2d robotPos) {
PathSegment currentSegment = segments.get(0);
return currentSegment.getRemainingDistance(currentSegment.getClosestPoint(robotPos));
}
/**
* @return the length of the current segment
*/
public double getSegmentLength() {
PathSegment currentSegment = segments.get(0);
return currentSegment.getLength();
}
public static class TargetPointReport {
public Translation2d closest_point;
public double closest_point_distance;
public double closest_point_speed;
public Translation2d lookahead_point;
public double max_speed;
public double lookahead_point_speed;
public double remaining_segment_distance;
public double remaining_path_distance;
public TargetPointReport() {
}
}
/**
* Gives the position of the lookahead point (and removes any segments prior to this point).
*
* @param robot
* Translation of the current robot pose.
* @return report containing everything we might want to know about the target point.
*/
public TargetPointReport getTargetPoint(Translation2d robot, Lookahead lookahead) {
TargetPointReport rv = new TargetPointReport();
PathSegment currentSegment = segments.get(0);
rv.closest_point = currentSegment.getClosestPoint(robot);
rv.closest_point_distance = new Translation2d(robot, rv.closest_point).norm();
/*
* if (segments.size() > 1) { // Check next segment to see if it is closer. final Translation2d
* next_segment_closest_point = segments.get(1).getClosestPoint(robot); final double
* next_segment_closest_point_distance = new Translation2d(robot, next_segment_closest_point) .norm(); if
* (next_segment_closest_point_distance < rv.closest_point_distance) { rv.closest_point =
* next_segment_closest_point; rv.closest_point_distance = next_segment_closest_point_distance;
* removeCurrentSegment(); currentSegment = segments.get(0); } }
*/
rv.remaining_segment_distance = currentSegment.getRemainingDistance(rv.closest_point);
rv.remaining_path_distance = rv.remaining_segment_distance;
for (int i = 1; i < segments.size(); ++i) {
rv.remaining_path_distance += segments.get(i).getLength();
}
rv.closest_point_speed = currentSegment
.getSpeedByDistance(currentSegment.getLength() - rv.remaining_segment_distance);
double lookahead_distance = lookahead.getLookaheadForSpeed(rv.closest_point_speed) + rv.closest_point_distance;
if (rv.remaining_segment_distance < lookahead_distance && segments.size() > 1) {
lookahead_distance -= rv.remaining_segment_distance;
for (int i = 1; i < segments.size(); ++i) {
currentSegment = segments.get(i);
final double length = currentSegment.getLength();
if (length < lookahead_distance && i < segments.size() - 1) {
lookahead_distance -= length;
} else {
break;
}
}
} else {
lookahead_distance += (currentSegment.getLength() - rv.remaining_segment_distance);
}
rv.max_speed = currentSegment.getMaxSpeed();
rv.lookahead_point = currentSegment.getPointByDistance(lookahead_distance);
rv.lookahead_point_speed = currentSegment.getSpeedByDistance(lookahead_distance);
checkSegmentDone(rv.closest_point);
return rv;
}
/**
* Gives the speed the robot should be traveling at the given position
*
* @param robotPos
* position of the robot
* @return speed robot should be traveling
*/
public double getSpeed(Translation2d robotPos) {
PathSegment currentSegment = segments.get(0);
return currentSegment.getSpeedByClosestPoint(robotPos);
}
/**
* Checks if the robot has finished traveling along the current segment then removes it from the path if it has
*
* @param robotPos
* robot position
*/
public void checkSegmentDone(Translation2d robotPos) {
PathSegment currentSegment = segments.get(0);
double remainingDist = currentSegment.getRemainingDistance(currentSegment.getClosestPoint(robotPos));
if (remainingDist < 0.1) {
// System.out.println("Removed segment from path: " + currentSegment);
removeCurrentSegment();
}
}
public void removeCurrentSegment() {
prevSegment = segments.remove(0);
String marker = prevSegment.getMarker();
if (marker != null)
mMarkersCrossed.add(marker);
}
/**
* Ensures that all speeds in the path are attainable and robot can slow down in time
*/
public void verifySpeeds() {
double maxStartSpeed = 0.0;
double[] startSpeeds = new double[segments.size() + 1];
startSpeeds[segments.size()] = 0.0;
for (int i = segments.size() - 1; i >= 0; i--) {
PathSegment segment = segments.get(i);
maxStartSpeed += Math
.sqrt(maxStartSpeed * maxStartSpeed + 2 * Constants.kPathFollowingMaxAccel * segment.getLength());
startSpeeds[i] = segment.getStartState().vel();
// System.out.println(maxStartSpeed + ", " + startSpeeds[i]);
if (startSpeeds[i] > maxStartSpeed) {
startSpeeds[i] = maxStartSpeed;
// System.out.println("Segment starting speed is too high!");
}
maxStartSpeed = startSpeeds[i];
}
for (int i = 0; i < segments.size(); i++) {
PathSegment segment = segments.get(i);
double endSpeed = startSpeeds[i + 1];
MotionState startState = (i > 0) ? segments.get(i - 1).getEndState() : new MotionState(0, 0, 0, 0);
startState = new MotionState(0, 0, startState.vel(), startState.vel());
segment.createMotionProfiler(startState, endSpeed);
}
}
public boolean hasPassedMarker(String marker) {
return mMarkersCrossed.contains(marker);
}
public String toString() {
String str = "";
for (PathSegment s : segments) {
str += s.toString() + "\n";
}
return str;
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathFollower.java
+198
View File
@@ -0,0 +1,198 @@
package org.usfirst.frc4388.utility.control;
import org.usfirst.frc4388.utility.math.RigidTransform2d;
import org.usfirst.frc4388.utility.math.Twist2d;
import org.usfirst.frc4388.utility.motion.MotionProfileConstraints;
import org.usfirst.frc4388.utility.motion.MotionProfileGoal;
import org.usfirst.frc4388.utility.motion.MotionProfileGoal.CompletionBehavior;
import org.usfirst.frc4388.utility.motion.MotionState;
import org.usfirst.frc4388.utility.motion.ProfileFollower;
/**
* A PathFollower follows a predefined path using a combination of feedforward and feedback control. It uses an
* AdaptivePurePursuitController to choose a reference pose and generate a steering command (curvature), and then a
* ProfileFollower to generate a profile (displacement and velocity) command.
*/
public class PathFollower {
private static final double kReallyBigNumber = 1E6;
public static class DebugOutput {
public double t;
public double pose_x;
public double pose_y;
public double pose_theta;
public double linear_displacement;
public double linear_velocity;
public double profile_displacement;
public double profile_velocity;
public double velocity_command_dx;
public double velocity_command_dy;
public double velocity_command_dtheta;
public double steering_command_dx;
public double steering_command_dy;
public double steering_command_dtheta;
public double cross_track_error;
public double along_track_error;
public double lookahead_point_x;
public double lookahead_point_y;
public double lookahead_point_velocity;
}
public static class Parameters {
public final Lookahead lookahead;
public final double inertia_gain;
public final double profile_kp;
public final double profile_ki;
public final double profile_kv;
public final double profile_kffv;
public final double profile_kffa;
public final double profile_max_abs_vel;
public final double profile_max_abs_acc;
public final double goal_pos_tolerance;
public final double goal_vel_tolerance;
public final double stop_steering_distance;
public Parameters(Lookahead lookahead, double inertia_gain, double profile_kp, double profile_ki,
double profile_kv, double profile_kffv, double profile_kffa, double profile_max_abs_vel,
double profile_max_abs_acc, double goal_pos_tolerance, double goal_vel_tolerance,
double stop_steering_distance) {
this.lookahead = lookahead;
this.inertia_gain = inertia_gain;
this.profile_kp = profile_kp;
this.profile_ki = profile_ki;
this.profile_kv = profile_kv;
this.profile_kffv = profile_kffv;
this.profile_kffa = profile_kffa;
this.profile_max_abs_vel = profile_max_abs_vel;
this.profile_max_abs_acc = profile_max_abs_acc;
this.goal_pos_tolerance = goal_pos_tolerance;
this.goal_vel_tolerance = goal_vel_tolerance;
this.stop_steering_distance = stop_steering_distance;
}
}
AdaptivePurePursuitController mSteeringController;
Twist2d mLastSteeringDelta;
ProfileFollower mVelocityController;
final double mInertiaGain;
boolean overrideFinished = false;
boolean doneSteering = false;
DebugOutput mDebugOutput = new DebugOutput();
double mMaxProfileVel;
double mMaxProfileAcc;
final double mGoalPosTolerance;
final double mGoalVelTolerance;
final double mStopSteeringDistance;
double mCrossTrackError = 0.0;
double mAlongTrackError = 0.0;
/**
* Create a new PathFollower for a given path.
*/
public PathFollower(Path path, boolean reversed, Parameters parameters) {
mSteeringController = new AdaptivePurePursuitController(path, reversed, parameters.lookahead);
mLastSteeringDelta = Twist2d.identity();
mVelocityController = new ProfileFollower(parameters.profile_kp, parameters.profile_ki, parameters.profile_kv,
parameters.profile_kffv, parameters.profile_kffa);
mVelocityController.setConstraints(
new MotionProfileConstraints(parameters.profile_max_abs_vel, parameters.profile_max_abs_acc));
mMaxProfileVel = parameters.profile_max_abs_vel;
mMaxProfileAcc = parameters.profile_max_abs_acc;
mGoalPosTolerance = parameters.goal_pos_tolerance;
mGoalVelTolerance = parameters.goal_vel_tolerance;
mInertiaGain = parameters.inertia_gain;
mStopSteeringDistance = parameters.stop_steering_distance;
}
/**
* Get new velocity commands to follow the path.
*
* @param t
* The current timestamp
* @param pose
* The current robot pose
* @param displacement
* The current robot displacement (total distance driven).
* @param velocity
* The current robot velocity.
* @return The velocity command to apply
*/
public synchronized Twist2d update(double t, RigidTransform2d pose, double displacement, double velocity) {
if (!mSteeringController.isFinished()) {
final AdaptivePurePursuitController.Command steering_command = mSteeringController.update(pose);
mDebugOutput.lookahead_point_x = steering_command.lookahead_point.x();
mDebugOutput.lookahead_point_y = steering_command.lookahead_point.y();
mDebugOutput.lookahead_point_velocity = steering_command.end_velocity;
mDebugOutput.steering_command_dx = steering_command.delta.dx;
mDebugOutput.steering_command_dy = steering_command.delta.dy;
mDebugOutput.steering_command_dtheta = steering_command.delta.dtheta;
mCrossTrackError = steering_command.cross_track_error;
mLastSteeringDelta = steering_command.delta;
mVelocityController.setGoalAndConstraints(
new MotionProfileGoal(displacement + steering_command.delta.dx,
Math.abs(steering_command.end_velocity), CompletionBehavior.VIOLATE_MAX_ACCEL,
mGoalPosTolerance, mGoalVelTolerance),
new MotionProfileConstraints(Math.min(mMaxProfileVel, steering_command.max_velocity),
mMaxProfileAcc));
if (steering_command.remaining_path_length < mStopSteeringDistance) {
doneSteering = true;
}
}
final double velocity_command = mVelocityController.update(new MotionState(t, displacement, velocity, 0.0), t);
mAlongTrackError = mVelocityController.getPosError();
final double curvature = mLastSteeringDelta.dtheta / mLastSteeringDelta.dx;
double dtheta = mLastSteeringDelta.dtheta;
if (!Double.isNaN(curvature) && Math.abs(curvature) < kReallyBigNumber) {
// Regenerate angular velocity command from adjusted curvature.
final double abs_velocity_setpoint = Math.abs(mVelocityController.getSetpoint().vel());
dtheta = mLastSteeringDelta.dx * curvature * (1.0 + mInertiaGain * abs_velocity_setpoint);
}
final double scale = velocity_command / mLastSteeringDelta.dx;
final Twist2d rv = new Twist2d(mLastSteeringDelta.dx * scale, 0.0, dtheta * scale);
// Fill out debug.
mDebugOutput.t = t;
mDebugOutput.pose_x = pose.getTranslation().x();
mDebugOutput.pose_y = pose.getTranslation().y();
mDebugOutput.pose_theta = pose.getRotation().getRadians();
mDebugOutput.linear_displacement = displacement;
mDebugOutput.linear_velocity = velocity;
mDebugOutput.profile_displacement = mVelocityController.getSetpoint().pos();
mDebugOutput.profile_velocity = mVelocityController.getSetpoint().vel();
mDebugOutput.velocity_command_dx = rv.dx;
mDebugOutput.velocity_command_dy = rv.dy;
mDebugOutput.velocity_command_dtheta = rv.dtheta;
mDebugOutput.cross_track_error = mCrossTrackError;
mDebugOutput.along_track_error = mAlongTrackError;
return rv;
}
public double getCrossTrackError() {
return mCrossTrackError;
}
public double getAlongTrackError() {
return mAlongTrackError;
}
public DebugOutput getDebug() {
return mDebugOutput;
}
public boolean isFinished() {
return (mSteeringController.isFinished() && mVelocityController.isFinishedProfile()
&& mVelocityController.onTarget()) || overrideFinished;
}
public void forceFinish() {
overrideFinished = true;
}
public boolean hasPassedMarker(String marker) {
return mSteeringController.hasPassedMarker(marker);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/control/PathSegment.java
+287
View File
@@ -0,0 +1,287 @@
package org.usfirst.frc4388.utility.control;
import java.util.Optional;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.utility.math.Rotation2d;
import org.usfirst.frc4388.utility.math.Translation2d;
import org.usfirst.frc4388.utility.motion.MotionProfile;
import org.usfirst.frc4388.utility.motion.MotionProfileConstraints;
import org.usfirst.frc4388.utility.motion.MotionProfileGenerator;
import org.usfirst.frc4388.utility.motion.MotionProfileGoal;
import org.usfirst.frc4388.utility.motion.MotionState;
/**
* Class representing a segment of the robot's autonomous path.
*/
public class PathSegment {
private Translation2d start;
private Translation2d end;
private Translation2d center;
private Translation2d deltaStart;
private Translation2d deltaEnd;
private double maxSpeed;
private boolean isLine;
private MotionProfile speedController;
private boolean extrapolateLookahead;
private String marker;
/**
* Constructor for a linear segment
*
* @param x1
* start x
* @param y1
* start y
* @param x2
* end x
* @param y2
* end y
* @param maxSpeed
* maximum speed allowed on the segment
*/
public PathSegment(double x1, double y1, double x2, double y2, double maxSpeed, MotionState startState,
double endSpeed) {
this.start = new Translation2d(x1, y1);
this.end = new Translation2d(x2, y2);
this.deltaStart = new Translation2d(start, end);
this.maxSpeed = maxSpeed;
extrapolateLookahead = false;
isLine = true;
createMotionProfiler(startState, endSpeed);
}
public PathSegment(double x1, double y1, double x2, double y2, double maxSpeed, MotionState startState,
double endSpeed, String marker) {
this.start = new Translation2d(x1, y1);
this.end = new Translation2d(x2, y2);
this.deltaStart = new Translation2d(start, end);
this.maxSpeed = maxSpeed;
extrapolateLookahead = false;
isLine = true;
this.marker = marker;
createMotionProfiler(startState, endSpeed);
}
/**
* Constructor for an arc segment
*
* @param x1
* start x
* @param y1
* start y
* @param x2
* end x
* @param y2
* end y
* @param cx
* center x
* @param cy
* center y
* @param maxSpeed
* maximum speed allowed on the segment
*/
public PathSegment(double x1, double y1, double x2, double y2, double cx, double cy, double maxSpeed,
MotionState startState, double endSpeed) {
this.start = new Translation2d(x1, y1);
this.end = new Translation2d(x2, y2);
this.center = new Translation2d(cx, cy);
this.deltaStart = new Translation2d(center, start);
this.deltaEnd = new Translation2d(center, end);
this.maxSpeed = maxSpeed;
extrapolateLookahead = false;
isLine = false;
createMotionProfiler(startState, endSpeed);
}
public PathSegment(double x1, double y1, double x2, double y2, double cx, double cy, double maxSpeed,
MotionState startState, double endSpeed, String marker) {
this.start = new Translation2d(x1, y1);
this.end = new Translation2d(x2, y2);
this.center = new Translation2d(cx, cy);
this.deltaStart = new Translation2d(center, start);
this.deltaEnd = new Translation2d(center, end);
this.maxSpeed = maxSpeed;
extrapolateLookahead = false;
isLine = false;
this.marker = marker;
createMotionProfiler(startState, endSpeed);
}
/**
* @return max speed of the segment
*/
public double getMaxSpeed() {
return maxSpeed;
}
public void createMotionProfiler(MotionState start_state, double end_speed) {
MotionProfileConstraints motionConstraints = new MotionProfileConstraints(maxSpeed,
Constants.kPathFollowingMaxAccel);
MotionProfileGoal goal_state = new MotionProfileGoal(getLength(), end_speed);
speedController = MotionProfileGenerator.generateProfile(motionConstraints, goal_state, start_state);
// System.out.println(speedController);
}
/**
* @return starting point of the segment
*/
public Translation2d getStart() {
return start;
}
/**
* @return end point of the segment
*/
public Translation2d getEnd() {
return end;
}
/**
* @return the total length of the segment
*/
public double getLength() {
if (isLine) {
return deltaStart.norm();
} else {
return deltaStart.norm() * Translation2d.getAngle(deltaStart, deltaEnd).getRadians();
}
}
/**
* Set whether or not to extrapolate the lookahead point. Should only be true for the last segment in the path
*
* @param val
*/
public void extrapolateLookahead(boolean val) {
extrapolateLookahead = val;
}
/**
* Gets the point on the segment closest to the robot
*
* @param position
* the current position of the robot
* @return the point on the segment closest to the robot
*/
public Translation2d getClosestPoint(Translation2d position) {
if (isLine) {
Translation2d delta = new Translation2d(start, end);
double u = ((position.x() - start.x()) * delta.x() + (position.y() - start.y()) * delta.y())
/ (delta.x() * delta.x() + delta.y() * delta.y());
if (u >= 0 && u <= 1)
return new Translation2d(start.x() + u * delta.x(), start.y() + u * delta.y());
return (u < 0) ? start : end;
} else {
Translation2d deltaPosition = new Translation2d(center, position);
deltaPosition = deltaPosition.scale(deltaStart.norm() / deltaPosition.norm());
if (Translation2d.cross(deltaPosition, deltaStart) * Translation2d.cross(deltaPosition, deltaEnd) < 0) {
return center.translateBy(deltaPosition);
} else {
Translation2d startDist = new Translation2d(position, start);
Translation2d endDist = new Translation2d(position, end);
return (endDist.norm() < startDist.norm()) ? end : start;
}
}
}
/**
* Calculates the point on the segment <code>dist</code> distance from the starting point along the segment.
*
* @param dist
* distance from the starting point
* @return point on the segment <code>dist</code> distance from the starting point
*/
public Translation2d getPointByDistance(double dist) {
double length = getLength();
if (!extrapolateLookahead && dist > length) {
dist = length;
}
if (isLine) {
return start.translateBy(deltaStart.scale(dist / length));
} else {
double deltaAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians()
* ((Translation2d.cross(deltaStart, deltaEnd) >= 0) ? 1 : -1);
deltaAngle *= dist / length;
Translation2d t = deltaStart.rotateBy(Rotation2d.fromRadians(deltaAngle));
return center.translateBy(t);
}
}
/**
* Gets the remaining distance left on the segment from point <code>point</code>
*
* @param point
* result of <code>getClosestPoint()</code>
* @return distance remaining
*/
public double getRemainingDistance(Translation2d position) {
if (isLine) {
return new Translation2d(end, position).norm();
} else {
Translation2d deltaPosition = new Translation2d(center, position);
double angle = Translation2d.getAngle(deltaEnd, deltaPosition).getRadians();
double totalAngle = Translation2d.getAngle(deltaStart, deltaEnd).getRadians();
return angle / totalAngle * getLength();
}
}
private double getDistanceTravelled(Translation2d robotPosition) {
Translation2d pathPosition = getClosestPoint(robotPosition);
double remainingDist = getRemainingDistance(pathPosition);
return getLength() - remainingDist;
}
public double getSpeedByDistance(double dist) {
if (dist < speedController.startPos()) {
dist = speedController.startPos();
} else if (dist > speedController.endPos()) {
dist = speedController.endPos();
}
Optional<MotionState> state = speedController.firstStateByPos(dist);
if (state.isPresent()) {
return state.get().vel();
} else {
System.out.println("Velocity does not exist at that position!");
return 0.0;
}
}
public double getSpeedByClosestPoint(Translation2d robotPosition) {
return getSpeedByDistance(getDistanceTravelled(robotPosition));
}
public MotionState getEndState() {
return speedController.endState();
}
public MotionState getStartState() {
return speedController.startState();
}
public String getMarker() {
return marker;
}
public String toString() {
if (isLine) {
return "(" + "start: " + start + ", end: " + end + ", speed: " + maxSpeed // + ", profile: " +
// speedController
+ ")";
} else {
return "(" + "start: " + start + ", end: " + end + ", center: " + center + ", speed: " + maxSpeed
+ ")"; // + ", profile: " + speedController + ")";
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java
+133
View File
@@ -0,0 +1,133 @@
package org.usfirst.frc4388.utility.control;
import java.util.Map;
import org.usfirst.frc4388.robot.Robot.OperationMode;
import org.usfirst.frc4388.utility.math.InterpolatingDouble;
import org.usfirst.frc4388.utility.math.InterpolatingTreeMap;
import org.usfirst.frc4388.utility.math.RigidTransform2d;
import org.usfirst.frc4388.utility.math.Rotation2d;
import org.usfirst.frc4388.utility.math.Twist2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* RobotState keeps track of the poses of various coordinate frames throughout the match. A coordinate frame is simply a
* point and direction in space that defines an (x,y) coordinate system. Transforms (or poses) keep track of the spatial
* relationship between different frames.
*
* Robot frames of interest (from parent to child):
*
* 1. Field frame: origin is where the robot is turned on
*
* 2. Vehicle frame: origin is the center of the robot wheelbase, facing forwards
*
* 3. Camera frame: origin is the center of the camera imager relative to the robot base.
*
* 4. Goal frame: origin is the center of the boiler (note that orientation in this frame is arbitrary). Also note that
* there can be multiple goal frames.
*
* As a kinematic chain with 4 frames, there are 3 transforms of interest:
*
* 1. Field-to-vehicle: This is tracked over time by integrating encoder and gyro measurements. It will inevitably
* drift, but is usually accurate over short time periods.
*
* 2. Vehicle-to-camera: This is a constant.
*
* 3. Camera-to-goal: This is a pure translation, and is measured by the vision system.
*/
public class RobotState {
private static RobotState instance_ = new RobotState();
public static RobotState getInstance() {
return instance_;
}
private static final int kObservationBufferSize = 100;
// FPGATimestamp -> RigidTransform2d or Rotation2d
private InterpolatingTreeMap<InterpolatingDouble, RigidTransform2d> field_to_vehicle_;
private Twist2d vehicle_velocity_predicted_;
private Twist2d vehicle_velocity_measured_;
private double distance_driven_;
private RobotState() {
reset(0, new RigidTransform2d());
}
/**
* Resets the field to robot transform (robot's position on the field)
*/
public synchronized void reset(double start_time, RigidTransform2d initial_field_to_vehicle) {
field_to_vehicle_ = new InterpolatingTreeMap<>(kObservationBufferSize);
field_to_vehicle_.put(new InterpolatingDouble(start_time), initial_field_to_vehicle);
vehicle_velocity_predicted_ = Twist2d.identity();
vehicle_velocity_measured_ = Twist2d.identity();
distance_driven_ = 0.0;
}
public synchronized void resetDistanceDriven() {
distance_driven_ = 0.0;
}
/**
* Returns the robot's position on the field at a certain time. Linearly interpolates between stored robot positions
* to fill in the gaps.
*/
public synchronized RigidTransform2d getFieldToVehicle(double timestamp) {
return field_to_vehicle_.getInterpolated(new InterpolatingDouble(timestamp));
}
public synchronized Map.Entry<InterpolatingDouble, RigidTransform2d> getLatestFieldToVehicle() {
return field_to_vehicle_.lastEntry();
}
public synchronized RigidTransform2d getPredictedFieldToVehicle(double lookahead_time) {
return getLatestFieldToVehicle().getValue()
.transformBy(RigidTransform2d.exp(vehicle_velocity_predicted_.scaled(lookahead_time)));
}
public synchronized void addFieldToVehicleObservation(double timestamp, RigidTransform2d observation) {
field_to_vehicle_.put(new InterpolatingDouble(timestamp), observation);
}
public synchronized void addObservations(double timestamp, Twist2d measured_velocity,
Twist2d predicted_velocity) {
addFieldToVehicleObservation(timestamp,
Kinematics.integrateForwardKinematics(getLatestFieldToVehicle().getValue(), measured_velocity));
vehicle_velocity_measured_ = measured_velocity;
vehicle_velocity_predicted_ = predicted_velocity;
}
public synchronized Twist2d generateOdometryFromSensors(double left_encoder_delta_distance,
double right_encoder_delta_distance, Rotation2d current_gyro_angle) {
final RigidTransform2d last_measurement = getLatestFieldToVehicle().getValue();
final Twist2d delta = Kinematics.forwardKinematics(last_measurement.getRotation(),
left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle);
distance_driven_ += delta.dx;
return delta;
}
public synchronized double getDistanceDriven() {
return distance_driven_;
}
public synchronized Twist2d getPredictedVelocity() {
return vehicle_velocity_predicted_;
}
public synchronized Twist2d getMeasuredVelocity() {
return vehicle_velocity_measured_;
}
public void updateStatus(OperationMode operationMode) {
if (operationMode == OperationMode.TEST) {
RigidTransform2d odometry = getLatestFieldToVehicle().getValue();
SmartDashboard.putNumber("robot_pose_x", odometry.getTranslation().x());
SmartDashboard.putNumber("robot_pose_y", odometry.getTranslation().y());
SmartDashboard.putNumber("robot_pose_theta", odometry.getRotation().getDegrees());
SmartDashboard.putNumber("robot velocity", vehicle_velocity_measured_.dx);
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/control/SynchronousPIDF.java
+333
View File
@@ -0,0 +1,333 @@
package org.usfirst.frc4388.utility.control;
//import edu.wpi.first.wpilibj.util.BoundaryException;
/**
* This class implements a PID Control Loop.
*
* Does all computation synchronously (i.e. the calculate() function must be called by the user from his own thread)
*/
public class SynchronousPIDF {
private double m_P; // factor for "proportional" control
private double m_I; // factor for "integral" control
private double m_D; // factor for "derivative" control
private double m_F; // factor for feed forward gain
private double m_maximumOutput = 1.0; // |maximum output|
private double m_minimumOutput = -1.0; // |minimum output|
private double m_maximumInput = 0.0; // maximum input - limit setpoint to
// this
private double m_minimumInput = 0.0; // minimum input - limit setpoint to
// this
private boolean m_continuous = false; // do the endpoints wrap around? eg.
// Absolute encoder
private double m_prevError = 0.0; // the prior sensor input (used to compute
// velocity)
private double m_totalError = 0.0; // the sum of the errors for use in the
// integral calc
private double m_setpoint = 0.0;
private double m_error = 0.0;
private double m_result = 0.0;
private double m_last_input = Double.NaN;
private double m_deadband = 0.0; // If the absolute error is less than
// deadband
// then treat error for the proportional
// term as 0
public SynchronousPIDF() {
}
/**
* Allocate a PID object with the given constants for P, I, D
*
* @param Kp
* the proportional coefficient
* @param Ki
* the integral coefficient
* @param Kd
* the derivative coefficient
*/
public SynchronousPIDF(double Kp, double Ki, double Kd) {
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = 0;
}
/**
* Allocate a PID object with the given constants for P, I, D
*
* @param Kp
* the proportional coefficient
* @param Ki
* the integral coefficient
* @param Kd
* the derivative coefficient
* @param Kf
* the feed forward gain coefficient
*/
public SynchronousPIDF(double Kp, double Ki, double Kd, double Kf) {
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
}
/**
* Read the input, calculate the output accordingly, and write to the output. This should be called at a constant
* rate by the user (ex. in a timed thread)
*
* @param input
* the input
* @param dt
* time passed since previous call to calculate
*/
public double calculate(double input, double dt) {
if (dt < 1E-6)
dt = 1E-6;
m_last_input = input;
m_error = m_setpoint - input;
if (m_continuous) {
if (Math.abs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
if (m_error > 0) {
m_error = m_error - m_maximumInput + m_minimumInput;
} else {
m_error = m_error + m_maximumInput - m_minimumInput;
}
}
}
if ((m_error * m_P < m_maximumOutput) && (m_error * m_P > m_minimumOutput)) {
m_totalError += m_error * dt;
} else {
m_totalError = 0;
}
// Don't blow away m_error so as to not break derivative
double proportionalError = Math.abs(m_error) < m_deadband ? 0 : m_error;
m_result = (m_P * proportionalError + m_I * m_totalError + m_D * (m_error - m_prevError) / dt
+ m_F * m_setpoint);
m_prevError = m_error;
if (m_result > m_maximumOutput) {
m_result = m_maximumOutput;
} else if (m_result < m_minimumOutput) {
m_result = m_minimumOutput;
}
return m_result;
}
/**
* Set the PID controller gain parameters. Set the proportional, integral, and differential coefficients.
*
* @param p
* Proportional coefficient
* @param i
* Integral coefficient
* @param d
* Differential coefficient
*/
public void setPID(double p, double i, double d) {
m_P = p;
m_I = i;
m_D = d;
}
/**
* Set the PID controller gain parameters. Set the proportional, integral, and differential coefficients.
*
* @param p
* Proportional coefficient
* @param i
* Integral coefficient
* @param d
* Differential coefficient
* @param f
* Feed forward coefficient
*/
public void setPID(double p, double i, double d, double f) {
m_P = p;
m_I = i;
m_D = d;
m_F = f;
}
/**
* Get the Proportional coefficient
*
* @return proportional coefficient
*/
public double getP() {
return m_P;
}
/**
* Get the Integral coefficient
*
* @return integral coefficient
*/
public double getI() {
return m_I;
}
/**
* Get the Differential coefficient
*
* @return differential coefficient
*/
public double getD() {
return m_D;
}
/**
* Get the Feed forward coefficient
*
* @return feed forward coefficient
*/
public double getF() {
return m_F;
}
/**
* Return the current PID result This is always centered on zero and constrained the the max and min outs
*
* @return the latest calculated output
*/
public double get() {
return m_result;
}
/**
* Set the PID controller to consider the input to be continuous, Rather then using the max and min in as
* constraints, it considers them to be the same point and automatically calculates the shortest route to the
* setpoint.
*
* @param continuous
* Set to true turns on continuous, false turns off continuous
*/
public void setContinuous(boolean continuous) {
m_continuous = continuous;
}
public void setDeadband(double deadband) {
m_deadband = deadband;
}
/**
* Set the PID controller to consider the input to be continuous, Rather then using the max and min in as
* constraints, it considers them to be the same point and automatically calculates the shortest route to the
* setpoint.
*/
public void setContinuous() {
this.setContinuous(true);
}
/**
* Sets the maximum and minimum values expected from the input.
*
* @param minimumInput
* the minimum value expected from the input
* @param maximumInput
* the maximum value expected from the output
*/
public void setInputRange(double minimumInput, double maximumInput) {
if (minimumInput > maximumInput) {
//throw new BoundaryException("Lower bound is greater than upper bound");
}
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
setSetpoint(m_setpoint);
}
/**
* Sets the minimum and maximum values to write.
*
* @param minimumOutput
* the minimum value to write to the output
* @param maximumOutput
* the maximum value to write to the output
*/
public void setOutputRange(double minimumOutput, double maximumOutput) {
if (minimumOutput > maximumOutput) {
//throw new BoundaryException("Lower bound is greater than upper bound");
}
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
/**
* Set the setpoint for the PID controller
*
* @param setpoint
* the desired setpoint
*/
public void setSetpoint(double setpoint) {
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput) {
m_setpoint = m_maximumInput;
} else if (setpoint < m_minimumInput) {
m_setpoint = m_minimumInput;
} else {
m_setpoint = setpoint;
}
} else {
m_setpoint = setpoint;
}
}
/**
* Returns the current setpoint of the PID controller
*
* @return the current setpoint
*/
public double getSetpoint() {
return m_setpoint;
}
/**
* Returns the current difference of the input from the setpoint
*
* @return the current error
*/
public double getError() {
return m_error;
}
/**
* Return true if the error is within the tolerance
*
* @return true if the error is less than the tolerance
*/
public boolean onTarget(double tolerance) {
return m_last_input != Double.NaN && Math.abs(m_last_input - m_setpoint) < tolerance;
}
/**
* Reset all internal terms.
*/
public void reset() {
m_last_input = Double.NaN;
m_prevError = 0;
m_totalError = 0;
m_result = 0;
m_setpoint = 0;
}
public void resetIntegrator() {
m_totalError = 0;
}
public String getState() {
String lState = "";
lState += "Kp: " + m_P + "\n";
lState += "Ki: " + m_I + "\n";
lState += "Kd: " + m_D + "\n";
return lState;
}
public String getType() {
return "PIDController";
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/Interpolable.java → 2019robot/src/main/java/org/usfirst/frc4388/utility/math/Interpolable.java
+7 -10
View File
@@ -1,9 +1,8 @@
package org.usfirst.frc4388.utility;
package org.usfirst.frc4388.utility.math;
/**
* Interpolable is an interface used by an Interpolating Tree as the Value type.
* Given two end points and an interpolation parameter on [0, 1], it calculates
* a new Interpolable representing the interpolated value.
* Interpolable is an interface used by an Interpolating Tree as the Value type. Given two end points and an
* interpolation parameter on [0, 1], it calculates a new Interpolable representing the interpolated value.
*
* @param <T>
* The Type of Interpolable
@@ -11,17 +10,15 @@ package org.usfirst.frc4388.utility;
*/
public interface Interpolable<T> {
/**
* Interpolates between this value and an other value according to a given
* parameter. If x is 0, the method should return this value. If x is 1, the
* method should return the other value. If 0 < x < 1, the return value
* should be interpolated proportionally between the two.
* Interpolates between this value and an other value according to a given parameter. If x is 0, the method should
* return this value. If x is 1, the method should return the other value. If 0 < x < 1, the return value should be
* interpolated proportionally between the two.
*
* @param other
* The value of the upper bound
* @param x
* The requested value. Should be between 0 and 1.
* @return Interpolable<T> The estimated average between the surrounding
* data
* @return Interpolable<T> The estimated average between the surrounding data
*/
public T interpolate(T other, double x);
}
2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingDouble.java
+47
View File
@@ -0,0 +1,47 @@
package org.usfirst.frc4388.utility.math;
/**
* A Double that can be interpolated using the InterpolatingTreeMap.
*
* @see InterpolatingTreeMap
*/
public class InterpolatingDouble implements Interpolable<InterpolatingDouble>, InverseInterpolable<InterpolatingDouble>,
Comparable<InterpolatingDouble> {
public Double value = 0.0;
public InterpolatingDouble(Double val) {
value = val;
}
@Override
public InterpolatingDouble interpolate(InterpolatingDouble other, double x) {
Double dydx = other.value - value;
Double searchY = dydx * x + value;
return new InterpolatingDouble(searchY);
}
@Override
public double inverseInterpolate(InterpolatingDouble upper, InterpolatingDouble query) {
double upper_to_lower = upper.value - value;
if (upper_to_lower <= 0) {
return 0;
}
double query_to_lower = query.value - value;
if (query_to_lower <= 0) {
return 0;
}
return query_to_lower / upper_to_lower;
}
@Override
public int compareTo(InterpolatingDouble other) {
if (other.value < value) {
return 1;
} else if (other.value > value) {
return -1;
} else {
return 0;
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingLong.java
+46
View File
@@ -0,0 +1,46 @@
package org.usfirst.frc4388.utility.math;
/**
* A Long that can be interpolated using the InterpolatingTreeMap.
*
* @see InterpolatingTreeMap
*/
public class InterpolatingLong implements Interpolable<InterpolatingLong>, InverseInterpolable<InterpolatingLong>,
Comparable<InterpolatingLong> {
public Long value = 0L;
public InterpolatingLong(Long val) {
value = val;
}
@Override
public InterpolatingLong interpolate(InterpolatingLong other, double x) {
Long dydx = other.value - value;
Double searchY = dydx * x + value;
return new InterpolatingLong(searchY.longValue());
}
@Override
public double inverseInterpolate(InterpolatingLong upper, InterpolatingLong query) {
long upper_to_lower = upper.value - value;
if (upper_to_lower <= 0) {
return 0;
}
long query_to_lower = query.value - value;
if (query_to_lower <= 0) {
return 0;
}
return query_to_lower / (double) upper_to_lower;
}
@Override
public int compareTo(InterpolatingLong other) {
if (other.value < value) {
return 1;
} else if (other.value > value) {
return -1;
} else {
return 0;
}
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java → 2019robot/src/main/java/org/usfirst/frc4388/utility/math/InterpolatingTreeMap.java
+5 -8
View File
@@ -1,12 +1,11 @@
package org.usfirst.frc4388.utility;
package org.usfirst.frc4388.utility.math;
import java.util.Map;
import java.util.TreeMap;
/**
* Interpolating Tree Maps are used to get values at points that are not defined
* by making a guess from points that are defined. This uses linear
* interpolation.
* Interpolating Tree Maps are used to get values at points that are not defined by making a guess from points that are
* defined. This uses linear interpolation.
*
* @param <K>
* The type of the key (must implement InverseInterpolable)
@@ -58,8 +57,7 @@ public class InterpolatingTreeMap<K extends InverseInterpolable<K> & Comparable<
*
* @param key
* Lookup for a value (does not have to exist)
* @return V or null; V if it is Interpolable or exists, null if it is at a
* bound and cannot average
* @return V or null; V if it is Interpolable or exists, null if it is at a bound and cannot average
*/
public V getInterpolated(K key) {
V gotval = get(key);
@@ -69,8 +67,7 @@ public class InterpolatingTreeMap<K extends InverseInterpolable<K> & Comparable<
K bottomBound = floorKey(key);
/**
* If attempting interpolation at ends of tree, return the nearest
* data point
* If attempting interpolation at ends of tree, return the nearest data point
*/
if (topBound == null && bottomBound == null) {
return null;
2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java → 2019robot/src/main/java/org/usfirst/frc4388/utility/math/InverseInterpolable.java
+7 -9
View File
@@ -1,9 +1,8 @@
package org.usfirst.frc4388.utility;
package org.usfirst.frc4388.utility.math;
/**
* InverseInterpolable is an interface used by an Interpolating Tree as the Key
* type. Given two endpoint keys and a third query key, an InverseInterpolable
* object can calculate the interpolation parameter of the query key on the
* InverseInterpolable is an interface used by an Interpolating Tree as the Key type. Given two endpoint keys and a
* third query key, an InverseInterpolable object can calculate the interpolation parameter of the query key on the
* interval [0, 1].
*
* @param <T>
@@ -12,14 +11,13 @@ package org.usfirst.frc4388.utility;
*/
public interface InverseInterpolable<T> {
/**
* Given this point (lower), a query point (query), and an upper point
* (upper), estimate how far (on [0, 1]) between 'lower' and 'upper' the
* query point lies.
* Given this point (lower), a query point (query), and an upper point (upper), estimate how far (on [0, 1]) between
* 'lower' and 'upper' the query point lies.
*
* @param upper
* @param query
* @return The interpolation parameter on [0, 1] representing how far
* between this point and the upper point the query point lies.
* @return The interpolation parameter on [0, 1] representing how far between this point and the upper point the
* query point lies.
*/
public double inverseInterpolate(T upper, T query);
}
2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java → 2019robot/src/main/java/org/usfirst/frc4388/utility/math/RigidTransform2d.java
+76 -25
View File
@@ -1,29 +1,23 @@
package org.usfirst.frc4388.utility;
package org.usfirst.frc4388.utility.math;
import static org.usfirst.frc4388.utility.Util.epsilonEquals;
/**
* Represents a 2d pose (rigid transform) containing translational and
* rotational elements.
* Represents a 2d pose (rigid transform) containing translational and rotational elements.
*
* Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
*/
public class RigidTransform2d implements Interpolable<RigidTransform2d> {
private final static double kEps = 1E-9;
protected static final double kEpsilon = 1E-9;
// Movement along an arc at constant curvature and velocity. We can use
// ideas from "differential calculus" to create new RigidTransform2d's from
// a Delta.
public static class Delta {
public final double dx;
public final double dy;
public final double dtheta;
protected static final RigidTransform2d kIdentity = new RigidTransform2d();
public Delta(double dx, double dy, double dtheta) {
this.dx = dx;
this.dy = dy;
this.dtheta = dtheta;
}
public static final RigidTransform2d identity() {
return kIdentity;
}
private final static double kEps = 1E-9;
protected Translation2d translation_;
protected Rotation2d rotation_;
@@ -54,7 +48,7 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
* Obtain a new RigidTransform2d from a (constant curvature) velocity. See:
* https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp
*/
public static RigidTransform2d fromVelocity(Delta delta) {
public static RigidTransform2d exp(Twist2d delta) {
double sin_theta = Math.sin(delta.dtheta);
double cos_theta = Math.cos(delta.dtheta);
double s, c;
@@ -69,6 +63,24 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
new Rotation2d(cos_theta, sin_theta, false));
}
/**
* Logical inverse of the above.
*/
public static Twist2d log(RigidTransform2d transform) {
final double dtheta = transform.getRotation().getRadians();
final double half_dtheta = 0.5 * dtheta;
final double cos_minus_one = transform.getRotation().cos() - 1.0;
double halftheta_by_tan_of_halfdtheta;
if (Math.abs(cos_minus_one) < kEps) {
halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().sin()) / cos_minus_one;
}
final Translation2d translation_part = transform.getTranslation()
.rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta, false));
return new Twist2d(translation_part.x(), translation_part.y(), dtheta);
}
public Translation2d getTranslation() {
return translation_;
}
@@ -86,8 +98,8 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
}
/**
* Transforming this RigidTransform2d means first translating by
* other.translation and then rotating by other.rotation
* Transforming this RigidTransform2d means first translating by other.translation and then rotating by
* other.rotation
*
* @param other
* The other transform.
@@ -99,8 +111,7 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
}
/**
* The inverse of this transform "undoes" the effect of translating by this
* transform.
* The inverse of this transform "undoes" the effect of translating by this transform.
*
* @return The opposite of this transform.
*/
@@ -109,9 +120,49 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
return new RigidTransform2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted);
}
public RigidTransform2d normal() {
return new RigidTransform2d(translation_, rotation_.normal());
}
/**
* Do linear interpolation of this transform (there are more accurate ways
* using constant curvature, but this is good enough).
* Finds the point where the heading of this transform intersects the heading of another. Returns (+INF, +INF) if
* parallel.
*/
public Translation2d intersection(RigidTransform2d other) {
final Rotation2d other_rotation = other.getRotation();
if (rotation_.isParallel(other_rotation)) {
// Lines are parallel.
return new Translation2d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
}
if (Math.abs(rotation_.cos()) < Math.abs(other_rotation.cos())) {
return intersectionInternal(this, other);
} else {
return intersectionInternal(other, this);
}
}
/**
* Return true if the heading of this transform is colinear with the heading of another.
*/
public boolean isColinear(RigidTransform2d other) {
final Twist2d twist = log(inverse().transformBy(other));
return (epsilonEquals(twist.dy, 0.0, kEpsilon) && epsilonEquals(twist.dtheta, 0.0, kEpsilon));
}
private static Translation2d intersectionInternal(RigidTransform2d a, RigidTransform2d b) {
final Rotation2d a_r = a.getRotation();
final Rotation2d b_r = b.getRotation();
final Translation2d a_t = a.getTranslation();
final Translation2d b_t = b.getTranslation();
final double tan_b = b_r.tan();
final double t = ((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y())
/ (a_r.sin() - a_r.cos() * tan_b);
return a_t.translateBy(a_r.toTranslation().scale(t));
}
/**
* Do twist interpolation of this transform assuming constant curvature.
*/
@Override
public RigidTransform2d interpolate(RigidTransform2d other, double x) {
@@ -120,8 +171,8 @@ public class RigidTransform2d implements Interpolable<RigidTransform2d> {
} else if (x >= 1) {
return new RigidTransform2d(other);
}
return new RigidTransform2d(translation_.interpolate(other.translation_, x),
rotation_.interpolate(other.rotation_, x));
final Twist2d twist = RigidTransform2d.log(inverse().transformBy(other));
return transformBy(RigidTransform2d.exp(twist.scaled(x)));
}
@Override
2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java → 2019robot/src/main/java/org/usfirst/frc4388/utility/math/Rotation2d.java
+31 -11
View File
@@ -1,14 +1,21 @@
package org.usfirst.frc4388.utility;
package org.usfirst.frc4388.utility.math;
import static org.usfirst.frc4388.utility.Util.epsilonEquals;
import java.text.DecimalFormat;
/**
* A rotation in a 2d coordinate frame represented a point on the unit circle
* (cosine and sine).
* A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
*
* Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
*/
public class Rotation2d implements Interpolable<Rotation2d> {
protected static final Rotation2d kIdentity = new Rotation2d();
public static final Rotation2d identity() {
return kIdentity;
}
protected static final double kEpsilon = 1E-9;
protected double cos_angle_;
@@ -31,6 +38,10 @@ public class Rotation2d implements Interpolable<Rotation2d> {
sin_angle_ = other.sin_angle_;
}
public Rotation2d(Translation2d direction, boolean normalize) {
this(direction.x(), direction.y(), normalize);
}
public static Rotation2d fromRadians(double angle_radians) {
return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false);
}
@@ -40,9 +51,8 @@ public class Rotation2d implements Interpolable<Rotation2d> {
}
/**
* From trig, we know that sin^2 + cos^2 == 1, but as we do math on this
* object we might accumulate rounding errors. Normalizing forces us to
* re-scale the sin and cos to reset rounding errors.
* From trig, we know that sin^2 + cos^2 == 1, but as we do math on this object we might accumulate rounding errors.
* Normalizing forces us to re-scale the sin and cos to reset rounding errors.
*/
public void normalize() {
double magnitude = Math.hypot(cos_angle_, sin_angle_);
@@ -64,7 +74,7 @@ public class Rotation2d implements Interpolable<Rotation2d> {
}
public double tan() {
if (cos_angle_ < kEpsilon) {
if (Math.abs(cos_angle_) < kEpsilon) {
if (sin_angle_ >= 0.0) {
return Double.POSITIVE_INFINITY;
} else {
@@ -83,12 +93,10 @@ public class Rotation2d implements Interpolable<Rotation2d> {
}
/**
* We can rotate this Rotation2d by adding together the effects of it and
* another rotation.
* We can rotate this Rotation2d by adding together the effects of it and another rotation.
*
* @param other
* The other rotation. See:
* https://en.wikipedia.org/wiki/Rotation_matrix
* The other rotation. See: https://en.wikipedia.org/wiki/Rotation_matrix
* @return This rotation rotated by other.
*/
public Rotation2d rotateBy(Rotation2d other) {
@@ -96,6 +104,10 @@ public class Rotation2d implements Interpolable<Rotation2d> {
cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true);
}
public Rotation2d normal() {
return new Rotation2d(-sin_angle_, cos_angle_, false);
}
/**
* The inverse of a Rotation2d "undoes" the effect of this rotation.
*
@@ -105,6 +117,14 @@ public class Rotation2d implements Interpolable<Rotation2d> {
return new Rotation2d(cos_angle_, -sin_angle_, false);
}
public boolean isParallel(Rotation2d other) {
return epsilonEquals(Translation2d.cross(toTranslation(), other.toTranslation()), 0.0, kEpsilon);
}
public Translation2d toTranslation() {
return new Translation2d(cos_angle_, sin_angle_);
}
@Override
public Rotation2d interpolate(Rotation2d other, double x) {
if (x <= 0) {
2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java → 2019robot/src/main/java/org/usfirst/frc4388/utility/math/Translation2d.java
+45 -8
View File
@@ -1,12 +1,17 @@
package org.usfirst.frc4388.utility;
package org.usfirst.frc4388.utility.math;
import java.text.DecimalFormat;
/**
* A translation in a 2d coordinate frame. Translations are simply shifts in an
* (x, y) plane.
* A translation in a 2d coordinate frame. Translations are simply shifts in an (x, y) plane.
*/
public class Translation2d implements Interpolable<Translation2d> {
protected static final Translation2d kIdentity = new Translation2d();
public static final Translation2d identity() {
return kIdentity;
}
protected double x_;
protected double y_;
@@ -25,20 +30,29 @@ public class Translation2d implements Interpolable<Translation2d> {
y_ = other.y_;
}
public Translation2d(Translation2d start, Translation2d end) {
x_ = end.x_ - start.x_;
y_ = end.y_ - start.y_;
}
/**
* The "norm" of a transform is the Euclidean distance in x and y.
*
* @return Sqrt(x^2 + y^2)
* @return sqrt(x^2 + y^2)
*/
public double norm() {
return Math.hypot(x_, y_);
}
public double getX() {
public double norm2() {
return x_ * x_ + y_ * y_;
}
public double x() {
return x_;
}
public double getY() {
public double y() {
return y_;
}
@@ -62,8 +76,7 @@ public class Translation2d implements Interpolable<Translation2d> {
}
/**
* We can also rotate Translation2d's. See:
* https://en.wikipedia.org/wiki/Rotation_matrix
* We can also rotate Translation2d's. See: https://en.wikipedia.org/wiki/Rotation_matrix
*
* @param rotation
* The rotation to apply.
@@ -73,6 +86,10 @@ public class Translation2d implements Interpolable<Translation2d> {
return new Translation2d(x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos());
}
public Rotation2d direction() {
return new Rotation2d(x_, y_, true);
}
/**
* The inverse simply means a Translation2d that "undoes" this object.
*
@@ -96,9 +113,29 @@ public class Translation2d implements Interpolable<Translation2d> {
return new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_);
}
public Translation2d scale(double s) {
return new Translation2d(x_ * s, y_ * s);
}
@Override
public String toString() {
final DecimalFormat fmt = new DecimalFormat("#0.000");
return "(" + fmt.format(x_) + "," + fmt.format(y_) + ")";
}
public static double dot(Translation2d a, Translation2d b) {
return a.x_ * b.x_ + a.y_ * b.y_;
}
public static Rotation2d getAngle(Translation2d a, Translation2d b) {
double cos_angle = dot(a, b) / (a.norm() * b.norm());
if (Double.isNaN(cos_angle)) {
return new Rotation2d();
}
return Rotation2d.fromRadians(Math.acos(Math.min(1.0, Math.max(cos_angle, -1.0))));
}
public static double cross(Translation2d a, Translation2d b) {
return a.x_ * b.y_ - a.y_ * b.x_;
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/math/Twist2d.java
+29
View File
@@ -0,0 +1,29 @@
package org.usfirst.frc4388.utility.math;
/**
* A movement along an arc at constant curvature and velocity. We can use ideas from "differential calculus" to create
* new RigidTransform2d's from a Twist2d and visa versa.
*
* A Twist can be used to represent a difference between two poses, a velocity, an acceleration, etc.
*/
public class Twist2d {
protected static final Twist2d kIdentity = new Twist2d(0.0, 0.0, 0.0);
public static final Twist2d identity() {
return kIdentity;
}
public final double dx;
public final double dy;
public final double dtheta; // Radians!
public Twist2d(double dx, double dy, double dtheta) {
this.dx = dx;
this.dy = dy;
this.dtheta = dtheta;
}
public Twist2d scaled(double scale) {
return new Twist2d(dx * scale, dy * scale, dtheta * scale);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/HeadingProfileFollower.java
+43
View File
@@ -0,0 +1,43 @@
package org.usfirst.frc4388.utility.motion;
import org.usfirst.frc4388.utility.math.Rotation2d;
/**
* Class to deal with angle wrapping for following a heading profile. All states are assumed to be in units of degrees,
* and wrap on the interval of [-180, 180].
*/
public class HeadingProfileFollower extends ProfileFollower {
public HeadingProfileFollower(double kp, double ki, double kv, double kffv, double kffa) {
super(kp, ki, kv, kffv, kffa);
}
@Override
public double update(MotionState latest_state, double t) {
final Rotation2d goal_rotation_inverse = Rotation2d.fromDegrees(mGoal.pos()).inverse();
// Update both the setpoint and latest state to be relative to the new goal.
if (mLatestSetpoint != null) {
mLatestSetpoint.motion_state = new MotionState(mLatestSetpoint.motion_state.t(),
mGoal.pos() + goal_rotation_inverse
.rotateBy(Rotation2d.fromDegrees(mLatestSetpoint.motion_state.pos())).getDegrees(),
mLatestSetpoint.motion_state.vel(), mLatestSetpoint.motion_state.acc());
}
final MotionState latest_state_unwrapped = new MotionState(latest_state.t(),
mGoal.pos() + goal_rotation_inverse.rotateBy(Rotation2d.fromDegrees(latest_state.pos())).getDegrees(),
latest_state.vel(), latest_state.acc());
double result = super.update(latest_state_unwrapped, t);
// Reset the integrator when we are close to the goal (encourage stiction!).
if (Math.abs(latest_state_unwrapped.pos() - mGoal.pos()) < mGoal.pos_tolerance()) {
result = 0.0;
super.resetIntegral();
}
return result;
}
/**
* Convert a motion state representing an angle to a properly wrapped angle.
*/
public static MotionState canonicalize(MotionState state) {
return new MotionState(state.t(), Rotation2d.fromDegrees(state.pos()).getDegrees(), state.vel(), state.acc());
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfile.java
+326
View File
@@ -0,0 +1,326 @@
package org.usfirst.frc4388.utility.motion;
import static org.usfirst.frc4388.utility.Util.epsilonEquals;
import static org.usfirst.frc4388.utility.motion.MotionUtil.kEpsilon;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;
import java.util.Optional;
/**
* A motion profile specifies a 1D time-parameterized trajectory. The trajectory is composed of successively coincident
* MotionSegments from which the desired state of motion at any given distance or time can be calculated.
*/
public class MotionProfile {
protected List<MotionSegment> mSegments;
/**
* Create an empty MotionProfile.
*/
public MotionProfile() {
mSegments = new ArrayList<>();
}
/**
* Create a MotionProfile from an existing list of segments (note that validity is not checked).
*
* @param segments
* The new segments of the profile.
*/
public MotionProfile(List<MotionSegment> segments) {
mSegments = segments;
}
/**
* Checks if the given MotionProfile is valid. This checks that:
*
* 1. All segments are valid.
*
* 2. Successive segments are C1 continuous in position and C0 continuous in velocity.
*
* @return True if the MotionProfile is valid.
*/
public boolean isValid() {
MotionSegment prev_segment = null;
for (MotionSegment s : mSegments) {
if (!s.isValid()) {
return false;
}
if (prev_segment != null && !s.start().coincident(prev_segment.end())) {
// Adjacent segments are not continuous.
System.err.println("Segments not continuous! End: " + prev_segment.end() + ", Start: " + s.start());
return false;
}
prev_segment = s;
}
return true;
}
/**
* Check if the profile is empty.
*
* @return True if there are no segments.
*/
public boolean isEmpty() {
return mSegments.isEmpty();
}
/**
* Get the interpolated MotionState at any given time.
*
* @param t
* The time to query.
* @return Empty if the time is outside the time bounds of the profile, or the resulting MotionState otherwise.
*/
public Optional<MotionState> stateByTime(double t) {
if (t < startTime() && t + kEpsilon >= startTime()) {
return Optional.of(startState());
}
if (t > endTime() && t - kEpsilon <= endTime()) {
return Optional.of(endState());
}
for (MotionSegment s : mSegments) {
if (s.containsTime(t)) {
return Optional.of(s.start().extrapolate(t));
}
}
return Optional.empty();
}
/**
* Get the interpolated MotionState at any given time, clamping to the endpoints if time is out of bounds.
*
* @param t
* The time to query.
* @return The MotionState at time t, or closest to it if t is outside the profile.
*/
public MotionState stateByTimeClamped(double t) {
if (t < startTime()) {
return startState();
} else if (t > endTime()) {
return endState();
}
for (MotionSegment s : mSegments) {
if (s.containsTime(t)) {
return s.start().extrapolate(t);
}
}
// Should never get here.
return MotionState.kInvalidState;
}
/**
* Get the interpolated MotionState by distance (the "pos()" field of MotionState). Note that since a profile may
* reverse, this method only returns the *first* instance of this position.
*
* @param pos
* The position to query.
* @return Empty if the profile never crosses pos or if the profile is invalid, or the resulting MotionState
* otherwise.
*/
public Optional<MotionState> firstStateByPos(double pos) {
for (MotionSegment s : mSegments) {
if (s.containsPos(pos)) {
if (epsilonEquals(s.end().pos(), pos, kEpsilon)) {
return Optional.of(s.end());
}
final double t = Math.min(s.start().nextTimeAtPos(pos), s.end().t());
if (Double.isNaN(t)) {
System.err.println("Error! We should reach 'pos' but we don't");
return Optional.empty();
}
return Optional.of(s.start().extrapolate(t));
}
}
// We never reach pos.
return Optional.empty();
}
/**
* Remove all parts of the profile prior to the query time. This eliminates whole segments and also shortens any
* segments containing t.
*
* @param t
* The query time.
*/
public void trimBeforeTime(double t) {
for (Iterator<MotionSegment> iterator = mSegments.iterator(); iterator.hasNext();) {
MotionSegment s = iterator.next();
if (s.end().t() <= t) {
// Segment is fully before t.
iterator.remove();
continue;
}
if (s.start().t() <= t) {
// Segment begins before t; let's shorten the segment.
s.setStart(s.start().extrapolate(t));
}
break;
}
}
/**
* Remove all segments.
*/
public void clear() {
mSegments.clear();
}
/**
* Remove all segments and initialize to the desired state (actually a segment of length 0 that starts and ends at
* initial_state).
*
* @param initial_state
* The MotionState to initialize to.
*/
public void reset(MotionState initial_state) {
clear();
mSegments.add(new MotionSegment(initial_state, initial_state));
}
/**
* Remove redundant segments (segments whose start and end states are coincident).
*/
public void consolidate() {
for (Iterator<MotionSegment> iterator = mSegments.iterator(); iterator.hasNext() && mSegments.size() > 1;) {
MotionSegment s = iterator.next();
if (s.start().coincident(s.end())) {
iterator.remove();
}
}
}
/**
* Add to the profile by applying an acceleration control for a given time. This is appended to the previous last
* state.
*
* @param acc
* The acceleration to apply.
* @param dt
* The period of time to apply the given acceleration.
*/
public void appendControl(double acc, double dt) {
if (isEmpty()) {
System.err.println("Error! Trying to append to empty profile");
return;
}
MotionState last_end_state = mSegments.get(mSegments.size() - 1).end();
MotionState new_start_state = new MotionState(last_end_state.t(), last_end_state.pos(), last_end_state.vel(),
acc);
appendSegment(new MotionSegment(new_start_state, new_start_state.extrapolate(new_start_state.t() + dt)));
}
/**
* Add to the profile by inserting a new segment. No validity checking is done.
*
* @param segment
* The segment to add.
*/
public void appendSegment(MotionSegment segment) {
mSegments.add(segment);
}
/**
* Add to the profile by inserting a new profile after the final state. No validity checking is done.
*
* @param profile
* The profile to add.
*/
public void appendProfile(MotionProfile profile) {
for (MotionSegment s : profile.segments()) {
appendSegment(s);
}
}
/**
* @return The number of segments.
*/
public int size() {
return mSegments.size();
}
/**
* @return The list of segments.
*/
public List<MotionSegment> segments() {
return mSegments;
}
/**
* @return The first state in the profile (or kInvalidState if empty).
*/
public MotionState startState() {
if (isEmpty()) {
return MotionState.kInvalidState;
}
return mSegments.get(0).start();
}
/**
* @return The time of the first state in the profile (or NaN if empty).
*/
public double startTime() {
return startState().t();
}
/**
* @return The pos of the first state in the profile (or NaN if empty).
*/
public double startPos() {
return startState().pos();
}
/**
* @return The last state in the profile (or kInvalidState if empty).
*/
public MotionState endState() {
if (isEmpty()) {
return MotionState.kInvalidState;
}
return mSegments.get(mSegments.size() - 1).end();
}
/**
* @return The time of the last state in the profile (or NaN if empty).
*/
public double endTime() {
return endState().t();
}
/**
* @return The pos of the last state in the profile (or NaN if empty).
*/
public double endPos() {
return endState().pos();
}
/**
* @return The duration of the entire profile.
*/
public double duration() {
return endTime() - startTime();
}
/**
* @return The total distance covered by the profile. Note that distance is the sum of absolute distances of all
* segments, so a reversing profile will count the distance covered in each direction.
*/
public double length() {
double length = 0.0;
for (MotionSegment s : segments()) {
length += Math.abs(s.end().pos() - s.start().pos());
}
return length;
}
@Override
public String toString() {
StringBuilder builder = new StringBuilder("Profile:");
for (MotionSegment s : segments()) {
builder.append("\n\t");
builder.append(s);
}
return builder.toString();
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileConstraints.java
+37
View File
@@ -0,0 +1,37 @@
package org.usfirst.frc4388.utility.motion;
/**
* Constraints for constructing a MotionProfile.
*/
public class MotionProfileConstraints {
protected double max_abs_vel = Double.POSITIVE_INFINITY;
protected double max_abs_acc = Double.POSITIVE_INFINITY;
public MotionProfileConstraints(double max_vel, double max_acc) {
this.max_abs_vel = Math.abs(max_vel);
this.max_abs_acc = Math.abs(max_acc);
}
/**
* @return The (positive) maximum allowed velocity.
*/
public double max_abs_vel() {
return max_abs_vel;
}
/**
* @return The (positive) maximum allowed acceleration.
*/
public double max_abs_acc() {
return max_abs_acc;
}
@Override
public boolean equals(Object obj) {
if (!(obj instanceof MotionProfileConstraints)) {
return false;
}
final MotionProfileConstraints other = (MotionProfileConstraints) obj;
return (other.max_abs_acc() == max_abs_acc()) && (other.max_abs_vel() == max_abs_vel());
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGenerator.java
+133
View File
@@ -0,0 +1,133 @@
package org.usfirst.frc4388.utility.motion;
import org.usfirst.frc4388.utility.motion.MotionProfileGoal.CompletionBehavior;
/**
* A MotionProfileGenerator generates minimum-time MotionProfiles to travel from a given MotionState to a given
* MotionProfileGoal while obeying a set of MotionProfileConstraints.
*/
public class MotionProfileGenerator {
// Static class.
private MotionProfileGenerator() {
}
protected static MotionProfile generateFlippedProfile(MotionProfileConstraints constraints,
MotionProfileGoal goal_state, MotionState prev_state) {
MotionProfile profile = generateProfile(constraints, goal_state.flipped(), prev_state.flipped());
for (MotionSegment s : profile.segments()) {
s.setStart(s.start().flipped());
s.setEnd(s.end().flipped());
}
return profile;
}
/**
* Generate a motion profile.
*
* @param constraints
* The constraints to use.
* @param goal_state
* The goal to use.
* @param prev_state
* The initial state to use.
* @return A motion profile from prev_state to goal_state that satisfies constraints.
*/
public synchronized static MotionProfile generateProfile(MotionProfileConstraints constraints,
MotionProfileGoal goal_state,
MotionState prev_state) {
double delta_pos = goal_state.pos() - prev_state.pos();
if (delta_pos < 0.0 || (delta_pos == 0.0 && prev_state.vel() < 0.0)) {
// For simplicity, we always assume the goal requires positive movement. If negative, we flip to solve, then
// flip the solution.
return generateFlippedProfile(constraints, goal_state, prev_state);
}
// Invariant from this point on: delta_pos >= 0.0
// Clamp the start state to be valid.
MotionState start_state = new MotionState(prev_state.t(), prev_state.pos(),
Math.signum(prev_state.vel()) * Math.min(Math.abs(prev_state.vel()), constraints.max_abs_vel()),
Math.signum(prev_state.acc()) * Math.min(Math.abs(prev_state.acc()), constraints.max_abs_acc()));
MotionProfile profile = new MotionProfile();
profile.reset(start_state);
// If our velocity is headed away from the goal, the first thing we need to do is to stop.
if (start_state.vel() < 0.0 && delta_pos > 0.0) {
final double stopping_time = Math.abs(start_state.vel() / constraints.max_abs_acc());
profile.appendControl(constraints.max_abs_acc(), stopping_time);
start_state = profile.endState();
delta_pos = goal_state.pos() - start_state.pos();
}
// Invariant from this point on: start_state.vel() >= 0.0
final double min_abs_vel_at_goal_sqr = start_state.vel2() - 2.0 * constraints.max_abs_acc() * delta_pos;
final double min_abs_vel_at_goal = Math.sqrt(Math.abs(min_abs_vel_at_goal_sqr));
final double max_abs_vel_at_goal = Math.sqrt(start_state.vel2() + 2.0 * constraints.max_abs_acc() * delta_pos);
double goal_vel = goal_state.max_abs_vel();
double max_acc = constraints.max_abs_acc();
if (min_abs_vel_at_goal_sqr > 0.0
&& min_abs_vel_at_goal > (goal_state.max_abs_vel() + goal_state.vel_tolerance())) {
// Overshoot is unavoidable with the current constraints. Look at completion_behavior to see what we should
// do.
if (goal_state.completion_behavior() == CompletionBehavior.VIOLATE_MAX_ABS_VEL) {
// Adjust the goal velocity.
goal_vel = min_abs_vel_at_goal;
} else if (goal_state.completion_behavior() == CompletionBehavior.VIOLATE_MAX_ACCEL) {
if (Math.abs(delta_pos) < goal_state.pos_tolerance()) {
// Special case: We are at the goal but moving too fast. This requires 'infinite' acceleration,
// which will result in NaNs below, so we can return the profile immediately.
profile.appendSegment(new MotionSegment(
new MotionState(profile.endTime(), profile.endPos(), profile.endState().vel(),
Double.NEGATIVE_INFINITY),
new MotionState(profile.endTime(), profile.endPos(), goal_vel, Double.NEGATIVE_INFINITY)));
profile.consolidate();
return profile;
}
// Adjust the max acceleration.
max_acc = Math.abs(goal_vel * goal_vel - start_state.vel2()) / (2.0 * delta_pos);
} else {
// We are going to overshoot the goal, so the first thing we need to do is come to a stop.
final double stopping_time = Math.abs(start_state.vel() / constraints.max_abs_acc());
profile.appendControl(-constraints.max_abs_acc(), stopping_time);
// Now we need to travel backwards, so generate a flipped profile.
profile.appendProfile(generateFlippedProfile(constraints, goal_state, profile.endState()));
profile.consolidate();
return profile;
}
}
goal_vel = Math.min(goal_vel, max_abs_vel_at_goal);
// Invariant from this point forward: We can achieve goal_vel at goal_state.pos exactly using no more than +/-
// max_acc.
// What is the maximum velocity we can reach (Vmax)? This is the intersection of two curves: one accelerating
// towards the goal from profile.finalState(), the other coming from the goal at max vel (in reverse). If Vmax
// is greater than constraints.max_abs_vel, we will clamp and cruise.
// Solve the following three equations to find Vmax (by substitution):
// Vmax^2 = Vstart^2 + 2*a*d_accel
// Vgoal^2 = Vmax^2 - 2*a*d_decel
// delta_pos = d_accel + d_decel
final double v_max = Math.min(constraints.max_abs_vel(),
Math.sqrt((start_state.vel2() + goal_vel * goal_vel) / 2.0 + delta_pos * max_acc));
// Accelerate to v_max
if (v_max > start_state.vel()) {
final double accel_time = (v_max - start_state.vel()) / max_acc;
profile.appendControl(max_acc, accel_time);
start_state = profile.endState();
}
// Figure out how much distance will be covered during deceleration.
final double distance_decel = Math.max(0.0,
(start_state.vel2() - goal_vel * goal_vel) / (2.0 * constraints.max_abs_acc()));
final double distance_cruise = Math.max(0.0, goal_state.pos() - start_state.pos() - distance_decel);
// Cruise at constant velocity.
if (distance_cruise > 0.0) {
final double cruise_time = distance_cruise / start_state.vel();
profile.appendControl(0.0, cruise_time);
start_state = profile.endState();
}
// Decelerate to goal velocity.
if (distance_decel > 0.0) {
final double decel_time = (start_state.vel() - goal_vel) / max_acc;
profile.appendControl(-max_acc, decel_time);
}
profile.consolidate();
return profile;
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionProfileGoal.java
+143
View File
@@ -0,0 +1,143 @@
package org.usfirst.frc4388.utility.motion;
import static org.usfirst.frc4388.utility.Util.epsilonEquals;
/**
* A MotionProfileGoal defines a desired position and maximum velocity (at this position), along with the behavior that
* should be used to determine if we are at the goal and what to do if it is infeasible to reach the goal within the
* desired velocity bounds.
*
*/
public class MotionProfileGoal {
/**
* A goal consists of a desired position and specified maximum velocity magnitude. But what should we do if we would
* reach the goal at a velocity greater than the maximum? This enum allows a user to specify a preference on
* behavior in this case.
*
* Example use-cases of each:
*
* OVERSHOOT - Generally used with a goal max_abs_vel of 0.0 to stop at the desired pos without violating any
* constraints.
*
* VIOLATE_MAX_ACCEL - If we absolutely do not want to pass the goal and are unwilling to violate the max_abs_vel
* (for example, there is an obstacle in front of us - slam the brakes harder than we'd like in order to avoid
* hitting it).
*
* VIOLATE_MAX_ABS_VEL - If the max velocity is just a general guideline and not a hard performance limit, it's
* better to slightly exceed it to avoid skidding wheels.
*/
public static enum CompletionBehavior {
// Overshoot the goal if necessary (at a velocity greater than max_abs_vel) and come back.
// Only valid if the goal velocity is 0.0 (otherwise VIOLATE_MAX_ACCEL will be used).
OVERSHOOT,
// If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the max accel
// constraint.
VIOLATE_MAX_ACCEL,
// If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the goal velocity.
VIOLATE_MAX_ABS_VEL
}
protected double pos;
protected double max_abs_vel;
protected CompletionBehavior completion_behavior = CompletionBehavior.OVERSHOOT;
protected double pos_tolerance = 1E-3;
protected double vel_tolerance = 1E-2;
public MotionProfileGoal() {
}
public MotionProfileGoal(double pos) {
this.pos = pos;
this.max_abs_vel = 0.0;
sanityCheck();
}
public MotionProfileGoal(double pos, double max_abs_vel) {
this.pos = pos;
this.max_abs_vel = max_abs_vel;
sanityCheck();
}
public MotionProfileGoal(double pos, double max_abs_vel, CompletionBehavior completion_behavior) {
this.pos = pos;
this.max_abs_vel = max_abs_vel;
this.completion_behavior = completion_behavior;
sanityCheck();
}
public MotionProfileGoal(double pos, double max_abs_vel, CompletionBehavior completion_behavior,
double pos_tolerance, double vel_tolerance) {
this.pos = pos;
this.max_abs_vel = max_abs_vel;
this.completion_behavior = completion_behavior;
this.pos_tolerance = pos_tolerance;
this.vel_tolerance = vel_tolerance;
sanityCheck();
}
public MotionProfileGoal(MotionProfileGoal other) {
this(other.pos, other.max_abs_vel, other.completion_behavior, other.pos_tolerance, other.vel_tolerance);
}
/**
* @return A flipped MotionProfileGoal (where the position is negated, but all other attributes remain the same).
*/
public MotionProfileGoal flipped() {
return new MotionProfileGoal(-pos, max_abs_vel, completion_behavior, pos_tolerance, vel_tolerance);
}
public double pos() {
return pos;
}
public double max_abs_vel() {
return max_abs_vel;
}
public double pos_tolerance() {
return pos_tolerance;
}
public double vel_tolerance() {
return vel_tolerance;
}
public CompletionBehavior completion_behavior() {
return completion_behavior;
}
public boolean atGoalState(MotionState state) {
return atGoalPos(state.pos()) && (Math.abs(state.vel()) < (max_abs_vel + vel_tolerance)
|| completion_behavior == CompletionBehavior.VIOLATE_MAX_ABS_VEL);
}
public boolean atGoalPos(double pos) {
return epsilonEquals(pos, this.pos, pos_tolerance);
}
/**
* This method makes sure that the completion behavior is compatible with the max goal velocity.
*/
protected void sanityCheck() {
if (max_abs_vel > vel_tolerance && completion_behavior == CompletionBehavior.OVERSHOOT) {
completion_behavior = CompletionBehavior.VIOLATE_MAX_ACCEL;
}
}
@Override
public String toString() {
return "pos: " + pos + " (+/- " + pos_tolerance + "), max_abs_vel: " + max_abs_vel + " (+/- " + vel_tolerance
+ "), completion behavior: " + completion_behavior.name();
}
@Override
public boolean equals(Object obj) {
if (!(obj instanceof MotionProfileGoal)) {
return false;
}
final MotionProfileGoal other = (MotionProfileGoal) obj;
return (other.completion_behavior() == completion_behavior()) && (other.pos() == pos())
&& (other.max_abs_vel() == max_abs_vel()) && (other.pos_tolerance() == pos_tolerance())
&& (other.vel_tolerance() == vel_tolerance());
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionSegment.java
+80
View File
@@ -0,0 +1,80 @@
package org.usfirst.frc4388.utility.motion;
import static org.usfirst.frc4388.utility.Util.epsilonEquals;
import static org.usfirst.frc4388.utility.motion.MotionUtil.kEpsilon;
/**
* A MotionSegment is a movement from a start MotionState to an end MotionState with a constant acceleration.
*/
public class MotionSegment {
protected MotionState mStart;
protected MotionState mEnd;
public MotionSegment(MotionState start, MotionState end) {
mStart = start;
mEnd = end;
}
/**
* Verifies that:
*
* 1. All segments have a constant acceleration.
*
* 2. All segments have monotonic position (sign of velocity doesn't change).
*
* 3. The time, position, velocity, and acceleration of the profile are consistent.
*/
public boolean isValid() {
if (!epsilonEquals(start().acc(), end().acc(), kEpsilon)) {
// Acceleration is not constant within the segment.
System.err.println(
"Segment acceleration not constant! Start acc: " + start().acc() + ", End acc: " + end().acc());
return false;
}
if (Math.signum(start().vel()) * Math.signum(end().vel()) < 0.0 && !epsilonEquals(start().vel(), 0.0, kEpsilon)
&& !epsilonEquals(end().vel(), 0.0, kEpsilon)) {
// Velocity direction reverses within the segment.
System.err.println("Segment velocity reverses! Start vel: " + start().vel() + ", End vel: " + end().vel());
return false;
}
if (!start().extrapolate(end().t()).equals(end())) {
// A single segment is not consistent.
if (start().t() == end().t() && Double.isInfinite(start().acc())) {
// One allowed exception: If acc is infinite and dt is zero.
return true;
}
System.err.println("Segment not consistent! Start: " + start() + ", End: " + end());
return false;
}
return true;
}
public boolean containsTime(double t) {
return t >= start().t() && t <= end().t();
}
public boolean containsPos(double pos) {
return pos >= start().pos() && pos <= end().pos() || pos <= start().pos() && pos >= end().pos();
}
public MotionState start() {
return mStart;
}
public void setStart(MotionState start) {
mStart = start;
}
public MotionState end() {
return mEnd;
}
public void setEnd(MotionState end) {
mEnd = end;
}
@Override
public String toString() {
return "Start: " + start() + ", End: " + end();
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionState.java
+166
View File
@@ -0,0 +1,166 @@
package org.usfirst.frc4388.utility.motion;
import static org.usfirst.frc4388.utility.Util.epsilonEquals;
import static org.usfirst.frc4388.utility.motion.MotionUtil.kEpsilon;
/**
* A MotionState is a completely specified state of 1D motion through time.
*/
public class MotionState {
protected final double t;
protected final double pos;
protected final double vel;
protected final double acc;
public static MotionState kInvalidState = new MotionState(Double.NaN, Double.NaN, Double.NaN, Double.NaN);
public MotionState(double t, double pos, double vel, double acc) {
this.t = t;
this.pos = pos;
this.vel = vel;
this.acc = acc;
}
public MotionState(MotionState state) {
this(state.t, state.pos, state.vel, state.acc);
}
public double t() {
return t;
}
public double pos() {
return pos;
}
public double vel() {
return vel;
}
public double vel2() {
return vel * vel;
}
public double acc() {
return acc;
}
/**
* Extrapolates this MotionState to the specified time by applying this MotionState's acceleration.
*
* @param t
* The time of the new MotionState.
* @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this state.
*/
public MotionState extrapolate(double t) {
return extrapolate(t, acc);
}
/**
*
* Extrapolates this MotionState to the specified time by applying a given acceleration to the (t, pos, vel) portion
* of this MotionState.
*
* @param t
* The time of the new MotionState.
* @param acc
* The acceleration to apply.
* @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this state (with the
* specified accel).
*/
public MotionState extrapolate(double t, double acc) {
final double dt = t - this.t;
return new MotionState(t, pos + vel * dt + .5 * acc * dt * dt, vel + acc * dt, acc);
}
/**
* Find the next time (first time > MotionState.t()) that this MotionState will be at pos. This is an inverse of the
* extrapolate() method.
*
* @param pos
* The position to query.
* @return The time when we are next at pos() if we are extrapolating with a positive dt. NaN if we never reach pos.
*/
public double nextTimeAtPos(double pos) {
if (epsilonEquals(pos, this.pos, kEpsilon)) {
// Already at pos.
return t;
}
if (epsilonEquals(acc, 0.0, kEpsilon)) {
// Zero acceleration case.
final double delta_pos = pos - this.pos;
if (!epsilonEquals(vel, 0.0, kEpsilon) && Math.signum(delta_pos) == Math.signum(vel)) {
// Constant velocity heading towards pos.
return delta_pos / vel + t;
}
return Double.NaN;
}
// Solve the quadratic formula.
// ax^2 + bx + c == 0
// x = dt
// a = .5 * acc
// b = vel
// c = this.pos - pos
final double disc = vel * vel - 2.0 * acc * (this.pos - pos);
if (disc < 0.0) {
// Extrapolating this MotionState never reaches the desired pos.
return Double.NaN;
}
final double sqrt_disc = Math.sqrt(disc);
final double max_dt = (-vel + sqrt_disc) / acc;
final double min_dt = (-vel - sqrt_disc) / acc;
if (min_dt >= 0.0 && (max_dt < 0.0 || min_dt < max_dt)) {
return t + min_dt;
}
if (max_dt >= 0.0) {
return t + max_dt;
}
// We only reach the desired pos in the past.
return Double.NaN;
}
@Override
public String toString() {
return "(t=" + t + ", pos=" + pos + ", vel=" + vel + ", acc=" + acc + ")";
}
/**
* Checks if two MotionStates are epsilon-equals (all fields are equal within a nominal tolerance).
*/
@Override
public boolean equals(Object other) {
return (other instanceof MotionState) && equals((MotionState) other, kEpsilon);
}
/**
* Checks if two MotionStates are epsilon-equals (all fields are equal within a specified tolerance).
*/
public boolean equals(MotionState other, double epsilon) {
return coincident(other, epsilon) && epsilonEquals(acc, other.acc, epsilon);
}
/**
* Checks if two MotionStates are coincident (t, pos, and vel are equal within a nominal tolerance, but acceleration
* may be different).
*/
public boolean coincident(MotionState other) {
return coincident(other, kEpsilon);
}
/**
* Checks if two MotionStates are coincident (t, pos, and vel are equal within a specified tolerance, but
* acceleration may be different).
*/
public boolean coincident(MotionState other, double epsilon) {
return epsilonEquals(t, other.t, epsilon) && epsilonEquals(pos, other.pos, epsilon)
&& epsilonEquals(vel, other.vel, epsilon);
}
/**
* Returns a MotionState that is the mirror image of this one. Pos, vel, and acc are all negated, but time is not.
*/
public MotionState flipped() {
return new MotionState(t, -pos, -vel, -acc);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/MotionUtil.java
+8
View File
@@ -0,0 +1,8 @@
package org.usfirst.frc4388.utility.motion;
public class MotionUtil {
/**
* A constant for consistent floating-point equality checking within this library.
*/
public static double kEpsilon = 1e-6;
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/ProfileFollower.java
+203
View File
@@ -0,0 +1,203 @@
package org.usfirst.frc4388.utility.motion;
import org.usfirst.frc4388.utility.motion.MotionProfileGoal.CompletionBehavior;
/**
* A controller for tracking a profile generated to attain a MotionProfileGoal. The controller uses feedforward on
* acceleration, velocity, and position; proportional feedback on velocity and position; and integral feedback on
* position.
*/
public class ProfileFollower {
protected double mKp;
protected double mKi;
protected double mKv;
protected double mKffv;
protected double mKffa;
protected double mMinOutput = Double.NEGATIVE_INFINITY;
protected double mMaxOutput = Double.POSITIVE_INFINITY;
protected MotionState mLatestActualState;
protected MotionState mInitialState;
protected double mLatestPosError;
protected double mLatestVelError;
protected double mTotalError;
protected MotionProfileGoal mGoal = null;
protected MotionProfileConstraints mConstraints = null;
protected SetpointGenerator mSetpointGenerator = new SetpointGenerator();
protected SetpointGenerator.Setpoint mLatestSetpoint = null;
/**
* Create a new ProfileFollower.
*
* @param kp
* The proportional gain on position error.
* @param ki
* The integral gain on position error.
* @param kv
* The proportional gain on velocity error (or derivative gain on position error).
* @param kffv
* The feedforward gain on velocity. Should be 1.0 if the units of the profile match the units of the
* output.
* @param kffa
* The feedforward gain on acceleration.
*/
public ProfileFollower(double kp, double ki, double kv, double kffv, double kffa) {
resetProfile();
setGains(kp, ki, kv, kffv, kffa);
}
public void setGains(double kp, double ki, double kv, double kffv, double kffa) {
mKp = kp;
mKi = ki;
mKv = kv;
mKffv = kffv;
mKffa = kffa;
}
/**
* Completely clear all state related to the current profile (min and max outputs are maintained).
*/
public void resetProfile() {
mTotalError = 0.0;
mInitialState = MotionState.kInvalidState;
mLatestActualState = MotionState.kInvalidState;
mLatestPosError = Double.NaN;
mLatestVelError = Double.NaN;
mSetpointGenerator.reset();
mGoal = null;
mConstraints = null;
resetSetpoint();
}
/**
* Specify a goal and constraints for achieving the goal.
*/
public void setGoalAndConstraints(MotionProfileGoal goal, MotionProfileConstraints constraints) {
if (mGoal != null && !mGoal.equals(goal) && mLatestSetpoint != null) {
// Clear the final state bit since the goal has changed.
mLatestSetpoint.final_setpoint = false;
}
mGoal = goal;
mConstraints = constraints;
}
public void setGoal(MotionProfileGoal goal) {
setGoalAndConstraints(goal, mConstraints);
}
/**
* @return The current goal (null if no goal has been set since the latest call to reset()).
*/
public MotionProfileGoal getGoal() {
return mGoal;
}
public void setConstraints(MotionProfileConstraints constraints) {
setGoalAndConstraints(mGoal, constraints);
}
public MotionState getSetpoint() {
return (mLatestSetpoint == null ? MotionState.kInvalidState : mLatestSetpoint.motion_state);
}
/**
* Reset just the setpoint. This means that the latest_state provided to update() will be used rather than feeding
* forward the previous setpoint the next time update() is called. This almost always forces a MotionProfile update,
* and may be warranted if tracking error gets very large.
*/
public void resetSetpoint() {
mLatestSetpoint = null;
}
public void resetIntegral() {
mTotalError = 0.0;
}
/**
* Update the setpoint and apply the control gains to generate a control output.
*
* @param latest_state
* The latest *actual* state, used only for feedback purposes (unless this is the first iteration or
* reset()/resetSetpoint() was just called, in which case this is the new start state for the profile).
* @param t
* The timestamp for which the setpoint is desired.
* @return An output that reflects the control output to apply to achieve the new setpoint.
*/
public synchronized double update(MotionState latest_state, double t) {
mLatestActualState = latest_state;
MotionState prev_state = latest_state;
if (mLatestSetpoint != null) {
prev_state = mLatestSetpoint.motion_state;
} else {
mInitialState = prev_state;
}
final double dt = Math.max(0.0, t - prev_state.t());
mLatestSetpoint = mSetpointGenerator.getSetpoint(mConstraints, mGoal, prev_state, t);
// Update error.
mLatestPosError = mLatestSetpoint.motion_state.pos() - latest_state.pos();
mLatestVelError = mLatestSetpoint.motion_state.vel() - latest_state.vel();
// Calculate the feedforward and proportional terms.
double output = mKp * mLatestPosError + mKv * mLatestVelError + mKffv * mLatestSetpoint.motion_state.vel()
+ (Double.isNaN(mLatestSetpoint.motion_state.acc()) ? 0.0 : mKffa * mLatestSetpoint.motion_state.acc());
if (output >= mMinOutput && output <= mMaxOutput) {
// Update integral.
mTotalError += mLatestPosError * dt;
output += mKi * mTotalError;
} else {
// Reset integral windup.
mTotalError = 0.0;
}
// Clamp to limits.
output = Math.max(mMinOutput, Math.min(mMaxOutput, output));
return output;
}
public void setMinOutput(double min_output) {
mMinOutput = min_output;
}
public void setMaxOutput(double max_output) {
mMaxOutput = max_output;
}
public double getPosError() {
return mLatestPosError;
}
public double getVelError() {
return mLatestVelError;
}
/**
* We are finished the profile when the final setpoint has been generated. Note that this does not check whether we
* are anywhere close to the final setpoint, however.
*
* @return True if the final setpoint has been generated for the current goal.
*/
public boolean isFinishedProfile() {
return mGoal != null && mLatestSetpoint != null && mLatestSetpoint.final_setpoint;
}
/**
* We are on target if our actual state achieves the goal (where the definition of achievement depends on the goal's
* completion behavior).
*
* @return True if we have actually achieved the current goal.
*/
public boolean onTarget() {
if (mGoal == null || mLatestSetpoint == null) {
return false;
}
// For the options that don't achieve the goal velocity exactly, also count any instance where we have passed
// the finish line.
final double goal_to_start = mGoal.pos() - mInitialState.pos();
final double goal_to_actual = mGoal.pos() - mLatestActualState.pos();
final boolean passed_goal_state = Math.signum(goal_to_start) * Math.signum(goal_to_actual) < 0.0;
return mGoal.atGoalState(mLatestActualState)
|| (mGoal.completion_behavior() != CompletionBehavior.OVERSHOOT && passed_goal_state);
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/motion/SetpointGenerator.java
+114
View File
@@ -0,0 +1,114 @@
package org.usfirst.frc4388.utility.motion;
import java.util.Optional;
/**
* A SetpointGenerate does just-in-time motion profile generation to supply a stream of setpoints that obey the given
* constraints to a controller. The profile is regenerated when any of the inputs change, but is cached (and trimmed as
* we go) if the only update is to the current state.
*
* Note that typically for smooth control, a user will feed the last iteration's setpoint as the argument to
* getSetpoint(), and should only use a measured state directly on the first iteration or if a large disturbance is
* detected.
*/
public class SetpointGenerator {
/**
* A Setpoint is just a MotionState and an additional flag indicating whether this is setpoint achieves the goal
* (useful for higher-level logic to know that it is now time to do something else).
*/
public static class Setpoint {
public MotionState motion_state;
public boolean final_setpoint;
public Setpoint(MotionState motion_state, boolean final_setpoint) {
this.motion_state = motion_state;
this.final_setpoint = final_setpoint;
}
}
protected MotionProfile mProfile = null;
protected MotionProfileGoal mGoal = null;
protected MotionProfileConstraints mConstraints = null;
public SetpointGenerator() {
}
/**
* Force a reset of the profile.
*/
public void reset() {
mProfile = null;
mGoal = null;
mConstraints = null;
}
/**
* Get a new Setpoint (and generate a new MotionProfile if necessary).
*
* @param constraints
* The constraints to use.
* @param goal
* The goal to use.
* @param prev_state
* The previous setpoint (or measured state of the system to do a reset).
* @param t
* The time to generate a setpoint for.
* @return The new Setpoint at time t.
*/
public synchronized Setpoint getSetpoint(MotionProfileConstraints constraints, MotionProfileGoal goal,
MotionState prev_state,
double t) {
boolean regenerate = mConstraints == null || !mConstraints.equals(constraints) || mGoal == null
|| !mGoal.equals(goal) || mProfile == null;
if (!regenerate && !mProfile.isEmpty()) {
Optional<MotionState> expected_state = mProfile.stateByTime(prev_state.t());
regenerate = !expected_state.isPresent() || !expected_state.get().equals(prev_state);
}
if (regenerate) {
// Regenerate the profile, as our current profile does not satisfy the inputs.
mConstraints = constraints;
mGoal = goal;
mProfile = MotionProfileGenerator.generateProfile(constraints, goal, prev_state);
// System.out.println("Regenerating profile: " + mProfile);
}
// Sample the profile at time t.
Setpoint rv = null;
if (!mProfile.isEmpty() && mProfile.isValid()) {
MotionState setpoint;
if (t > mProfile.endTime()) {
setpoint = mProfile.endState();
} else if (t < mProfile.startTime()) {
setpoint = mProfile.startState();
} else {
setpoint = mProfile.stateByTime(t).get();
}
// Shorten the profile and return the new setpoint.
mProfile.trimBeforeTime(t);
rv = new Setpoint(setpoint, mProfile.isEmpty() || mGoal.atGoalState(setpoint));
}
// Invalid or empty profile - just output the same state again.
if (rv == null) {
rv = new Setpoint(prev_state, true);
}
if (rv.final_setpoint) {
// Ensure the final setpoint matches the goal exactly.
rv.motion_state = new MotionState(rv.motion_state.t(), mGoal.pos(),
Math.signum(rv.motion_state.vel()) * Math.max(mGoal.max_abs_vel(), Math.abs(rv.motion_state.vel())),
0.0);
}
return rv;
}
/**
* Get the full profile from the latest call to getSetpoint(). Useful to check estimated time or distance to goal.
*
* @return The profile from the latest call to getSetpoint(), or null if there is not yet a profile.
*/
public MotionProfile getProfile() {
return mProfile;
}
}