private void enabledInit()
{
Debug.Print("Enabled");
pcm.StartCompressor();
leftMaster.SetNeutralMode(NeutralMode.Coast);
leftSlave1.SetNeutralMode(NeutralMode.Coast);
leftSlave2.SetNeutralMode(NeutralMode.Coast);
rightMaster.SetNeutralMode(NeutralMode.Coast);
rightSlave1.SetNeutralMode(NeutralMode.Coast);
rightSlave2.SetNeutralMode(NeutralMode.Coast);
}
public static void Main()
{
isPressed = false;
lightsIsPressed = false;
blinkLightsIsPressed = false;
lightsOn = true;
right.SetNeutralMode(NeutralMode.Brake);
rightSlave.SetNeutralMode(NeutralMode.Brake);
left.SetNeutralMode(NeutralMode.Brake);
leftSlave.SetNeutralMode(NeutralMode.Brake);
elevationMotor.SetNeutralMode(NeutralMode.Brake);
elevationMotor.ConfigOpenloopRamp(0.25f);
/* loop forever */
while (true)
{
/* drive robot using gamepad */
Drive();
ShootButton(8);
ToggleLights(2);
/* feed watchdog to keep Talon's enabled */
CTRE.Phoenix.Watchdog.Feed();
/* run this task every 20ms */
Thread.Sleep(20);
}
}
static void Drive()
{
//Joystick values range from -1 to 1
//CSK 11/30/2018 axis(0) is left joystick X axis - left right
//The minus signs are affected by the motor wiring polarity
//If rotation is backwards from what is expected then either reverse the wires to the motor or change the minus sign
double main_throttle = _gamepad.GetAxis(LEFT_JOY_Y);
double steering = _gamepad.GetAxis(RIGHT_JOY_X);
//twist is right joystick horizontal (steering)
double leftMotors;
double rightMotors;
//CSK 12/6/2018 Make sure drive controllers are in coast mode
leftdrive1.SetNeutralMode(NeutralMode.Coast);
rightdrive1.SetNeutralMode(NeutralMode.Coast);
leftMotors = -main_throttle.Deadband() - steering;
rightMotors = main_throttle.Deadband() - steering;
leftMotors = leftMotors.ClampRange(FULL_REVERSE, FULL_FORWARD);
rightMotors = rightMotors.ClampRange(FULL_REVERSE, FULL_FORWARD);
leftdrive1.Set(ControlMode.PercentOutput, leftMotors);
rightdrive1.Set(ControlMode.PercentOutput, rightMotors);
#if (HASDISPLAY)
//VVV CSK 11/2/2018 From HeroBridge_with_Arcade_And_Display code
DisplayData(leftMotors, rightMotors);
#endif
return;
}
// Initialize all variables and start the main loop
public void Init()
{
controller = new Controller();
frontLeftMotor = new TalonSRX(1);
frontRightMotor = new TalonSRX(2);
backLeftMotor = new TalonSRX(3);
backRightMotor = new TalonSRX(4);
frontLeftMotor.SetNeutralMode(NeutralMode.Brake);
frontRightMotor.SetNeutralMode(NeutralMode.Brake);
backLeftMotor.SetNeutralMode(NeutralMode.Brake);
backRightMotor.SetNeutralMode(NeutralMode.Brake);
backLeftMotor.Follow(frontLeftMotor);
backRightMotor.Follow(frontRightMotor);
intakePivot = new TalonSRX(5);
intakeRoller = new TalonSRX(6);
disabled = false;
prevButton = false;
InitPeriodic();
}
/**
* Setup all of the configuration parameters.
*/
public void SetupConfig()
{
/* Factory Default all hardware to prevent unexpected behaviour */
_talon.ConfigFactoryDefault();
/* specify sensor characteristics */
_talon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0);
_talon.SetSensorPhase(false); /* make sure positive motor output means sensor moves in position direction */
/* brake or coast during neutral */
_talon.SetNeutralMode(NeutralMode.Brake);
/* closed-loop and motion-magic parameters */
_talon.Config_kF(kSlotIdx, 0.1153f, kTimeoutMs); // 8874 native sensor units per 100ms at full motor output (+1023)
_talon.Config_kP(kSlotIdx, 2.00f, kTimeoutMs);
_talon.Config_kI(kSlotIdx, 0f, kTimeoutMs);
_talon.Config_kD(kSlotIdx, 20f, kTimeoutMs);
_talon.Config_IntegralZone(kSlotIdx, 0, kTimeoutMs);
_talon.SelectProfileSlot(kSlotIdx, 0); /* select this slot */
_talon.ConfigNominalOutputForward(0f, kTimeoutMs);
_talon.ConfigNominalOutputReverse(0f, kTimeoutMs);
_talon.ConfigPeakOutputForward(1.0f, kTimeoutMs);
_talon.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
_talon.ConfigMotionCruiseVelocity(8000, kTimeoutMs); // 8000 native units
_talon.ConfigMotionAcceleration(16000, kTimeoutMs); // 16000 native units per sec, (0.5s to reach cruise velocity).
/* Home the relative sensor,
* alternatively you can throttle until limit switch,
* use an absolute signal like CtreMagEncoder_Absolute or analog sensor.
*/
_talon.SetSelectedSensorPosition(0, kTimeoutMs);
}
public static TalonSRX CreateAugerRotator(int CAN_ID)
{
TalonSRX talon = new TalonSRX(CAN_ID);
talon.SetNeutralMode(NeutralMode.Brake);
return(talon);
}
private void initMotors()
{
leftTalon.ConfigFactoryDefault();
rightTalon.ConfigFactoryDefault();
switch (isCoast)
{
case true:
leftTalon.SetNeutralMode(NeutralMode.Coast);
rightTalon.SetNeutralMode(NeutralMode.Coast);
break;
case false:
leftTalon.SetNeutralMode(NeutralMode.Brake);
rightTalon.SetNeutralMode(NeutralMode.Brake);
break;
default:
break;
}
}
//DriveBase
public static TalonSRX CreateDriveBaseTalon(int CAN_ID, bool inverted = false)
{
TalonSRX talon = new TalonSRX(CAN_ID);
const int MAX_CURRENT = 10;
talon.ConfigContinuousCurrentLimit(MAX_CURRENT);
talon.ConfigPeakCurrentLimit(2 * MAX_CURRENT);
talon.ConfigPeakCurrentDuration(0);
talon.SetInverted(inverted);
talon.SetNeutralMode(NeutralMode.Brake);
return(talon);
}
public static void Drive(GameController GAMEPAD, StringBuilder stringBuilder)
{
/*Talon and Encoder Constants*/
right.SetNeutralMode(NeutralMode.Brake);
//rightSlave.SetNeutralMode(NeutralMode.Brake);
left.SetNeutralMode(NeutralMode.Brake);
//leftSlave.SetNeutralMode(NeutralMode.Brake);
/*Right side of drivetrain needs to be inverted*/
right.SetInverted(true);
//rightSlave.SetInverted(true);
//left.ConfigSelectedFeedbackSensor(FeedbackDevice.QuadEncoder,Helpers.PID,Helpers.timeoutMs);
//leftSlave.ConfigRemoteFeedbackFilter(left.GetDeviceID(), RemoteSensorSource.RemoteSensorSource_TalonSRX_SelectedSensor, Helpers.remotePID, Helpers.timeoutMs);
//right.ConfigSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, Helpers.PID, Helpers.timeoutMs);
//rightSlave.ConfigRemoteFeedbackFilter(left.GetDeviceID(), RemoteSensorSource.RemoteSensorSource_TalonSRX_SelectedSensor, Helpers.remotePID, Helpers.timeoutMs);
/*End Constants*/
if (null == GAMEPAD)
{
GAMEPAD = new GameController(UsbHostDevice.GetInstance(0));
}
double x = GAMEPAD.GetAxis(1);
double y = GAMEPAD.GetAxis(3);
//Helpers.Deadband(ref x);
//Helpers.Deadband(ref y);
//Pow(x,2) gives finer controls over the drivebase
//.5 for total half-speed reduction
//sign(x) returns the sign, which is useful since the pow removes the negative sign.
double leftThrot = (System.Math.Pow(x, 2)) * .5 * System.Math.Sign(x);
double rightThrot = (System.Math.Pow(y, 2)) * .5 * System.Math.Sign(y);
//TODO
//Uncomment when ready to test on a robot
left.Set(ControlMode.PercentOutput, leftThrot);
//leftSlave.Set(ControlMode.PercentOutput, leftThrot);
right.Set(ControlMode.PercentOutput, -rightThrot);
//rightSlave.Set(ControlMode.PercentOutput, -rightThrot);
stringBuilder.Append("\t");
stringBuilder.Append(leftThrot);
stringBuilder.Append("\t");
stringBuilder.Append(rightThrot);
}
private Chassis()
{
Robotmap map = Robotmap.GetInstance();
//Constants for variables to use
left_motor = new TalonSRX(map.GetLeftDrive_ID());
left_motor.SetNeutralMode(NeutralMode.Brake);
left_motor.SetInverted(true); // Can invert the direction the motors are moving
right_motor = new TalonSRX(map.GetRightDrive_ID());
right_motor.SetNeutralMode(NeutralMode.Brake);
right_motor.SetInverted(false); // Can invert the direction the motors are moving
drive_assister = new PigeonIMU(map.Get_ID_Pigeon()); //TODO Get real name of pigeon from Christina
bumper_switch = Robotmap.GETCANController();
}
public Cannon()
{
angleMotor.SetNeutralMode(NeutralMode.Brake);
angleMotor.ConfigPeakOutputForward(0.5f);
angleMotor.ConfigPeakOutputReverse(-0.5f);
angleMotor.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog);
angleMotor.ConfigSelectedFeedbackCoefficient(1.0f);
angleMotor.Config_kP(0.001f);
revolverMotor.SetInverted(true);
revolverMotor.SetNeutralMode(NeutralMode.Brake);
revolverMotor.ConfigPeakOutputForward(0.5f);
revolverMotor.ConfigPeakOutputReverse(0.5f);
revolverMotor.ConfigContinuousCurrentLimit(20);
revolverMotor.ConfigPeakCurrentLimit(0);
revolverMotor.ConfigPeakCurrentDuration(0);
revolverMotor.EnableCurrentLimit(true);
_setpoint = Angle;
}
public void Config(int talonId0, int talonId1)
{
talon0 = new TalonSRX(talonId0);
talon1 = new TalonSRX(talonId1);
talon0.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0);
talon0.SetSensorPhase(false);
talon0.SetNeutralMode(NeutralMode.Brake);
talon0.Config_kF(kSlotId, 0.1153f, kTimeout);
talon0.Config_kP(kSlotId, 2.00f, kTimeout);
talon0.Config_kI(kSlotId, 0f, kTimeout);
talon0.Config_kD(kSlotId, 20f, kTimeout);
talon0.Config_IntegralZone(kSlotId, 0, kTimeout);
talon0.SelectProfileSlot(kSlotId, 0);
talon0.ConfigNominalOutputForward(0f, kTimeout);
talon0.ConfigNominalOutputReverse(0f, kTimeout);
talon0.ConfigPeakOutputForward(1.0f, kTimeout);
talon0.ConfigPeakOutputReverse(-1.0f, kTimeout);
talon0.ConfigMotionCruiseVelocity(8000, kTimeout);
talon0.ConfigMotionAcceleration(16000, kTimeout);
}
public Lift()
{
const int MAX_CURRENT = 15;
const int TIMEOUT_MS = 100;
LeftActuator.ConfigContinuousCurrentLimit(MAX_CURRENT, TIMEOUT_MS);
RightActuator.ConfigContinuousCurrentLimit(MAX_CURRENT, TIMEOUT_MS);
LeftActuator.SetNeutralMode(NeutralMode.Brake);
RightActuator.SetNeutralMode(NeutralMode.Brake);
LeftActuator.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog);
RightActuator.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog);
const float P = 33.57f;
const float I = 0;
LeftActuator.Config_kP(P);
RightActuator.Config_kP(P);
LeftActuator.Config_kI(I);
RightActuator.Config_kI(I);
LeftActuator.ConfigMotionAcceleration(150);
RightActuator.ConfigMotionAcceleration(150);
RightActuator.ConfigMotionCruiseVelocity(4);
LeftActuator.ConfigMotionCruiseVelocity(4);
ExcavationBelt.ConfigContinuousCurrentLimit(MAX_CURRENT, TIMEOUT_MS); //REMOVE ME EVENTUALLY
ExcavationBelt.SetInverted(true);
CollectionBelt.ConfigContinuousCurrentLimit(MAX_CURRENT, TIMEOUT_MS);
Digger.ConfigContinuousCurrentLimit(MAX_CURRENT, TIMEOUT_MS);
Digger.SetInverted(true);
}
public static void Main()
{
/* Disable drivetrain/motors */
_rightTalon.Set(ControlMode.PercentOutput, 0);
_leftTalon.Set(ControlMode.PercentOutput, 0);
#if (fourWheeled)
_rightFollower.Follow(_rightTalon);
_leftFollower.Follow(_leftTalon);
#endif
/* Configure output and sensor direction */
_rightTalon.SetInverted(true);
_leftTalon.SetInverted(false);
#if (fourWheeled)
_rightFollower.SetInverted(true);
_leftFollower.SetInverted(false);
#endif
/* Mode print */
Debug.Print("This is arcade drive using Arbitrary Feedforward");
bool Btn1 = false;
bool Btn2 = false;
bool Btn3 = false;
bool Btn4 = false;
bool Btn10 = false;
bool VoltageComp = false;
bool CurrentLimit = false;
bool NeutralState = false;
bool RampRate = false;
bool FirstCall = true;
while (true)
{
/* Enable motor controllers if gamepad connected */
if (_gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
/* Gamepad Stick Control */
float forward = -1 * _gamepad.GetAxis(1);
float turn = 1 * _gamepad.GetAxis(2);
CTRE.Phoenix.Util.Deadband(ref forward);
CTRE.Phoenix.Util.Deadband(ref turn);
turn *= 0.5f; //Scaled down for safety
forward *= 0.75f;
bool btn1 = _gamepad.GetButton(1);
bool btn2 = _gamepad.GetButton(2);
bool btn3 = _gamepad.GetButton(3);
bool btn4 = _gamepad.GetButton(4);
bool btn10 = _gamepad.GetButton(10);
if (btn1 && !Btn1)
{
VoltageComp = !VoltageComp;
FirstCall = true;
}
else if (btn2 && !Btn2)
{
CurrentLimit = !CurrentLimit;
FirstCall = true;
}
else if (btn3 && !Btn3)
{
NeutralState = !NeutralState;
FirstCall = true;
}
else if (btn4 && !Btn4)
{
RampRate = !RampRate;
FirstCall = true;
}
else if (btn10 && !Btn10)
{
VoltageComp = false;
CurrentLimit = false;
NeutralState = false;
RampRate = false;
FirstCall = true;
}
Btn1 = btn1;
Btn2 = btn2;
Btn3 = btn3;
Btn4 = btn4;
Btn10 = btn10;
if (VoltageComp)
{
_rightTalon.ConfigVoltageCompSaturation(10, 10);
_rightTalon.ConfigVoltageMeasurementFilter(16, 10);
_rightTalon.EnableVoltageCompensation(true);
_leftTalon.ConfigVoltageCompSaturation(10, 10);
_leftTalon.ConfigVoltageMeasurementFilter(16, 10);
_leftTalon.EnableVoltageCompensation(true);
if (FirstCall)
{
Debug.Print("Voltage Compensation: On");
}
}
else
{
_rightTalon.EnableVoltageCompensation(false);
_leftTalon.EnableVoltageCompensation(false);
if (FirstCall)
{
Debug.Print("Voltage Compensation: Off");
}
}
if (CurrentLimit)
{
_rightTalon.ConfigContinuousCurrentLimit(10, 10);
_rightTalon.ConfigPeakCurrentLimit(10, 10);
_rightTalon.ConfigPeakCurrentDuration(0, 10);
_rightTalon.EnableCurrentLimit(true);
_leftTalon.ConfigContinuousCurrentLimit(10, 10);
_leftTalon.ConfigPeakCurrentLimit(10, 10);
_leftTalon.ConfigPeakCurrentDuration(0, 10);
_leftTalon.EnableCurrentLimit(true);
if (FirstCall)
{
Debug.Print("Current Limit: On");
}
}
else
{
_rightTalon.EnableCurrentLimit(false);
_leftTalon.EnableCurrentLimit(false);
if (FirstCall)
{
Debug.Print("Current Limit: Off");
}
}
if (NeutralState)
{
_rightTalon.SetNeutralMode(NeutralMode.Coast);
_leftTalon.SetNeutralMode(NeutralMode.Coast);
#if (fourWheeled)
_rightFollower.SetNeutralMode(NeutralMode.Coast);
_leftFollower.SetNeutralMode(NeutralMode.Coast);
#endif
if (FirstCall)
{
Debug.Print("Neutral Mode: Coast");
}
}
else
{
_rightTalon.SetNeutralMode(NeutralMode.Brake);
_leftTalon.SetNeutralMode(NeutralMode.Brake);
#if (fourWheeled)
_rightFollower.SetNeutralMode(NeutralMode.Brake);
_leftFollower.SetNeutralMode(NeutralMode.Brake);
#endif
if (FirstCall)
{
Debug.Print("Neutral Mode: Brake");
}
}
if (RampRate)
{
_rightTalon.ConfigOpenloopRamp(3, 0);
_leftTalon.ConfigOpenloopRamp(3, 0);
if (FirstCall)
{
Debug.Print("Ramp Rate: On, 3 Seconds");
}
}
else
{
_rightTalon.ConfigOpenloopRamp(0.0f, 0);
_leftTalon.ConfigOpenloopRamp(0.0f, 0);
if (FirstCall)
{
Debug.Print("Ramp Rate: Off, 0 Seconds");
}
}
/* Use Arbitrary FeedForward to create an Arcade Drive Control by modifying the forward output */
_rightTalon.Set(ControlMode.PercentOutput, forward, DemandType.ArbitraryFeedForward, -turn);
_leftTalon.Set(ControlMode.PercentOutput, forward, DemandType.ArbitraryFeedForward, +turn);
if (FirstCall)
{
Debug.Print("");
}
FirstCall = false;
Thread.Sleep(5);
}
}
