/**
* ************************************
*
* <p>Does the configuration required for basic drive. So, when switching to basic drive this gets
* called and now the robot will work with basic drive.
*
* <p>************************************
*/
public void setBasic() {
current = profiles.BASIC;
leftMotor1.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
rightMotor1.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
}
public static void teleopPeriodic() {
/*
double newArmPos = gamepad.getRawAxis(1);
if(Math.abs(newArmPos) <= ARM_DEADZONE) {
newArmPos = 0.0;
}
double newMotorPos = (newArmPos * ARM_SPEED_MULTIPLIER) + frontArmMotor.getPosition();
//double newMotorPos = (newArmPos * ARM_SPEED_MULTIPLIER);
frontArmMotor.set(newMotorPos);
System.out.println("input = " + newArmPos + " target pos = " + newMotorPos + " enc pos = " + frontArmMotor.getPosition());
*/
// PercentVbus test ONLY!!
double armSpeed = gamepad.getRawAxis(1);
if (Math.abs(armSpeed) < ARM_DEADZONE) {
armSpeed = 0.0f;
}
armSpeed *= ARM_MULTIPLIER;
double pos = frontArmMotor.getPosition();
if (((pos > SOFT_ENCODER_LIMIT_1) && armSpeed < 0.0)
|| ((pos < SOFT_ENCODER_LIMIT_2) && armSpeed > 0.0)) armSpeed = 0.0;
frontArmMotor.set(armSpeed);
System.out.println("armSpeed = " + armSpeed + " enc pos = " + frontArmMotor.getPosition());
// check for roller motion (right gamepad joystick)
double rollerSpeed = gamepad.getRawAxis(3);
if (Math.abs(rollerSpeed) < ROLLER_DEADZONE) {
rollerSpeed = 0.0f;
}
rollerSpeed *= ARM_ROLLER_MULTIPLIER;
frontArmRollerMotor.set(rollerSpeed);
}
/**
* ************************************************
*
* <p>Basic move is non PID, so it is cubic drive in percentVBus mode. -1 to 1
*
* @param left is left drive value (+) for forward (-) for reverse
* @param right is right drive value (+) for forward (-) for reverse
* <p>************************************************
*/
public void moveBasic(double left, double right) {
if (getState() != CANTalon.TalonControlMode.PercentVbus) setBasic();
leftMotor1.set(left);
rightMotor1.set(right);
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
chooser = new SendableChooser();
chooser.addDefault("Default Auto", defaultAuto);
chooser.addObject("My Auto", customAuto);
SmartDashboard.putData("Auto choices", chooser);
controlMethod = new SendableChooser();
controlMethod.addDefault("Tank Control", tankControl);
controlMethod.addObject("Stick Control", stickControl);
controlMethod.addObject("Clayton Control", claytonControl);
SmartDashboard.putData("Control method", controlMethod);
rightMotor = new CANTalon(RIGHT_INDEX);
rightSlave = new CANTalon(RIGHT_INDEX + 1);
rightSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
rightSlave.set(RIGHT_INDEX);
rightMotor.setInverted(true);
leftMotor = new CANTalon(LEFT_INDEX);
leftSlave = new CANTalon(LEFT_INDEX + 1);
leftSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
leftSlave.set(LEFT_INDEX);
intakeMotor = new CANTalon(INTAKE_INDEX);
// Arm motor is currently disabled
// armMotor = new CANTalon(ARM_INDEX);
compressor = new Compressor(0);
compressor.setClosedLoopControl(true);
intakePneumatic = new DoubleSolenoid(1, 2);
intakePneumatic.set(DoubleSolenoid.Value.kOff);
gamepad = new Gamepad(0);
}
/**
* ********************************
*
* <p>Configures encoders: - Sets if the encoders are inverted - Configures tick count on encoders
* (ours are 360)
*
* <p>********************************
*/
private void setupEncoders() {
leftMotor1.reverseSensor(invertLeftEncoder);
rightMotor1.reverseSensor(invertRightEncoder);
leftMotor1.configEncoderCodesPerRev(360);
rightMotor1.configEncoderCodesPerRev(360);
}
public void startWinch() {
// TODO Don't start if the limit switch is set
if (!atLowerLimit()) {
d_motor.set(0.75);
} else {
d_motor.set(0);
}
}
public void tankDrive() {
double left = OI.getInstance().getLeftStick().getY();
double right = OI.getInstance().getRightStick().getY();
drive.tankDrive(left, right);
frontRight.set(RobotMap.REAR_RIGHT_MOTOR);
frontLeft.set(RobotMap.REAR_LEFT_MOTOR);
}
public static void getencoder() {
PulseWidthpos = talon_arm.getPulseWidthPosition();
PulseWidthus = talon_arm.getPulseWidthRiseToFallUs();
periodus = talon_arm.getPulseWidthRiseToRiseUs();
PulseWidthVel = talon_arm.getPulseWidthVelocity();
FeedbackDeviceStatus sensorstaus =
talon_arm.isSensorPresent(FeedbackDevice.CtreMagEncoder_Absolute);
}
public double getRightEncoder(String inFt) {
if (inFt.equals("in")) {
double value = RIGHT_MOTOR1.getEncPosition();
return countsToInch(value);
} else {
double value = RIGHT_MOTOR1.getEncPosition();
return countsToFeet(value);
}
}
/**
* Set the setpoint for the PIDController
*
* @param setpoint the desired setpoint
*/
public synchronized void setSetpoint(double setpoint) {
m_setpoint = setpoint;
m_talon.set(setpoint);
;
reportPosition(m_talon.getPosition());
if (m_debugging) SmartDashboard.putNumber(m_name + " Talon Setpoint", m_setpoint);
}
/**
* ************************************
*
* <p>Does the configuration required for speed drive. So, when switching to speed drive this gets
* called and now the robot will work with speed drive.
*
* <p>************************************
*/
public void setSpeedMode() {
current = profiles.SPEED;
leftMotor1.setProfile(0);
rightMotor1.setProfile(0);
leftMotor1.changeControlMode(CANTalon.TalonControlMode.Speed);
rightMotor1.changeControlMode(CANTalon.TalonControlMode.Speed);
}
/**
* ************************************************
*
* <p>PIDSpeed move is a velocity based PID. -1 to 1
*
* @param leftSpeed is left drive value (+) for forward (-) for reverse
* @param rightSpeed is right drive value (+) for forward (-) for reverse
* <p>************************************************
*/
public void movePIDSpeed(double leftSpeed, double rightSpeed) {
if (getState() != CANTalon.TalonControlMode.Speed) setSpeedMode();
leftMotor1.set(
leftSpeed
* MOTOR_TOP_RPM); // Multiply by the top RPM because the -1 to 1 is the percent of the
// top RPM you would like to travel
rightMotor1.set(rightSpeed * MOTOR_TOP_RPM);
}
/**
* ************************************
*
* <p>Does the configuration required for positional drive. So, when switching to positional drive
* this gets called and now the robot will work with positional drive.
*
* <p>************************************
*/
public void setPositional() {
current = profiles.POSITIONAL;
leftMotor1.setProfile(0);
rightMotor1.setProfile(0);
// rightMotor1.reverseSensor(true);
leftMotor1.changeControlMode(CANTalon.TalonControlMode.Position);
rightMotor1.changeControlMode(CANTalon.TalonControlMode.Position);
}
public MasterCANTalon(int port) {
talon = new CANTalon(port);
talon.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);
talon.reverseSensor(false);
talon.setProfile(0);
talon.setF(0.1722);
talon.setP(0.1);
talon.setI(0);
talon.setD(0);
talon.configNominalOutputVoltage(+0.0f, -0.0f);
talon.configPeakOutputVoltage(+12.0f, -12.0f);
talon.changeControlMode(TalonControlMode.Speed);
}
public DriveTrainSubsystem() {
RIGHT_MOTOR1 = new CANTalon(RobotMap.DRIVERIGHT_MOTOR1);
RIGHT_MOTOR2 = new CANTalon(RobotMap.DRIVERIGHT_MOTOR2);
LEFT_MOTOR1 = new CANTalon(RobotMap.DRIVELEFT_MOTOR1);
LEFT_MOTOR2 = new CANTalon(RobotMap.DRIVELEFT_MOTOR2);
LEFT_MOTOR1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
LEFT_MOTOR1.setPosition(0);
RIGHT_MOTOR1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
RIGHT_MOTOR1.setPosition(0);
LiveWindow.addActuator("Drive", "R1", RIGHT_MOTOR1);
LiveWindow.addActuator("Drive", "R2", RIGHT_MOTOR2);
LiveWindow.addActuator("Drive", "L1", LEFT_MOTOR1);
LiveWindow.addActuator("Drive", "L2", LEFT_MOTOR2);
}
/** This method resets the motor position sensor, typically an encoder. */
public void resetPosition() {
if (feedbackDeviceIsPot) {
zeroPosition = super.getPosition();
} else {
super.setPosition(0.0);
}
} // resetPosition
public void teleopPeriodic() {
double throttle = (driveStick.getThrottle() + 1) / -2;
double xAxis = throttle * (Math.abs(driveStick.getX()) > 0.15 ? driveStick.getX() : 0.0);
double yAxis = throttle * (Math.abs(driveStick.getY()) > 0.15 ? driveStick.getY() : 0.0);
double rotation =
throttle * (Math.abs(driveStick.getTwist()) > 0.15 ? driveStick.getTwist() : 0.0);
// Mecanum drive
drive.mecanumDrive_Cartesian(xAxis, yAxis, rotation, 0);
// Elevator control
elevator.set(driveStick.getRawButton(5) ? -1.0 : driveStick.getRawButton(3) ? 1.0 : 0.0);
// Intake control
intakeTensionMotor.set(
driveStick.getRawButton(2) ? -1.0 : driveStick.getRawButton(7) ? 1.0 : 0.0);
if (Math.abs(controlStick.getTwist()) > 0) {
double toteTurnSpeed = -controlStick.getTwist();
intakeMotors[0].set(
toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
intakeMotors[1].set(
toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
} else {
double intakeSpeed =
driveStick.getRawButton(1) ? -1.0 : driveStick.getRawButton(8) ? 0.4 : 0.0;
intakeMotors[0].set(-intakeSpeed);
intakeMotors[1].set(intakeSpeed);
}
}
public static void init() {
if (inited) {
talon_arm = new CANTalon(talon_arm_id);
talon_arm.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Absolute);
JoyDrive.init();
}
}
public static void Dashboard() {
SmartDashboard.putNumber("arm_value", talon_arm.get());
SmartDashboard.putInt("PulseWidthpos", PulseWidthpos);
SmartDashboard.putInt("PulseWidthus", PulseWidthus);
SmartDashboard.putInt("periodus", periodus);
SmartDashboard.putInt("PulseWidthVel", PulseWidthVel);
SmartDashboard.putNumber("val", JoyDrive.val);
}
/**
* This method sets the output power of the motor controller.
*
* @param output specifies the output power for the motor controller in the range of -1.0 to 1.0.
*/
public void setPower(double power) {
if (softLowerLimitEnabled && power < 0.0 && getPosition() <= softLowerLimit
|| softUpperLimitEnabled && power > 0.0 && getPosition() >= softUpperLimit) {
power = 0.0;
}
super.set(power);
} // setPower
private void initSmartDashboard() {
SmartDashboard.putNumber(m_name + " P", m_talon.getP());
SmartDashboard.putNumber(m_name + " I", m_talon.getI());
SmartDashboard.putNumber(m_name + " D", m_talon.getD());
SmartDashboard.putNumber(m_name + " F", m_talon.getF());
SmartDashboard.putNumber(m_name + " I*100", (m_talon.getI()) * 100);
SmartDashboard.putNumber(m_name + " Talon Setpoint", m_setpoint);
SmartDashboard.putNumber(m_name + " Position", m_talon.getPosition());
SmartDashboard.putNumber(m_name + " Tolerance", m_tolerance);
SmartDashboard.putNumber(m_name + " Error", m_talon.getClosedLoopError());
SmartDashboard.putNumber(m_name + " Output", m_talon.get());
SmartDashboard.putBoolean(m_name + " Enabled", m_talon.isControlEnabled());
}
/**
* ************************************************
*
* <p>PIDPositional move is a position based PID that will move to a position some distance away
* from current location
*
* @param leftDistanceInches is left drive value (+) for forward (-) for reverse
* @param rightDistanceInches is right drive value (+) for forward (-) for reverse
* <p>************************************************
*/
public void movePIDPositional(double leftDistanceInches, double rightDistanceInches) {
if (getState() != CANTalon.TalonControlMode.Position) setPositional();
double travelDistanceLeft = getLeftDistanceInches() + leftDistanceInches;
double travelDistanceRight = getRightDistanceInches() + rightDistanceInches;
travelDistanceLeft *= -1;
travelDistanceRight *= -1;
travelDistanceLeft /= ((FINAL_RADIUS * (2 * Math.PI)));
// travelDistanceLeft *= 360.0;
travelDistanceRight /= ((FINAL_RADIUS * (2 * Math.PI)));
// travelDistanceRight *= 360.0;
leftMotor1.set(travelDistanceLeft);
rightMotor1.set(travelDistanceRight);
}
public static void disabledInit() {
if (!initialized) initialize();
// if exiting from teleop mode (game is over)...
if (teleopMode) {
System.out.println("FrontArmAssembly: exiting teleop, moving to coast mode");
// relax front arm motor (coast mode)
frontArmMotor.enableBrakeMode(false);
}
}
public void usePIDOutput(double output) {
// Use output to drive your system, like a motor
// e.g. yourMotor.set(output);
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT
hinge.pidWrite(output);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT
}
// static initializer
public static void initialize() {
if (!initialized) {
gamepad = new Joystick(GAMEPAD_ID);
initialized = true;
teleopMode = false;
// create and initialize arm motor
frontArmMotor = new CANTalon(FRONT_ARM_MOTOR_ID);
if (frontArmMotor != null) {
System.out.println("Initializing front arm motor (position control)...");
// VERY IMPORTANT - resets talon faults to render them usable again!!
frontArmMotor.clearStickyFaults();
frontArmMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
frontArmMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
// set brake mode
frontArmMotor.enableBrakeMode(true);
// initializes encoder to zero
frontArmMotor.setPosition(0);
} else System.out.println("ERROR: Front Arm motor not initialized!");
// create and initialize roller motor
frontArmRollerMotor = new CANTalon(FRONT_ARM_ROLLER_ID);
if (frontArmRollerMotor != null) {
System.out.println("Initializing front arm roller motor (PercentVbus control)...");
frontArmRollerMotor.clearStickyFaults();
// set up roller motor for percent Vbus control mode
frontArmRollerMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
frontArmRollerMotor.enableBrakeMode(false);
// initializes speed of rollers to zero
frontArmRollerMotor.set(0);
} else System.out.println("ERROR: Front Arm roller motor not initialized!");
}
}
/**
* 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 synchronized void setPID(double p, double i, double d, double f) {
m_P = p;
m_I = i;
m_D = d;
m_F = f;
m_talon.setProfile(0);
m_talon.setP(p);
m_talon.setI(i);
m_talon.setD(d);
m_talon.setF(f);
if (m_debugging) {
SmartDashboard.putNumber(m_name + " P", m_talon.getP());
SmartDashboard.putNumber(m_name + " I", m_talon.getI());
SmartDashboard.putNumber(m_name + " D", m_talon.getD());
SmartDashboard.putNumber(m_name + " F", m_talon.getF());
}
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
MotorOutput motorOutput = drivetrainJoystick.motionForGamepadInput(gamepad);
leftMotor.set(motorOutput.left * MAX_SPEED);
rightMotor.set(motorOutput.right * MAX_SPEED);
double in = gamepad.getLeftTrigger();
double out = gamepad.getRightTrigger();
intakeMotor.set((in - out) * INTAKE_SPEED);
double fore = gamepad.getButtonLeftBack() ? 1 : 0;
double back = gamepad.getButtonRightBack() ? 1 : 0;
// Don't run the arm motor because it's not properly fastened
// armMotor.set((fore - back) * ARM_SPEED);
if (gamepad.getButtonA()) {
intakePneumatic.set(DoubleSolenoid.Value.kForward);
} else if (gamepad.getButtonB()) {
intakePneumatic.set(DoubleSolenoid.Value.kReverse);
} else {
intakePneumatic.set(DoubleSolenoid.Value.kOff);
}
}
public SpeedController() {
speedController.setFeedbackDevice(FeedbackDevice.QuadEncoder);
speedController.configEncoderCodesPerRev(20);
speedController.setP(0.75);
speedController.setI(0.01);
speedController.setD(0.0);
speedController.setCloseLoopRampRate(.01);
}
/**
* ***************************
*
* <p>Completes everything required for PID to work: - Sets encoder/potentiometer type - Sets the
* P, I, D, and F terms - configures min and max voltages
*
* <p>***************************
*/
private void setupPID() {
leftMotor1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
rightMotor1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
leftMotor1.configNominalOutputVoltage(+0f, -0f);
rightMotor1.configNominalOutputVoltage(+0f, -0f);
leftMotor1.configPeakOutputVoltage(+12.0f, -12.0f);
rightMotor1.configPeakOutputVoltage(+12.0f, -12.0f);
// leftMotor1.
setPID(0.62, 0.006, 0.0, 0.36341);
}
@Override
public TalonSRX setStatusFrameRate(StatusFrameRate frameRate, int periodMillis) {
talon.setStatusFrameRateMs(
edu.wpi.first.wpilibj.CANTalon.StatusFrameRate.valueOf(frameRate.value()), periodMillis);
double periodInSeconds = periodMillis / 1000.0;
switch (frameRate) {
case FEEDBACK:
feedbackRateInSeconds = periodInSeconds;
break;
case QUADRATURE_ENCODER:
quadratureRateInSeconds = periodInSeconds;
break;
case ANALOG_TEMPERATURE_BATTERY_VOLTAGE:
analogRateInSeconds = periodInSeconds;
break;
case GENERAL:
// nothing to set, since our code doesn't use the "general" frames
break;
}
return this;
}