public static void Main()
{
/* Get the Joystick values from the CANbus. */
GameController _gamepad = new GameController(new CANJoystickSource(0));
TalonSRX leftTalon = new TalonSRX(10);
TalonSRX rightTalon = new TalonSRX(11);
/* Factory Default all hardware to prevent unexpected behaviour */
leftTalon.ConfigFactoryDefault();
rightTalon.ConfigFactoryDefault();
/* Set Inverted on one side so "forward" relative to the robot is a Green LED state */
leftTalon.SetInverted(false);
rightTalon.SetInverted(true);
/* loop forever */
while (true)
{
/* Basic Drivetrain, negate the axes so forward is positive */
leftTalon.Set(ControlMode.PercentOutput, _gamepad.GetAxis(1) * -1);
rightTalon.Set(ControlMode.PercentOutput, _gamepad.GetAxis(3) * -1);
/* wait a bit */
Thread.Sleep(10);
/* Normally we would need to feed the Watchdog here.
* We're relying on the roboRIO to provide the enable
* signal, so it's not necessary.
**/
}
}
private Intake()
{
Robotmap map = Robotmap.GetInstance();
intakeMotor = new TalonSRX(map.GetIntake_ID()); // Creates the TalonSRX motor.
intakeMotor.SetInverted(true); // If you get value i will invert it. if it is going the wrong way it can be inverted.
m_currentState = INTAKESTATE.Off;
}
private FlagElevator()
{
elevatorMotor = new TalonSRX(ELEVATOR_CAN_ID);
elevatorMotor.SetInverted(true); //can change if the motor is mounted the wrong way.
m_currentState = ELEVATORSTATE.StoppedLocation;
}
private void robotInit()
{
js0 = new GameController(UsbHostDevice.GetInstance());
leftSlave.Follow(leftMaster);
rightSlave.Follow(rightMaster);
shooterWheelSlave.Follow(shooterWheelMaster);
rightMaster.SetInverted(true);
rightSlave.SetInverted(true);
bottomIntake.SetInverted(true);
}
public DriveBase()
{
const int MAX_CURRENT = 10;
const int TIMEOUT_MS = 150;
FrontLeft.ConfigContinuousCurrentLimit(MAX_CURRENT, TIMEOUT_MS);
FrontRight.ConfigContinuousCurrentLimit(MAX_CURRENT, TIMEOUT_MS);
BackLeft.ConfigContinuousCurrentLimit(MAX_CURRENT, TIMEOUT_MS);
BackRight.ConfigContinuousCurrentLimit(MAX_CURRENT, TIMEOUT_MS);
FrontRight.SetInverted(true);
BackRight.SetInverted(true);
}
//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 void Setup()
{
TalonSRX armTalon = (TalonSRX)_gearBox.MasterMotorController;
armTalon.SetInverted(false);
armTalon.SetStatusFramePeriod(StatusFrame.Status_1_General_, 10); //Send updates every 10ms instead of 10ms
armTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 1);
armTalon.SetSensorPhase(true); //reversed sensor
armTalon.ConfigForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyClosed);
armTalon.ConfigReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyClosed);
armTalon.ConfigClearPositionOnLimitR(false, 10); //enable on reverse limit
armTalon.ConfigClearPositionOnLimitF(false, 10);
}
private void robotInit()
{
js0 = new GameController(UsbHostDevice.GetInstance());
leftSlave1.Follow(leftMaster);
leftSlave2.Follow(leftMaster);
rightSlave1.Follow(rightMaster);
rightSlave2.Follow(rightMaster);
intake2.Follow(intake1);
rightMaster.SetInverted(true);
rightSlave1.SetInverted(true);
rightSlave2.SetInverted(true);
}
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 Transfer()
{
Robotmap map = Robotmap.GetInstance();
transferMotor = new TalonSRX(map.GetTransfer_ID()); //Creates the motor
transferMotor.SetInverted(true);
CANController = Robotmap.GETCANController(); //Creates the first sensor
numberOfBalls = 0; //The original amount of balls
glowing = LED.GetInstance();
m_currentState = TRANSFER_STATE.TRANSFER_OFF;
eject = Intake.GetInstance();
}
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();
}
//Excavation
public static TalonSRX CreateLinearActuator(int CAN_ID)
{
TalonSRX talon = new TalonSRX(CAN_ID);
talon.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog);
talon.ConfigFeedbackNotContinuous(true, 100);
talon.SetInverted(true);
talon.SetSensorPhase(true);
talon.ConfigPeakCurrentLimit(6);
talon.ConfigPeakCurrentDuration(150);
talon.ConfigContinuousCurrentLimit(3);
// talon.SetStatusFramePeriod(StatusFrame.Status_1_General_, 25); // Sets encoder value feedback rate -TODO
return(talon);
}
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 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);
}
}
static void Initialize()
{
// Serial port
_uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200);
_uart.Open();
// Victor SPX Slaves
// Left Slave
victor1.Set(ControlMode.Follower, 0);
// Right Slave
victor3.Set(ControlMode.Follower, 2);
// Talon SRX Slaves
// Feeder Slave
feederL.Set(ControlMode.Follower, 0);
feederL.SetInverted(true);
// Intake Slave
intakeLft.Set(ControlMode.Follower, 2);
intakeLft.SetInverted(true);
// Hood
hood.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog, 0, kTimeoutMs);
hood.SetSensorPhase(false);
hood.Config_kP(0, 30f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */
hood.Config_kI(0, 0.0005f, kTimeoutMs);
hood.Config_kD(0, 0f, kTimeoutMs);
hood.Config_kF(0, 0f, kTimeoutMs);
/* use slot0 for closed-looping */
hood.SelectProfileSlot(0, 0);
/* set the peak and nominal outputs, 1.0 means full */
hood.ConfigNominalOutputForward(0.0f, kTimeoutMs);
hood.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
hood.ConfigPeakOutputForward(+1.0f, kTimeoutMs);
hood.ConfigPeakOutputReverse(-1.0f, kTimeoutMs);
hood.ConfigForwardSoftLimitThreshold(HOOD_LOWER_BOUND_ANALOG, kTimeoutMs);
hood.ConfigReverseSoftLimitThreshold(HOOD_UPPER_BOUND_ANALOG, kTimeoutMs);
hood.ConfigForwardSoftLimitEnable(true, kTimeoutMs);
hood.ConfigReverseSoftLimitEnable(true, kTimeoutMs);
/* how much error is allowed? This defaults to 0. */
hood.ConfigAllowableClosedloopError(0, 5, kTimeoutMs);
//***********************
// MAY NEED TUNING
//***********************
// Turret
turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 1, kTimeoutMs);
int absPos = turret.GetSelectedSensorPosition(1);
turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
turret.SetSelectedSensorPosition(0, 0, kTimeoutMs);
turret.SetSensorPhase(true);
turret.Config_IntegralZone(20);
turret.Config_kP(0, 2f, kTimeoutMs); // tweak this first, a little bit of overshoot is okay
turret.Config_kI(0, 0f, kTimeoutMs);
turret.Config_kD(0, 0f, kTimeoutMs);
turret.Config_kF(0, 0f, kTimeoutMs);
// use slot0 for closed-looping
turret.SelectProfileSlot(0, 0);
// set the peak and nominal outputs, 1.0 means full
turret.ConfigNominalOutputForward(0.0f, kTimeoutMs);
turret.ConfigNominalOutputReverse(0.0f, kTimeoutMs);
turret.ConfigPeakOutputForward(+0.5f, kTimeoutMs);
turret.ConfigPeakOutputReverse(-0.5f, kTimeoutMs);
turret.ConfigReverseSoftLimitThreshold(TURRET_UPPER_BOUND_ANALOG, kTimeoutMs);
turret.ConfigForwardSoftLimitThreshold(TURRET_LOWER_BOUND_ANALOG, kTimeoutMs);
turret.ConfigReverseSoftLimitEnable(true, kTimeoutMs);
turret.ConfigForwardSoftLimitEnable(true, kTimeoutMs);
// how much error is allowed? This defaults to 0.
turret.ConfigAllowableClosedloopError(0, 5, kTimeoutMs);
// Shooter
shooterSensorTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs);
shooterSensorTalon.SetSensorPhase(false);
shooterSensorTalon.ConfigPeakOutputReverse(0.0f, kTimeoutMs);
shooterSensorTalon.ConfigPeakOutputForward(1.0f, kTimeoutMs);
shooterVESC = new PWMSpeedController(CTRE.HERO.IO.Port3.PWM_Pin7);
shooterVESC.Set(0);
pcm.SetSolenoidOutput(shooterFeedbackLEDPort, false);
}
