///////////////////////// Function to Stop All Motors /////////////////////////
static void StopAllMotors()
{
// Stopping Each Individual Motor
frontMotor.Set(0.0f);
backRightMotor.Set(0.0f);
backLeftMotor.Set(0.0f);
}
/**
* Zero the sensor and zero the throttle.
*/
void ZeroSensorAndThrottle()
{
_talon.SetPosition(0); /* start our position at zero, this example uses relative positions */
_targetPosition = 0;
/* zero throttle */
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
_talon.Set(0);
Thread.Sleep(100); /* wait a bit to make sure the Setposition() above takes effect before sampling */
}
void Drive()
{
FillBtns(ref _btns);
float y = -1 * _gamepad.GetAxis(1);
Deadband(ref y);
_talon.ProcessMotionProfileBuffer();
/* button handler, if btn5 pressed launch MP, if btn7 pressed, enter voltage mode */
if (_btns[5] && !_btnsLast[5])
{
/* disable MP to clear IsLast */
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kMotionProfile);
_talon.Set(0);
CTRE.Watchdog.Feed();
Thread.Sleep(10);
/* buffer new pts in HERO */
CTRE.TalonSrx.TrajectoryPoint point = new CTRE.TalonSrx.TrajectoryPoint();
_talon.ClearMotionProfileHasUnderrun();
_talon.ClearMotionProfileTrajectories();
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
point.position = (float)MotionProfile.Points[i][0];
point.velocity = (float)MotionProfile.Points[i][1];
point.timeDurMs = MotionProfile.kDurationMs;
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
point.velocityOnly = false;
point.profileSlotSelect = 0;
_talon.PushMotionProfileTrajectory(point);
}
/* send the first few pts to Talon */
for (int i = 0; i < 5; ++i)
{
CTRE.Watchdog.Feed();
Thread.Sleep(10);
_talon.ProcessMotionProfileBuffer();
}
/*start MP */
_talon.Set(1);
}
else if (_btns[7] && !_btnsLast[7])
{
_talon.SetControlMode(CTRE.TalonSrx.ControlMode.kVoltage);
}
/* if in voltage mode, update output voltage */
if (_talon.GetControlMode() == CTRE.TalonSrx.ControlMode.kVoltage)
{
_talon.Set(14.0f * y);
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
/** spin in this routine forever */
public void RunForever()
{
/* config our talon, don't continue until it's successful */
int initStatus = SetupConfig(); /* configuration */
while (initStatus != 0)
{
Instrument.PrintConfigError();
initStatus = SetupConfig(); /* (re)config*/
}
/* robot loop */
while (true)
{
/* get joystick params */
float leftY = -1f * _gamepad.GetAxis(1);
bool btnTopLeftShoulder = _gamepad.GetButton(5);
bool btnBtmLeftShoulder = _gamepad.GetButton(7);
Deadband(ref leftY);
/* keep robot enabled if gamepad is connected and in 'D' mode */
if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
/* set the control mode based on button pressed */
if (btnTopLeftShoulder)
{
_mode = CTRE.TalonSrx.ControlMode.kPercentVbus;
}
if (btnBtmLeftShoulder)
{
_mode = CTRE.TalonSrx.ControlMode.kMotionMagic;
}
/* calc the Talon output based on mode */
if (_mode == CTRE.TalonSrx.ControlMode.kPercentVbus)
{
float output = leftY; // [-1, +1] percent duty cycle
_talon.SetControlMode(_mode);
_talon.Set(output);
}
else if (_mode == CTRE.TalonSrx.ControlMode.kMotionMagic)
{
float servoToRotation = leftY * 10;// [-10, +10] rotations
_talon.SetControlMode(_mode);
_talon.Set(servoToRotation);
}
/* instrumentation */
Instrument.Process(_talon);
/* wait a bit */
System.Threading.Thread.Sleep(5);
}
}
static void Drive()
{
if (null == _gamepad)
{
_gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance());
}
float x = _gamepad.GetAxis(0);
float y = -1 * _gamepad.GetAxis(1);
float twist = _gamepad.GetAxis(2);
Deadband(ref x);
Deadband(ref y);
Deadband(ref twist);
float leftThrot = y + twist;
float rightThrot = y - twist;
left.Set(leftThrot);
leftSlave.Set(leftThrot);
right.Set(-rightThrot);
rightSlave.Set(-rightThrot);
stringBuilder.Append("\t");
stringBuilder.Append(x);
stringBuilder.Append("\t");
stringBuilder.Append(y);
stringBuilder.Append("\t");
stringBuilder.Append(twist);
}
static void Drive()
{
float x = _gamepad.GetAxis(0);
float y = -1 * _gamepad.GetAxis(1);
float twist = _gamepad.GetAxis(2);
Deadband(ref x);
Deadband(ref y);
Deadband(ref twist);
float leftThrot = y + twist;
float rightThrot = y - twist;
left.Set(leftThrot);
leftSlave.Set(leftThrot);
right.Set(-rightThrot);
rightSlave.Set(-rightThrot);
stringBuilder.Append("\t");
stringBuilder.Append(x);
stringBuilder.Append("\t");
stringBuilder.Append(y);
stringBuilder.Append("\t");
stringBuilder.Append(twist);
}
//I think this is where the magic happens and the motors move
private static void processInboundData(ArrayList setpointDataList, ArrayList talons)
{
//For each setpointData, for the talon that matches it set each mode and setpoint based on the list information
for (int i = 0; i < setpointDataList.Count; i++)
{
try
{
SetpointData setpointData = (SetpointData)setpointDataList[i];
float setpointVal = (float)(setpointData.getSetpoint());
CTRE.TalonSrx talon = (CTRE.TalonSrx)talons[i];
if (talon.GetControlMode() != setpointData.getMode())
{
talon.SetControlMode(setpointData.getMode());
//Debug.Print(setpointData.getMode().ToString());
}
if (talon.GetSetpoint() != setpointVal)
{
talon.Set(setpointVal);
Debug.Print("Setting it to value = " + setpointVal.ToString());
//Debug.Print(setpointVal.ToString());
}
}
catch (ArgumentOutOfRangeException ex)
{
Debug.Print(ex.ToString());
}
}
}
public static void Main()
{
/* create a gamepad object */
CTRE.Gamepad myGamepad = new CTRE.Gamepad(new CTRE.UsbHostDevice());
/* create a talon, the Talon Device ID in HERO LifeBoat is zero */
CTRE.TalonSrx myTalon = new CTRE.TalonSrx(0);
/* simple counter to print and watch using the debugger */
int counter = 0;
/* loop forever */
while (true)
{
/* added inside the while loop */
if (myGamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
/* print the axis value */
Debug.Print("axis:" + myGamepad.GetAxis(1));
/* pass axis value to talon */
myTalon.Set(myGamepad.GetAxis(1));
/* allow motor control */
CTRE.Watchdog.Feed();
}
/* increment counter */
++counter; /* try to land a breakpoint here */
/* wait a bit */
System.Threading.Thread.Sleep(10);
}
}
public override Int32 Run(ZSDK.Gamepad gamepad)
{
Single LeftVal = 0, RightVal = 0;
Boolean Slow = true;
ReadGamepad(gamepad, ref LeftVal, ref RightVal, ref Slow);
#if DEBUG
Debug.Print(ToString() + " [RAW] L:" + LeftVal.ToString() + " - R:" + RightVal.ToString());
#endif
/* Apply RPM conversion to values. */
if (USE_SPEED_MODE)
{
LeftVal *= MAX_RPM;
RightVal *= MAX_RPM;
}
/* Drive in slow mode. */
if (Slow)
{
LeftVal *= SLOW_MULT;
RightVal *= SLOW_MULT;
}
#if DEBUG
Debug.Print(ToString() + " [SET] L:" + LeftVal.ToString() + " - R:" + RightVal.ToString());
#endif
Right.Set(RightVal);
Left.Set(LeftVal);
return(0);
}
static void ArcadeDrive(float moveValue, float rotateValue)
{
float leftMotorSpeed;
float rightMotorSpeed;
rotateValue = Limit(rotateValue);
moveValue = Limit(moveValue);
// Square values for smoothness
if (rotateValue >= 0.0f)
{
rotateValue = rotateValue * rotateValue;
}
else
{
rotateValue = -(rotateValue * rotateValue);
}
if (moveValue >= 0.0f)
{
moveValue = moveValue * moveValue;
}
else
{
moveValue = -(moveValue * moveValue);
}
if (rotateValue > 0.0)
{
if (moveValue > 0.0)
{
leftMotorSpeed = rotateValue - moveValue;
rightMotorSpeed = (float)System.Math.Max(rotateValue, moveValue);
}
else
{
leftMotorSpeed = (float)System.Math.Max(rotateValue, -moveValue);
rightMotorSpeed = rotateValue + moveValue;
}
}
else
{
if (moveValue > 0.0)
{
leftMotorSpeed = -(float)System.Math.Max(-rotateValue, moveValue);
rightMotorSpeed = rotateValue + moveValue;
}
else
{
leftMotorSpeed = rotateValue - moveValue;
rightMotorSpeed = -(float)System.Math.Max(-rotateValue, -moveValue);
}
}
//left.Set(leftMotorSpeed);
//leftSlave.Set(leftMotorSpeed);
right.Set(rightMotorSpeed);
rightSlave.Set(rightMotorSpeed);
}
public static void CheesyDrive(float throttle, float steer)
{
float coeff = 1.0f;
float steeringComponent = steer * TURNING_CONSTANT;
float throttleComponent = throttle * coeff;
leftDrive.Set(throttleComponent + steeringComponent);
rightDrive.Set(throttleComponent - steeringComponent);
}
public static void Main()
{
CTRE.Gamepad _gamepad = new CTRE.Gamepad(new CTRE.UsbHostDevice());
//0 - left x
//1 - left y
//2 - right x
//
//5 - LB
//6 - RB
ledSpike.SetControlMode(CTRE.TalonSrx.ControlMode.kVoltage);
ledSpike.SetCurrentLimit(1);
while (true)
{
float y = -_gamepad.GetAxis(1);
float turn = _gamepad.GetAxis(2);
Deadband(ref y);
Deadband(ref turn);
if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
if (_gamepad.GetButton(5))
{
ArcadeDrive(y, turn);
/* feed watchdog to keep Talon's enabled */
CTRE.Watchdog.Feed();
}
if (_gamepad.GetButton(6))
{
ledSpike.Set(3.5f);
CTRE.Watchdog.Feed();
}
else
{
ledSpike.Set(0);
}
}
/* run this task every 10ms */
Thread.Sleep(10);
}
}
static void Drive()
{
float y = _gamepad.GetAxis(1);
float twist = _gamepad.GetAxis(4);
Deadband(ref y);
Deadband(ref twist);
float leftThrot = y + twist;
float rightThrot = y - twist;
left1.Set(leftThrot);
right1.Set(-rightThrot);
stringBuilder.Append("\t");
stringBuilder.Append(y);
stringBuilder.Append("\t");
stringBuilder.Append(twist);
}
private static void processInboundData(ArrayList setpointDataList, ArrayList talons)
{
for (int i = 0; i < setpointDataList.Count; i++)
{
SetpointData setpointData = (SetpointData)setpointDataList[i];
float setpointVal = (float)(setpointData.getSetpoint());
CTRE.TalonSrx talon = (CTRE.TalonSrx)talons[i];
if (talon.GetControlMode() != setpointData.getMode())
{
talon.SetControlMode(setpointData.getMode());
Debug.Print(setpointData.getMode().ToString());
}
if (talon.GetSetpoint() != setpointVal)
{
talon.Set(setpointVal);
Debug.Print(setpointVal.ToString());
}
}
}
static void Run()
{
float x = _gamepad.GetAxis(1);
Deadband(ref x);
//Set the Maximum Current Limit for the Talon (in Amps)
talon.SetCurrentLimit(10);
//Enable the Current Limiting Feature.
talon.EnableCurrentLimit(true);
talon.Set(x);
float current = talon.GetOutputCurrent();
stringBuilder.Append("\t");
stringBuilder.Append(current);
stringBuilder.Append("\t");
}
static void Drive()
{
if (null == _gamepad)
{
_gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance());
}
float x = _gamepad.GetAxis(0); // Positive is strafe-right, negative is strafe-left
float y = -1 * _gamepad.GetAxis(1); // Positive is forward, negative is reverse
float turn = _gamepad.GetAxis(2); // Positive is turn-right, negative is turn-left
Deadband(ref x);
Deadband(ref y);
Deadband(ref turn);
float leftFrnt_throt = y + x + turn; // left front moves positive for forward, strafe-right, turn-right
float leftRear_throt = y - x + turn; // left rear moves positive for forward, strafe-left, turn-right
float rghtFrnt_throt = y - x - turn; // right front moves positive for forward, strafe-left, turn-left
float rghtRear_throt = y + x - turn; // right rear moves positive for forward, strafe-right, turn-left
/* normalize here, there a many way to accomplish this, this is a simple solution */
Normalize(ref leftFrnt_throt);
Normalize(ref leftRear_throt);
Normalize(ref rghtFrnt_throt);
Normalize(ref rghtRear_throt);
/* everything up until this point assumes positive spins motor so that robot moves forward.
* But typically one side of the robot has to drive negative (red LED) to move robor forward.
* Assuming the left-side has to be negative to move robot forward, flip the left side */
leftFrnt_throt *= -1;
leftRear_throt *= -1;
leftFrnt.Set(leftFrnt_throt);
leftRear.Set(leftRear_throt);
rghtFrnt.Set(rghtFrnt_throt);
rghtRear.Set(rghtRear_throt);
}
public void RunForever()
{
/* enable XInput, if gamepad is in DInput it will disable robot. This way you can
* use X mode for drive, and D mode for disable (instead of vice versa as the
* stock HERO implementation traditionally does). */
CTRE.UsbHostDevice.EnableXInputDevices();
while (true)
{
if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected)
{
CTRE.Watchdog.Feed();
}
/* get buttons */
bool[] btns = new bool[_buttons.Length];
for (uint i = 1; i < 20; ++i)
{
btns[i] = _gamepad.GetButton(i);
}
/* get sticks */
for (uint i = 0; i < _sticks.Length; ++i)
{
_sticks[i] = _gamepad.GetAxis(i);
}
/* yield for a bit, and track timeouts */
System.Threading.Thread.Sleep(10);
if (_rumblingTimeMs < 5000)
{
_rumblingTimeMs += 10;
}
/* update the Talon using the shoulder analog triggers */
_tal.Set((_sticks[5] - _sticks[4]) * 0.60f);
/* fire some solenoids based on buttons */
_driver.Set(0, _buttons[1]);
_driver.Set(1, _buttons[2]);
_driver.Set(2, _buttons[3]);
_driver.Set(3, _buttons[4]);
/* rumble state machine */
switch (_rumblinSt)
{
/* rumbling is disabled, require some off time to save battery */
case 0:
_gamepad.SetLeftRumble(0);
_gamepad.SetRightRumble(0);
if (_rumblingTimeMs < 100)
{
/* waiting for off-time */
}
else if ((btns[1] && !_buttons[1])) /* button off => on */
{
/* off time long enough, user pressed btn */
_rumblingTimeMs = 0;
_rumblinSt = 1;
_gamepad.SetLeftRumble(0xFF);
}
else if ((btns[2] && !_buttons[2])) /* button off => on */
{
/* off time long enough, user pressed btn */
_rumblingTimeMs = 0;
_rumblinSt = 1;
_gamepad.SetRightRumble(0xFF);
}
break;
/* already vibrating, track the time */
case 1:
if (_rumblingTimeMs > 500)
{
/* vibrating too long, turn off now */
_rumblingTimeMs = 0;
_rumblinSt = 0;
_gamepad.SetLeftRumble(0);
_gamepad.SetRightRumble(0);
}
else if ((btns[3] && !_buttons[3])) /* button off => on */
{
/* immedietely turn off */
_rumblingTimeMs = 0;
_rumblinSt = 0;
_gamepad.SetLeftRumble(0);
_gamepad.SetRightRumble(0);
}
else if ((btns[1] && !_buttons[1])) /* button off => on */
{
_gamepad.SetLeftRumble(0xFF);
}
else if ((btns[2] && !_buttons[2])) /* button off => on */
{
_gamepad.SetRightRumble(0xFF);
}
break;
}
/* this will likley be replaced with a strongly typed interface,
* control the LEDs on the center XBOX emblem. */
if (btns[5] && !_buttons[5])
{
_gamepad.SetLEDCode(6);
}
if (btns[6] && !_buttons[6])
{
_gamepad.SetLEDCode(7);
}
if (btns[7] && !_buttons[7])
{
_gamepad.SetLEDCode(8);
}
if (btns[8] && !_buttons[8])
{
_gamepad.SetLEDCode(9);
}
/* build line to print */
StringBuilder sb = new StringBuilder();
foreach (float stick in _sticks)
{
sb.Append(Format(stick));
sb.Append(",");
}
sb.Append("-");
for (uint i = 1; i < _buttons.Length; ++i)
{
if (_buttons[i])
{
sb.Append("b" + i + ",");
}
}
/* print useful info */
sb.AppendLine();
Debug.Print(sb.ToString());
/* save button states for button-change states */
_buttons = btns;
}
}
public override Int32 Run(ZSDK.Gamepad gamepad)
{
Boolean Fire = false;
if (gamepad is ZSDK.LogitechGamepad)
{
if ((gamepad as ZSDK.LogitechGamepad).Button_A)
{
Fire = true; Mode = FireMode.Normal;
}
else if ((gamepad as ZSDK.LogitechGamepad).Button_Y)
{
Fire = true; Mode = FireMode.NoReset;
}
}
else
{
if (gamepad.Buttons[0])
{
Fire = true; Mode = FireMode.Normal;
}
else if (gamepad.Buttons[1])
{
Fire = true; Mode = FireMode.NoReset;
}
}
if (!Winch.GetForwardLimitOK() || !Winch.GetReverseLimitOK())
{
Winch.Set(0);
}
if (!Dog.GetForwardLimitOK() || !Dog.GetReverseLimitOK())
{
Dog.Set(0);
}
#if DEBUG
Debug.Print(ToString() + " [STATE] " + State.ToString());
#endif
switch (State)
{
default:
case BallLaunchState.Unknown:
// State 0
// Assume we are started Released
Winch.Set(0);
Dog.Set(0);
State = BallLaunchState.Released;
break;
case BallLaunchState.Loaded:
// State 1
Winch.Set(0);
Dog.Set(0);
if (Fire)
{
// Run Dog TalonSRX to fire.
Dog.Set(DOG_FIRE_DIRECTION);
State = BallLaunchState.Releasing;
}
break;
case BallLaunchState.Loading:
// State 2
// Start the winch.
// Check for winch limit switch.
Boolean WinchLimitOK = Winch.GetReverseLimitOK();
if (!WinchLimitOK)
{
Winch.Set(0);
State = BallLaunchState.Loaded;
}
else
{
Boolean WinchStart = DateTime.Now.Subtract(WinchStartTime).Ticks <= WINCH_START_WAIT;
Single WinchSpeed = WinchStart ? WINCH_DIRECTION_START : WINCH_DIRECTION_END;
#if DEBUG
Debug.Print(ToString() + " [WINCH SPEED] " + WinchSpeed.ToString());
#endif
Winch.Set(WinchSpeed);
}
break;
case BallLaunchState.Released:
// State 3
// Reset the Dog Gear.
// Check for dog limit switch.
Boolean DogLimitOK = Dog.GetReverseLimitOK();
if (!DogLimitOK)
{
Dog.Set(0);
switch (Mode)
{
default:
case FireMode.Normal:
State = BallLaunchState.Loading;
break;
case FireMode.NoReset:
State = BallLaunchState.Loaded;
break;
}
WinchStartTime = DateTime.Now;
}
else
{
Dog.Set(DOG_RESET_DIRECTION);
}
break;
case BallLaunchState.Releasing:
// State 4
// Wait for a complete fire.
Boolean DogFireLimitOK = Dog.GetForwardLimitOK();
if (!DogFireLimitOK)
{
FireTime = DateTime.Now;
State = BallLaunchState.Reseting;
}
break;
case BallLaunchState.Reseting:
Int64 fire_ticks = DateTime.Now.Subtract(FireTime).Ticks;
if (fire_ticks >= FIRE_WAIT)
{
State = BallLaunchState.Released;
}
break;
}
return(0);
}
