mirror of
https://github.com/Team4388/2019-Hit-or-Miss.git
synced 2026-06-09 00:28:05 -06:00
Update drive tain
started convesion, stoped for testing, not ready to use
This commit is contained in:
lukesta182
@@ -1,4 +1,3 @@
|
||||
|
||||
package org.usfirst.frc4388.robot.subsystems;
|
||||
|
||||
import java.util.ArrayList;
|
||||
@@ -10,9 +9,11 @@ import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.RobotMap;
|
||||
import org.usfirst.frc4388.utility.control.AdaptivePurePursuitController;
|
||||
import org.usfirst.frc4388.utility.BHRMathUtils;
|
||||
//import org.usfirst.frc4388.utility.CANTalonEncoder;
|
||||
import org.usfirst.frc4388.utility.TalonSRXEncoder;
|
||||
import org.usfirst.frc4388.utility.TalonSRXFactory;
|
||||
import org.usfirst.frc4388.utility.ControlLoopable;
|
||||
import org.usfirst.frc4388.utility.control.Kinematics;
|
||||
import org.usfirst.frc4388.utility.MMTalonPIDController;
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController;
|
||||
import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
|
||||
import org.usfirst.frc4388.utility.MPTalonPIDController;
|
||||
@@ -22,13 +23,13 @@ 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.control.Path;
|
||||
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.math.Translation2d;
|
||||
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
@@ -37,15 +38,6 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
import com.revrobotics.CANEncoder;
|
||||
import com.revrobotics.CANPIDController;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkMax.IdleMode;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANEncoder;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
|
||||
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
||||
//import edu.wpi.first.wpilibj.DigitalInput;
|
||||
@@ -99,24 +91,14 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
public static final double MP_MAX_TURN_T2 = 200;
|
||||
|
||||
// Motor controllers
|
||||
private ArrayList<CANPIDController> motorControllers = new ArrayList<CANPIDController>();
|
||||
private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
|
||||
|
||||
private CANSparkMax leftDrive1;
|
||||
private CANSparkMax leftDrive2;
|
||||
private CANSparkMax rightDrive1;
|
||||
private CANSparkMax rightDrive2;
|
||||
private CANEncoder left_distance;
|
||||
private CANEncoder right_distance;
|
||||
private TalonSRXEncoder leftDrive1;
|
||||
|
||||
|
||||
|
||||
|
||||
private CANSparkMax m_motor;
|
||||
private CANEncoder m_encoder;
|
||||
private TalonSRXEncoder rightDrive1;
|
||||
|
||||
private DifferentialDrive m_drive;
|
||||
|
||||
//PID encoder and motor
|
||||
|
||||
//private DigitalInput hopperSensorRed;
|
||||
//private DigitalInput hopperSensorBlue;
|
||||
@@ -170,6 +152,9 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
private PIDParams mpStraightPIDParams = new PIDParams(.7, 0.0, 0.000000, .001, 0.001, .7); // 4 colsons
|
||||
// private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.03); // 4 omni
|
||||
private PIDParams mpHoldPIDParams = new PIDParams(1, 0, 0, 0.0, 0.0, 0.0);
|
||||
|
||||
private MMTalonPIDController mmStraightController;
|
||||
private PIDParams mmStraightPIDParams = new PIDParams(0, 0, 0, 0.24);
|
||||
|
||||
private MPSoftwarePIDController mpTurnController; // p i d a v g izone
|
||||
private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100); // 4 colson wheels
|
||||
@@ -203,96 +188,69 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
private AHRS gyroNavX;
|
||||
private boolean useGyroLock;
|
||||
private double gyroLockAngleDeg;
|
||||
private double kPGyro = 0.07;
|
||||
//private double kPGyro = 0.0625;
|
||||
|
||||
//private double kPGyro = 0.04;
|
||||
private double kPGyro = 0.0625;
|
||||
private boolean isCalibrating = false;
|
||||
private double gyroOffsetDeg = 0;
|
||||
|
||||
public Drive() {
|
||||
try {
|
||||
//System.err.println("Beginning of Drive.");
|
||||
|
||||
leftDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, MotorType.kBrushless);
|
||||
//leftDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder);
|
||||
leftDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID, MotorType.kBrushless);
|
||||
//encoderLeft = new CANEncoder(leftDrive1);
|
||||
|
||||
|
||||
rightDrive1 = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, MotorType.kBrushless);
|
||||
rightDrive2 = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless);
|
||||
|
||||
|
||||
//leftDrive1.restoreFactoryDefaults();
|
||||
leftDrive1 = TalonSRXFactory.createTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder);
|
||||
|
||||
//System.err.println("After Constructors.");
|
||||
|
||||
//gyroPigeon = new PigeonImu(leftDrive2);
|
||||
rightDrive1 = TalonSRXFactory.createTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder);
|
||||
|
||||
|
||||
|
||||
|
||||
gyroNavX = new AHRS(SPI.Port.kMXP);
|
||||
leftDrive2.follow(leftDrive1);
|
||||
rightDrive2.follow(rightDrive1);
|
||||
|
||||
//hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID);
|
||||
//hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID);
|
||||
|
||||
//leftDrive1.clearStickyFaults();
|
||||
//leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
|
||||
//leftDrive1.setNominalClosedLoopVoltage(12.0);
|
||||
leftDrive1.clearFaults();
|
||||
leftDrive1.setInverted(false);//false on practice, true on comp
|
||||
//leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot //not needed for spark
|
||||
//leftDrive1.setSafetyEnabled(false); //not needed for spark
|
||||
leftDrive1.clearStickyFaults(0);
|
||||
leftDrive1.setInverted(false);//false on comp 2108, false on microbot
|
||||
leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot
|
||||
leftDrive1.setSafetyEnabled(false);
|
||||
//leftDrive1.setCurrentLimit(15);
|
||||
//leftDrive1.enableCurrentLimit(true);
|
||||
leftDrive1.setIdleMode(IdleMode.kBrake);
|
||||
//leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); // not needed for spark?
|
||||
//leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); // not needed for spark?
|
||||
//leftDrive1.configNominalOutputForward(+1.0f, 0); // not needed for spark?
|
||||
//leftDrive1.configNominalOutputReverse(-1.0f, 0); // not needed for spark?
|
||||
leftDrive1.setNeutralMode(NeutralMode.Brake);
|
||||
leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
|
||||
leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
|
||||
leftDrive1.configNominalOutputForward(+1.0f, 0);
|
||||
leftDrive1.configNominalOutputReverse(-1.0f, 0);
|
||||
|
||||
|
||||
// if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
|
||||
// Driver.reportError("Could not detect left drive encoder encoder!", false);
|
||||
// }
|
||||
|
||||
|
||||
leftDrive2.setInverted(false);//false on practice. true on comp
|
||||
//leftDrive2.setSafetyEnabled(false);
|
||||
leftDrive2.setIdleMode(IdleMode.kBrake);
|
||||
//leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); // set above
|
||||
|
||||
|
||||
|
||||
//rightDrive1.clearStickyFaults();
|
||||
//rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE);
|
||||
//rightDrive1.setNominalClosedLoopVoltage(12.0);
|
||||
rightDrive1.clearFaults();
|
||||
rightDrive1.setInverted(false);//false on comp, false on practice
|
||||
//rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
|
||||
//rightDrive1.setSafetyEnabled(false);
|
||||
rightDrive1.setIdleMode(IdleMode.kBrake);
|
||||
//rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
|
||||
//rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
|
||||
//rightDrive1.configNominalOutputForward(+1.0f, 0);
|
||||
//rightDrive1.configNominalOutputReverse(-1.0f, 0);
|
||||
rightDrive1.clearStickyFaults(0);
|
||||
rightDrive1.setInverted(true);//true on comp 2108, false on microbot
|
||||
rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot
|
||||
rightDrive1.setSafetyEnabled(false);
|
||||
rightDrive1.setNeutralMode(NeutralMode.Brake);
|
||||
rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0);
|
||||
rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0);
|
||||
rightDrive1.configNominalOutputForward(+1.0f, 0);
|
||||
rightDrive1.configNominalOutputReverse(-1.0f, 0);
|
||||
// if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
|
||||
// DriverStation.reportError("Could not detect right drive encoder encoder!", false);
|
||||
// }
|
||||
|
||||
|
||||
rightDrive2.setInverted(false);//True on comp 2019, false on practice
|
||||
//rightDrive2.setSafetyEnabled(false);
|
||||
rightDrive2.setIdleMode(IdleMode.kBrake);
|
||||
//rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID());
|
||||
|
||||
//System.err.println("After motor settings.");
|
||||
|
||||
CANPIDController leftDrive1_Controller = new CANPIDController(leftDrive1);
|
||||
CANPIDController rightDrive1_Controller = new CANPIDController(rightDrive1);
|
||||
|
||||
motorControllers.add(leftDrive1_Controller);
|
||||
motorControllers.add(rightDrive1_Controller);
|
||||
|
||||
//System.err.println("After motorControllers.");
|
||||
|
||||
motorControllers.add(leftDrive1);
|
||||
motorControllers.add(rightDrive1);
|
||||
|
||||
//m_drive = new RobotDrive(leftDrive1, rightDrive1);
|
||||
//m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
|
||||
@@ -303,11 +261,8 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
//m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true); //TODO URGENT: verify
|
||||
m_drive.setSafetyEnabled(false);
|
||||
|
||||
|
||||
//speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID);
|
||||
//System.err.println("End of Drive.");
|
||||
}
|
||||
|
||||
catch (Exception e) {
|
||||
System.err.println("An error occurred in the DriveTrain constructor");
|
||||
}
|
||||
@@ -315,15 +270,15 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
|
||||
public void setToBrakeOnNeutral(boolean brakeVsCoast) {
|
||||
if (brakeVsCoast) {
|
||||
leftDrive1.setIdleMode(IdleMode.kBrake);
|
||||
leftDrive2.setIdleMode(IdleMode.kBrake);
|
||||
rightDrive1.setIdleMode(IdleMode.kBrake);
|
||||
rightDrive2.setIdleMode(IdleMode.kBrake);
|
||||
leftDrive1.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
rightDrive1.setNeutralMode(NeutralMode.Brake);
|
||||
|
||||
} else {
|
||||
leftDrive1.setIdleMode(IdleMode.kCoast);
|
||||
leftDrive2.setIdleMode(IdleMode.kCoast);
|
||||
rightDrive1.setIdleMode(IdleMode.kCoast);
|
||||
rightDrive2.setIdleMode(IdleMode.kCoast);
|
||||
leftDrive1.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
rightDrive1.setNeutralMode(NeutralMode.Coast);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -384,35 +339,22 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
// return isHopperSensorBlueOn();
|
||||
// }
|
||||
//}
|
||||
|
||||
public void setStraightMM(double distanceInches, double maxVelocity, double maxAcceleration, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
|
||||
double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
|
||||
mmStraightController.setPID(mmStraightPIDParams);
|
||||
mmStraightController.setMMStraightTarget(0, distanceInches, maxVelocity, maxAcceleration, useGyroLock, yawAngle, true);
|
||||
setControlMode(DriveControlMode.MOTION_MAGIC);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
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) {
|
||||
|
||||
//public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) {
|
||||
// double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg();
|
||||
// mpStraightController.setPID(mpStraightPIDParams);
|
||||
// mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true);
|
||||
@@ -449,7 +391,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);
|
||||
@@ -471,7 +413,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);
|
||||
@@ -482,14 +424,13 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
}
|
||||
|
||||
public void updatePose() {
|
||||
//m_encoder = m_motor.getEncoder();
|
||||
left_distance = leftDrive1.getEncoder();
|
||||
right_distance = rightDrive1.getEncoder();
|
||||
double left_distance = leftDrive1.getPositionWorld();
|
||||
double right_distance = rightDrive1.getPositionWorld();
|
||||
Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg());
|
||||
lastPose = currentPose;
|
||||
// currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle);
|
||||
// left_encoder_prev_distance_ = left_distance;
|
||||
// right_encoder_prev_distance_ = right_distance;
|
||||
currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle);
|
||||
left_encoder_prev_distance_ = left_distance;
|
||||
right_encoder_prev_distance_ = right_distance;
|
||||
}
|
||||
|
||||
public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) {
|
||||
@@ -503,46 +444,34 @@ 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;
|
||||
if (controlMode == DriveControlMode.JOYSTICK) {
|
||||
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
|
||||
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
|
||||
leftDrive1.set(0); //TODO URGENT: make sure not called when robot moving
|
||||
rightDrive1.set(0); //TODO URGENT: make sure not called when robot moving
|
||||
leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
|
||||
rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
|
||||
}
|
||||
else if (controlMode == DriveControlMode.MANUAL) {
|
||||
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
|
||||
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
|
||||
leftDrive1.set(0); //TODO URGENT: make sure not called when robot moving
|
||||
rightDrive1.set(0); //TODO URGENT: make sure not called when robot moving
|
||||
leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
|
||||
rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
|
||||
}
|
||||
else if (controlMode == DriveControlMode.CLIMB) {
|
||||
//leftDrive1.changeControlMode(TalonControlMode.PercentVbus);
|
||||
//rightDrive1.changeControlMode(TalonControlMode.PercentVbus);
|
||||
leftDrive1.set(0); //TODO URGENT: make sure not called when robot moving
|
||||
rightDrive1.set(0); //TODO URGENT: make sure not called when robot moving
|
||||
leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
|
||||
rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
else if (controlMode == DriveControlMode.HOLD) {
|
||||
mpStraightController.setPID(mpHoldPIDParams);
|
||||
//leftDrive1.changeControlMode(TalonControlMode.Position);
|
||||
@@ -551,11 +480,11 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
//rightDrive1.changeControlMode(TalonControlMode.Position);
|
||||
//rightDrive1.setPosition(0);
|
||||
//rightDrive1.set(0);
|
||||
//leftDrive1.setSelectedSensorPosition(0, 0, 0); //not needed for spark? TODO: verify want 0="Primary closed-loop", with no timeout
|
||||
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);
|
||||
}*/
|
||||
leftDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
|
||||
leftDrive1.set(ControlMode.Position, 0);
|
||||
rightDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout
|
||||
rightDrive1.set(ControlMode.Position, 0);
|
||||
}
|
||||
isFinished = false;
|
||||
}
|
||||
|
||||
@@ -584,13 +513,16 @@ 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.MOTION_MAGIC) {
|
||||
isFinished = mmStraightController.controlLoopUpdate(getGyroAngleDeg());
|
||||
}
|
||||
else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) {
|
||||
updatePose();
|
||||
isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
|
||||
}*/
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -704,7 +636,7 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
|
||||
m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
|
||||
}
|
||||
|
||||
|
||||
m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75);
|
||||
// break;
|
||||
// case CONTROLLER_XBOX_ARCADE_RIGHT:
|
||||
@@ -738,10 +670,14 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
|
||||
public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) {
|
||||
|
||||
|
||||
leftDrive1.set(leftPercentOutput);
|
||||
rightDrive1.set(rightPercentOutput);
|
||||
|
||||
if (elevatorRight.getSelectedSensorPosition(0) >= 3550) {
|
||||
leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput*.5);
|
||||
rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput*.5);
|
||||
}
|
||||
else /*(elevatorRight.getSelectedSensorPosition(0) < 3550)*/ {
|
||||
leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput);
|
||||
rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput);
|
||||
}
|
||||
}
|
||||
|
||||
private boolean inDeadZone(double input) {
|
||||
@@ -814,14 +750,14 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
|
||||
@Override
|
||||
public void setPeriodMs(long periodMs) {
|
||||
/* FIX TODAY
|
||||
//mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers);
|
||||
mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers);
|
||||
mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers);
|
||||
pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers);
|
||||
mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers);
|
||||
mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers);
|
||||
adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers);
|
||||
*/
|
||||
|
||||
this.periodMs = periodMs;
|
||||
}
|
||||
|
||||
@@ -854,34 +790,37 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
}
|
||||
|
||||
public double getLeftPositionWorld() {
|
||||
return 0;//leftDrive1.getPositionWorld(); FIX TODAY
|
||||
return leftDrive1.getPositionWorld();
|
||||
}
|
||||
|
||||
public double getRightPositionWorld() {
|
||||
return 0;//rightDrive1.getPositionWorld(); FIX TODAY
|
||||
return rightDrive1.getPositionWorld();
|
||||
}
|
||||
|
||||
public void updateStatus(Robot.OperationMode operationMode) {
|
||||
if (operationMode == Robot.OperationMode.TEST) {
|
||||
try {
|
||||
SmartDashboard.putNumber("Gyro Value", getGyroAngleDeg());
|
||||
SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
|
||||
|
||||
|
||||
SmartDashboard.putNumber("Right Pos Ticks", right_distance.getPosition());//rightDrive1.getSelectedSensorPosition(0)); FIX TODAY
|
||||
SmartDashboard.putNumber("Left Pos Ticks", left_distance.getPosition());//leftDrive1.getSelectedSensorPosition(0));
|
||||
|
||||
|
||||
//SmartDashboard.putNumber("Right Pos Inches", -encoderRight.getPosition());//rightDrive1.getPositionWorld());
|
||||
//SmartDashboard.putNumber("Left Pos Inches", encoderLeft.getPosition());//leftDrive1.getPositionWorld());
|
||||
//SmartDashboard.putNumber("Right Vel Ft-Sec", 0);//rightDrive1.getVelocityWorld() / 12);
|
||||
//SmartDashboard.putNumber("Left Vel Ft-Sec", 0);//leftDrive1.getVelocityWorld() / 12);
|
||||
SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld());
|
||||
SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld());
|
||||
SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12);
|
||||
SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.getVelocityWorld() / 12);
|
||||
//SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID));
|
||||
//SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID));
|
||||
// SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID));
|
||||
//SmartDashboard.putNumber("Right 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID));
|
||||
//SmartDashboard.putNumber("Right 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID));
|
||||
// SmartDashboard.putNumber("Right 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID));
|
||||
//SmartDashboard.putBoolean("Hopper Sensor Red", isHopperSensorRedOn());
|
||||
//SmartDashboard.putBoolean("Hopper Sensor Blue", isHopperSensorBlueOn());
|
||||
SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD);
|
||||
//SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg());
|
||||
SmartDashboard.putNumber("Yaw Angle Deg", Math.abs(getGyroAngleDeg() % 360));
|
||||
//MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint();
|
||||
//double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0;
|
||||
//SmartDashboard.putNumber("Gyro Delta", delta);
|
||||
SmartDashboard.putNumber("Yaw Angle Deg", getGyroAngleDeg());
|
||||
MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint();
|
||||
double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0;
|
||||
SmartDashboard.putNumber("Gyro Delta", delta);
|
||||
//SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating);
|
||||
SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating());
|
||||
SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg);
|
||||
@@ -890,22 +829,14 @@ public class Drive extends Subsystem implements ControlLoopable
|
||||
SmartDashboard.putNumber("Move Output", m_moveOutput);
|
||||
SmartDashboard.putNumber("Steer Input", m_steerInput);
|
||||
SmartDashboard.putNumber("Move Input", m_moveInput);
|
||||
SmartDashboard.putString("MODE", "TEST");
|
||||
//if (left_distance.getPosition() != 0 && right_distance.getPosition() != 0){
|
||||
// SmartDashboard.putNumber("Distance Inches", (left_distance.getPosition()-right_distance.getPosition())/2);//rightDrive1.getPositionWorld());
|
||||
//}
|
||||
}
|
||||
catch (Exception e) {
|
||||
System.err.println("Drivetrain update status error" +e.getMessage());
|
||||
System.err.println("Drivetrain update status error");
|
||||
}
|
||||
}
|
||||
else if (operationMode == Robot.OperationMode.COMPETITION) {
|
||||
SmartDashboard.putNumber("Yaw Angle Deg", Math.abs(getGyroAngleDeg() % 360));
|
||||
SmartDashboard.putString("MODE", "SICKO");
|
||||
if (left_distance.getPosition() != 0 && left_distance.getPosition() != 0){
|
||||
SmartDashboard.putNumber("Distance Inches", (left_distance.getPosition()-right_distance.getPosition())/2);//rightDrive1.getPositionWorld());
|
||||
}
|
||||
SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0);
|
||||
else {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user