private void RunBelts(ref XboxController controller)
{
double SPEED = 1;
if (controller.BUTTONS.RB)
{
SPEED = -.7;
}
if (controller.BUTTONS.A)
{
ExcavationBelt.Set(ControlMode.PercentOutput, SPEED);
}
else
{
ExcavationBelt.Set(ControlMode.PercentOutput, 0);
}
if (controller.BUTTONS.Y)
{
CollectionBelt.Set(ControlMode.PercentOutput, SPEED);
}
else
{
CollectionBelt.Set(ControlMode.PercentOutput, 0);
}
}
/**
* Mecanum Drive that is inverted on the left side and decreases output when low battery
*
* @param Forward Forward/Backward drive of mecanum drive
* @param Strafe Left/Right drive of mecanum drive
* @param Twist Turn left/Right of mecanum drive
*/
private static void MecanumDrive(float Forward, float Strafe, float Twist)
{
float leftFrnt = (Forward + Strafe + Twist); // left front moves positive for forward, strafe-right, turn-right
float leftRear = (Forward - Strafe + Twist); // left rear moves positive for forward, strafe-left, turn-right
float rghtFrnt = (Forward - Strafe - Twist); // right front moves positive for forward, strafe-left, turn-left
float rghtRear = (Forward + Strafe - Twist); // right rear moves positive for forward, strafe-right, turn-left
/* Invert left sided motors */
leftFrnt *= -1;
leftRear *= -1;
/* If battery is lower than 10% scale down output */
if (_Battery.IsLow())
{
leftFrnt *= 0.5f;
leftRear *= 0.5f;
rghtFrnt *= 0.5f;
rghtRear *= 0.5f;
}
/* Feed Talons */
RightFront.Set(ControlMode.PercentOutput, rghtFrnt);
RightRear.Set(ControlMode.PercentOutput, rghtRear);
LeftFront.Set(ControlMode.PercentOutput, leftFrnt);
LeftRear.Set(ControlMode.PercentOutput, leftRear);
}
void Loop10Ms()
{
/* get all the buttons */
FillBtns(ref _btns);
/* scale the x-axis to go from 0 to sensorRange, left to right */
int leftAxisX = (int)(((_sensorRange / 2) * _gamepad.GetAxis(0)) + (_sensorRange / 2));
float rightAxisX = kJoystickScaler * _gamepad.GetAxis(2);
Deadband(ref rightAxisX);
if (rightAxisX != 0)
{
_talon.Set(ControlMode.PercentOutput, rightAxisX);
}
else if (_talon.GetControlMode() == ControlMode.PercentOutput)
{
_targetPosition = _talon.GetSelectedSensorPosition(0);
/* user has let go of the stick, lets closed-loop whereever we happen to be */
EnableClosedLoop();
}
/* When you press the 'A' button on a Logitech Gamepad
* and the enable button is pressed */
if (_btns[2] && !_btnsLast[2] && _gamepad.GetButton(kEnableButton))
{
_targetPosition = servo(leftAxisX, _talon.GetSelectedSensorPosition(0), _sensorRange);
EnableClosedLoop();
}
}
private void Start() //Sets motor speed
{
//Lower sensor says when a ball passes through a the tube and incruments the ball
//upper sensor tells us tha twe have balls in the tube.
Robotmap map = Robotmap.GetInstance();
transferMotor.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, transferMotorSpeed);
bool ballPresent = IsLowerSensorTripped();
if (ballPresent)
{
numberOfBalls += 1;
}
bool ballPresentTop = IsUpperSensorTripped();
if (!ballPresentTop && numberOfBalls > 0)
{
numberOfBalls = 0;
}
if (numberOfBalls == 0)
{
glowing.set_color(LED.MECHANISM_LED.GREEN);
eject.setState(Intake.INTAKESTATE.Sweep);
}
else if (numberOfBalls == 1)
{
glowing.set_color(LED.MECHANISM_LED.YELLOW);
eject.setState(Intake.INTAKESTATE.Sweep);
}
else
{
glowing.set_color(LED.MECHANISM_LED.RED);
eject.setState(Intake.INTAKESTATE.Expel);
}
}
private void RunActuators(ref XboxController controller)
{
//Update Encoders
Utils.Print("R, L CURRENT");
Utils.Print(OUTPUT);
Utils.Print(RightActuator.GetSelectedSensorPosition());
Utils.Print("");
//Run Motors
if (controller.POV == controller.POV_UP)
{
OUTPUT = 300;
}
else if (controller.POV == controller.POV_DOWN)
{
OUTPUT = 800;
}
//if ((OUTPUT < RightActuator.GetSelectedSensorPosition() && RightActuator.GetSelectedSensorVelocity() > 1)
// || (OUTPUT > RightActuator.GetSelectedSensorPosition() && RightActuator.GetSelectedSensorVelocity() < 1))
//{
// LeftActuator.Set(ControlMode.PercentOutput, 0);
// RightActuator.Set(ControlMode.PercentOutput, 0);
//}
//else
//{
LeftActuator.Set(ControlMode.MotionMagic, OUTPUT);
RightActuator.Set(ControlMode.MotionMagic, OUTPUT);
//}
}
public void Motion()
{
if (buttonLeft == true && modeSwitch == false)
{
modeSwitch = true;
if (motionMode == ControlMode.PercentOutput)
{
motionMode = ControlMode.MotionMagic;
}
else
{
motionMode = ControlMode.PercentOutput;
}
}
if (buttonLeft == false && modeSwitch == true)
{
modeSwitch = false;
}
if (motionMode == ControlMode.PercentOutput)
{
output = 0;
talon0.Set(motionMode, yAxisA);
}
else if (motionMode == ControlMode.MotionMagic)
{
output = yAxisA * 4096 * analog0;
MotionToggle();
MotionSave();
MotionOutput();
talon0.Set(motionMode, output);
}
Thread.Sleep(10);
}
/**
* Zero the sensor and zero the throttle.
*/
void ZeroSensorAndThrottle()
{
_talon.SetSelectedSensorPosition(0, kTimeoutMs); /* start our position at zero, this example uses relative positions */
_targetPosition = 0;
/* zero throttle */
_talon.Set(ControlMode.PercentOutput, 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 percent output mode */
if (_btns[5] && !_btnsLast[5])
{
/* disable MP to clear IsLast */
_talon.Set(ControlMode.MotionProfile, 0);
Watchdog.Feed();
Thread.Sleep(10);
/* buffer new pts in HERO */
TrajectoryPoint point = new TrajectoryPoint();
_talon.ClearMotionProfileHasUnderrun();
_talon.ClearMotionProfileTrajectories();
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
point.position = (float)MotionProfile.Points[i][0] * kTicksPerRotation; //convert from rotations to sensor units
point.velocity = (float)MotionProfile.Points[i][1] * kTicksPerRotation / 600; //convert from RPM to sensor units per 100 ms.
point.headingDeg = 0; //not used in this example
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
point.profileSlotSelect0 = 0;
point.profileSlotSelect1 = 0; //not used in this example
point.timeDur = TrajectoryPoint.TrajectoryDuration.TrajectoryDuration_10ms;
_talon.PushMotionProfileTrajectory(point);
}
/* send the first few pts to Talon */
for (int i = 0; i < 5; ++i)
{
Watchdog.Feed();
Thread.Sleep(10);
_talon.ProcessMotionProfileBuffer();
}
/*start MP */
_talon.Set(ControlMode.MotionProfile, 1);
}
else if (_btns[7] && !_btnsLast[7])
{
_talon.Set(ControlMode.PercentOutput, 0);
}
/* if not in motion profile mode, update output percent */
if (_talon.GetControlMode() != ControlMode.MotionProfile)
{
_talon.Set(ControlMode.PercentOutput, y);
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
public void runIntake()
{
if (js0.GetButton(RobotMap.LEFT_TRIGGER))
{
bottomIntake.Set(ControlMode.PercentOutput, RobotMap.INTAKE_SPEED);
}
else
{
bottomIntake.Set(ControlMode.PercentOutput, 0);
}
}
static void TargetLight()
{
if (!ledFlashActive)
{
if (gamepad.GetButton((uint)EBut.LT))
{
shooterSensorTalon.Set(ControlMode.PercentOutput, 1.0);
}
else
{
shooterSensorTalon.Set(ControlMode.PercentOutput, 0.0);
}
}
}
private void enabledPeriodic()
{
// 2 - left and right, right stick
// 5 - up and down, right stick
float power = -1 * js0.GetAxis(RobotMap.DRIVE_AXIS);
// cubic function generated with ti regression model
// 4x^3 - 6x^2 + 3x
power = (float)(4 * Math.Pow(power, 3) - 6 * Math.Pow(power, 2) + 3 * power);
float rotate = -0.4f * js0.GetAxis(RobotMap.ROTATE_AXIS);
drivePercentage(power, rotate);
// Intake
// in / out
if (js0.GetButton(RobotMap.INTAKE_IN) && !js0.GetButton(RobotMap.INTAKE_OUT))
{
intake1.Set(ControlMode.PercentOutput, 0.60);
}
else if (!js0.GetButton(RobotMap.INTAKE_IN) && js0.GetButton(RobotMap.INTAKE_OUT))
{
intake1.Set(ControlMode.PercentOutput, -1);
}
else
{
intake1.Set(ControlMode.PercentOutput, 0);
}
// raise / lower
if (js0.GetButton(RobotMap.RAISE_INTAKE) && !js0.GetButton(RobotMap.LOWER_INTAKE))
{
raiseIntake();
}
else if (!js0.GetButton(RobotMap.RAISE_INTAKE) && js0.GetButton(RobotMap.LOWER_INTAKE))
{
lowerIntake();
}
// open / close
if (js0.GetButton(RobotMap.OPEN_INTAKE))
{
openIntake();
}
else
{
closeIntake();
}
}
public void ServoToSpeed(float speedRPM)
{
/* close loop constants */
_ServoParameters.P = 0.005f;
_ServoParameters.I = 0 * 0.0001f;
_ServoParameters.F = 6f / 2000f; // about 6V for 2000 RPM
/* save the target */
_targetSpeedRPM = speedRPM;
/* get measured speed */
float measuredSpeedRpm = MeasuredSpeed;
/* robot controller level closed loop, replace with firmware close loop later */
float output = _ServoParameters.PID(_targetSpeedRPM, measuredSpeedRpm, 0);
_tal.Set(ControlMode.PercentOutput, (output / 2));
}
public void IntakeRoller()
{
if (controller.GetLeftBumper())
{
intakeRoller.Set(ControlMode.PercentOutput, INTAKE_SPEED);
}
else if (controller.GetRightBumper())
{
intakeRoller.Set(ControlMode.PercentOutput, EJECT_SPEED);
}
else
{
intakeRoller.Set(ControlMode.PercentOutput, CONSTANT_INTAKE_SPEED);
}
}
static void Intake()
{
// Buttons are toggles
if (gamepad.GetButton((uint)EBut.LB))
{
intakeRgt.Set(ControlMode.PercentOutput, intakeSpeed);
setTurretAngle(0);
isIntaking = true;
}
else
{
intakeRgt.Set(ControlMode.PercentOutput, 0);
isIntaking = false;
}
}
static void Drive()
{
if (null == _gamepad)
{
_gamepad = new GameController(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(ControlMode.PercentOutput, leftThrot);
//leftSlave.Set(ControlMode.PercentOutput, leftThrot);
right.Set(ControlMode.PercentOutput, -rightThrot);
//rightSlave.Set(ControlMode.PercentOutput, -rightThrot);
stringBuilder.Append("\t");
stringBuilder.Append(x);
stringBuilder.Append("\t");
stringBuilder.Append(y);
stringBuilder.Append("\t");
stringBuilder.Append(twist);
}
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 void SetSpeeds(double L_Speed, double R_Speed)
{
FrontLeft.Set(ControlMode.PercentOutput, L_Speed);
FrontRight.Set(ControlMode.PercentOutput, R_Speed);
BackLeft.Set(ControlMode.PercentOutput, L_Speed);
BackRight.Set(ControlMode.PercentOutput, R_Speed);
}
static void Drive()
{
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(ControlMode.PercentOutput, leftFrnt_throt);
leftRear.Set(ControlMode.PercentOutput, leftRear_throt);
rghtFrnt.Set(ControlMode.PercentOutput, rghtFrnt_throt);
rghtRear.Set(ControlMode.PercentOutput, rghtRear_throt);
}
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;
}
static void setHoodAngle(float angle)
{
int sensorPos = (int)linRelate(angle, HOOD_LOWER_BOUND_ANGLE, HOOD_UPPER_BOUND_ANGLE,
HOOD_LOWER_BOUND_ANALOG, HOOD_UPPER_BOUND_ANALOG);
hood.Set(ControlMode.Position, sensorPos);
}
public void Climb(Joystick stick)
{
double max = 1 - stick.GetRawAxis(XboxMap.RightTrigger);
ClimberOne.Set((max) * stick.GetRawAxis(XboxMap.LeftTrigger));
ClimberTwo.Set((-max) * stick.GetRawAxis(XboxMap.LeftTrigger));
}
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(ControlMode.PercentOutput, leftThrot);
leftSlave.Set(ControlMode.PercentOutput, leftThrot);
right.Set(ControlMode.PercentOutput, -rightThrot);
rightSlave.Set(ControlMode.PercentOutput, -rightThrot);
stringBuilder.Append("\t");
stringBuilder.Append(x);
stringBuilder.Append("\t");
stringBuilder.Append(y);
stringBuilder.Append("\t");
stringBuilder.Append(twist);
}
public void IntakePivot()
{
double controllerOutput = Utilities.Limit(controller.GetRightTrigger() -
controller.GetLeftTrigger());
intakePivot.Set(ControlMode.PercentOutput, (float)controllerOutput * PIVOT_SPEED);
}
private void RunDigger(ref XboxController controller)
{
if (controller.BUTTONS.LT)
{
Digger.Set(ControlMode.PercentOutput, 1);
}
else if (controller.BUTTONS.RT)
{
Digger.Set(ControlMode.PercentOutput, -1);
}
else
{
Digger.Set(ControlMode.PercentOutput, 0);
}
}
/** spin in this routine forever */
public void RunForever()
{
SetupConfig(); /* configuration */
/* 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() == UsbDeviceConnection.Connected)
{
Watchdog.Feed();
}
/* set the control mode based on button pressed */
if (btnTopLeftShoulder)
{
_mode = ControlMode.PercentOutput;
}
if (btnBtmLeftShoulder)
{
_mode = ControlMode.MotionMagic;
}
/* calc the Talon output based on mode */
if (_mode == ControlMode.PercentOutput)
{
float output = leftY; // [-1, +1] percent duty cycle
_talon.Set(_mode, output);
}
else if (_mode == ControlMode.MotionMagic)
{
float servoToRotation = leftY * 10;// [-10, +10] rotations
_talon.Set(_mode, servoToRotation);
}
/* instrumentation */
Instrument.Process(_talon);
/* wait a bit */
System.Threading.Thread.Sleep(5);
}
}
private void MoveForSeconds(int s, double p, TalonSRX talon)
{
DateTime sTime = DateTime.UtcNow;
while (DateTime.UtcNow - startTime < TimeSpan.FromTicks(TimeSpan.TicksPerSecond * s))
{
talon.Set(CTRE.Phoenix.MotorControl.ControlMode.PercentOutput, p);
}
}
static void Feeder()
{
if (isIntaking)
{
feederR.Set(ControlMode.PercentOutput, -0.1);
}
else
{
// Buttons are toggles
if (gamepad.GetButton((uint)EBut.RB))
{
feederR.Set(ControlMode.PercentOutput, feederSpeed);
}
else
{
feederR.Set(ControlMode.PercentOutput, 0);
}
}
}
public void Drive()
{
double forward = Utilities.Limit(controller.GetLeftStickY()) * DRIVE_FORWARD_SPEED;
double turn = Utilities.Limit(controller.GetRightStickX()) * DRIVE_TURN_SPEED;
frontRightMotor.Set(ControlMode.PercentOutput, (float)-forward,
DemandType.ArbitraryFeedForward, (float)turn);
frontLeftMotor.Set(ControlMode.PercentOutput, (float)forward,
DemandType.ArbitraryFeedForward, (float)turn);
}
public void drive
(
double right_speed,
double left_speed
//Variables for moving the robot
)
{
left_motor.Set(ControlMode.PercentOutput, left_speed);
right_motor.Set(ControlMode.PercentOutput, right_speed);
//Using the past variables
//Making the wheels able to go backwards
}
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);
}