void SquareInputs(void)
{
if(stick.GetY() < 0)
{
if(DoubleSolenoid::kReverse == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * -4.0), stick.GetX());
}
else if(DoubleSolenoid::kForward == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * -1.0), stick.GetX());
}
}
else if(stick.GetY() > 0)
{
if(DoubleSolenoid::kReverse == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * 4.0), stick.GetX());
}
else if(DoubleSolenoid::kForward == shifter.Get())
{
myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * 1.0), stick.GetX());
}
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
HSLImage *Himage;
Threshold targetThreshold(247, 255, 60, 140, 10, 50);
BinaryImage *matchingPixels;
vector<ParticleAnalysisReport> *pReport;
//myRobot->SetSafetyEnabled(true);
Saftey->SetEnabled(false);
AxisCamera &mycam = AxisCamera::GetInstance("10.15.10.11");
mycam.WriteResolution(AxisCamera::kResolution_640x480);
mycam.WriteCompression(20);
mycam.WriteBrightness(25);
Wait(3.0);
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
float X[2];
float Y[2];
float Z[2];
while(IsOperatorControl())
{
X[1] = Stick1->GetX();
X[2] = Stick2->GetX();
Y[1] = Stick1->GetY();
Y[2] = Stick2->GetY();
Z[1] = Stick1->GetZ();
Z[2] = Stick2->GetZ();
Jaguar1->Set(Y[1]);
Jaguar2->Set(Y[2]);
Wait(0.005);
if (mycam.IsFreshImage())
{
Himage = mycam.GetImage();
matchingPixels = Himage->ThresholdHSL(targetThreshold);
pReport = matchingPixels->GetOrderedParticleAnalysisReports();
for (unsigned int i = 0; i < pReport->size(); i++)
{
printf("Index: %d X Center: %d Y Center: %d \n", i, (*pReport)[i].center_mass_x, (*pReport)[i].center_mass_y);
}
delete Himage;
delete matchingPixels;
delete pReport;
}
}
//myRobot->ArcadeDrive(stick); // drive with arcade style (use right stick)
//Wait(0.005); // wait for a motor update time
}
void OperatorControl(void)
{
NetTest();
return;
myRobot.SetSafetyEnabled(true);
digEncoder.Start();
const double ppsTOrpm = 60.0/250.0; //Convert from Pos per Second to Rotations per Minute by multiplication
// (See the second number on the back of the encoder to replace 250 for different encoders)
const float VoltsToIn = 41.0; // Convert from volts to cm by multiplication (volts from ultrasonic).
// This value worked for distances between 1' and 10'.
while (IsOperatorControl())
{
if (stick.GetRawButton(4)) {
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1);
}
else if (stick.GetRawButton(5))
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1);
}
else
{
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
}
myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm));
SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch);
SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage());
Wait(0.1);
}
digEncoder.Stop();
}
void TeleopPeriodic(void)
{
myarm->prepareSignal();
// Call the drive routine to drive the robot.
if(rightStick->GetRawButton(1)|| leftStick->GetRawButton(1))
drive->MecanumDrive_Cartesian(rightStick->GetX()/2,leftStick->GetY()/2,leftStick->GetX()/2,0.00);
else
drive->MecanumDrive_Cartesian(rightStick->GetX(),leftStick->GetY(),leftStick->GetX(),0.00);
GetStateForArm();
//Wait(.1);
if(modeArm==DDCArm::kManualOveride)
{
myarm->OperateArm(0,0,0,modeArm);
//Shoulder movement
if(leftStick->GetRawButton(3))
myarm->MoveShoulder(1);
else if(leftStick->GetRawButton(2))
myarm->MoveShoulder(-1);
else
myarm->MoveShoulder(0);
//Elbow Movement
if(rightStick->GetRawButton(3))
myarm->MoveElbow(1);
else if (rightStick->GetRawButton(2))
myarm->MoveElbow(-1);
else
myarm->MoveElbow(0);
//Wrist Movement
if(rightStick->GetRawButton(4))
myarm->MoveWrist(-1);
else if(rightStick->GetRawButton(5))
myarm->MoveWrist(1);
else
myarm->MoveWrist(0);
}
else
myarm->OperateArm(0.0,0.0,peg,modeArm);
if(leftStick->GetRawButton(4))
myarm->MoveClaw(-1);
else if(leftStick->GetRawButton(5))
myarm->MoveClaw(1);
else
myarm->MoveClaw(0);
if(rightStick->GetRawButton(10))
{
printf("S: %f \n E: %f \n W: %f \n \n",myarm->GetShoulderVoltage(),myarm->GetElbowVoltage(), myarm->GetWristVoltage());
}
deploy->OperateDeployment(fire,pull);
// Send Data to the Driver Station for Monitoring (w/in .
//sendIOPortData();
//Wait(.1);
}
void RobotDemo::arcade_tank_code()
{
if (drive_stick_prim ->GetRawButton(arcade_button))
{
arcadedrive = true;
}
else
{
if (drive_stick_prim ->GetRawButton(tank_button))
{
arcadedrive = false;
}
}
if (drive_stick_prim->GetRawButton(2) || drive_stick_sec->GetRawButton(2))
{
slow_control = true;
}
else
{
slow_control = false;
}
if (arcadedrive)
{
if (slow_control)
{
drive->ArcadeDrive(Adjustment_Speed * (drive_stick_prim->GetY()),
Adjustment_Speed * (drive_stick_prim->GetX()), true);
}
else
{
drive->ArcadeDrive((drive_stick_prim->GetY()),
(drive_stick_prim->GetX()), true);
}
}
else
{
if (slow_control)
{
drive->TankDrive(Adjustment_Speed * (drive_stick_prim->GetY()),
Adjustment_Speed * (drive_stick_sec->GetY()), true);
}
else
{
drive->TankDrive((drive_stick_prim->GetY()),
(drive_stick_sec->GetY()), true);
}
}
}
void TeleopPeriodic() {
// Comment the next line out to disable movement
drive->doDrive(stick->GetX(), -stick->GetY());
intake->periodic();
shooter->periodic();
camSystem->periodic();
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool collisionDetected = false;
double curr_world_linear_accel_x = ahrs->GetWorldLinearAccelX();
double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x;
last_world_linear_accel_x = curr_world_linear_accel_x;
double curr_world_linear_accel_y = ahrs->GetWorldLinearAccelY();
double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y;
last_world_linear_accel_y = curr_world_linear_accel_y;
if ( ( fabs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) ||
( fabs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) {
collisionDetected = true;
}
SmartDashboard::PutBoolean( "CollisionDetected", collisionDetected);
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and Z axis for rotation. */
/* Use navX MXP yaw angle to define Field-centric transform */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
stick.GetZ(),ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
/**
* Get the X value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
*
* @param port The USB port for this joystick.
*/
float GetX(UINT32 port, JoystickHand hand)
{
Joystick *stick = getJoystick(port);
if (stick == NULL)
return 0;
return stick->GetX((Joystick::JoystickHand) hand);
}
/**
* @brief Sets a cached joystick value.
* @param joy_id Which joystick to set the cached value for.
* @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons.
*/
void Proxy::SetJoystick(int joy_id, Joystick & stick)
{
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS+1 && joy_id >= 0);
char tmp[32];
sprintf(tmp, "Joy%d", joy_id);
string name = tmp;
if(!disableAxes[joy_id-1]) {
set(name + 'X', stick.GetX());
set(name + 'Y', stick.GetY());
set(name + 'Z', stick.GetZ());
set(name + 'R', stick.GetTwist());
set(name + 'T', stick.GetThrottle());
for(int AxisId=1; AxisId<=6; AxisId++) {
sprintf(tmp, "%sA%d", name.c_str(), AxisId);
set(tmp, stick.GetRawAxis(AxisId));
}
} else {
if(!stick.GetRawButton(disableAxes[joy_id-1])) {
set(name + 'X', stick.GetX());
set(name + 'Y', stick.GetY());
set(name + 'Z', stick.GetZ());
set(name + 'R', stick.GetTwist());
set(name + 'T', stick.GetThrottle());
for(int AxisId=1; AxisId<=6; AxisId++) {
sprintf(tmp, "%sA%d", name.c_str(), AxisId);
set(tmp, stick.GetRawAxis(AxisId));
}
}
}
if(!disableButtons[joy_id-1]) {
for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
sprintf(tmp, "%sB%d", name.c_str(), i);
set(tmp,stick.GetRawButton(i));
}
set(name + "BT", stick.GetTrigger());
} else {
if(!stick.GetRawButton(disableButtons[joy_id-1])) {
for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
sprintf(tmp, "%sB%d", name.c_str(), i);
set(tmp,stick.GetRawButton(i));
}
set(name + "BT", stick.GetTrigger());
}
}
}
void TeleopPeriodic()
{
SmartDashboard::PutNumber("joystickX",stick.GetX());
SmartDashboard::PutNumber("joystickY",stick.GetY());
//SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1));
//SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage());
SmartDashboard::PutBoolean("infra",infra.Get());
SmartDashboard::PutNumber("accelX",accel.GetX());
SmartDashboard::PutNumber("accelY",accel.GetY());
SmartDashboard::PutNumber("accelZ",accel.GetZ());
servo.Set(
trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick
);
SmartDashboard::PutNumber("servo", servo.Get());
jag1.Set(stick.GetY());
jag2.Set(stick.GetY());
//tal1.Set(stick.GetY());
SmartDashboard::PutNumber("jag1", jag1.Get());
SmartDashboard::PutNumber("jag2", jag2.Get());
/*SmartDashboard::PutNumber("encpos", enc.Get());
SmartDashboard::PutNumber("encspd", enc.GetRate());*/
if (stick.GetRawButton(1) && !actuatePressed) {
pistonVal=!pistonVal;
piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse);
actuatePressed = true;
}
else if (!stick.GetRawButton(1))
actuatePressed = false;
SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward);
}
/**
* Drive based upon joystick inputs, and automatically control
* motors if the robot begins tipping.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled()) {
double xAxisRate = stick.GetX();
double yAxisRate = stick.GetY();
double pitchAngleDegrees = ahrs->GetPitch();
double rollAngleDegrees = ahrs->GetRoll();
if ( !autoBalanceXMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceXMode = true;
}
else if ( autoBalanceXMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceXMode = false;
}
if ( !autoBalanceYMode &&
(fabs(pitchAngleDegrees) >=
fabs(kOffBalanceThresholdDegrees))) {
autoBalanceYMode = true;
}
else if ( autoBalanceYMode &&
(fabs(pitchAngleDegrees) <=
fabs(kOnBalanceThresholdDegrees))) {
autoBalanceYMode = false;
}
// Control drive system automatically,
// driving in reverse direction of pitch/roll angle,
// with a magnitude based upon the angle
if ( autoBalanceXMode ) {
double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0);
xAxisRate = sin(pitchAngleRadians) * -1;
}
if ( autoBalanceYMode ) {
double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0);
yAxisRate = sin(rollAngleRadians) * -1;
}
try {
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ());
} catch (std::exception ex ) {
std::string err_string = "Drive system error: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
// the loop routine runs over and over again forever:
void loop() {
if(enabled)
{
S1overflow = 0.0;
S2overflow = 0.0;
Stick.Update();
driveSpeed = Stick.GetX();
rotSpeed = Stick.GetY();
S1 = driveSpeed + rotSpeed;
S2 = driveSpeed + (-rotSpeed);
if(S1 > 1) { //calculate S1 overflow
S1overflow = S1 - 1.00;
S1 = 1.00;
}
else{
if(S1 < -1.00) {
S1overflow = S1 + 1.00;
S1 = -1.00;
}
}
if(S2 > 1) { //calculate S2 overflow
S2overflow = S2 - 1.00;
S2 = 1.00;
}
else{
if(S2 < -1.00) {
S2overflow = S2 + 1.00;
S2 = -1.00;
}
}
S1 = S1 + (-S2overflow);
S2 = S2 + (-S1overflow);
if(reverseS1) {
S1 = (-S1);
}
if(reverseS2) {
S2 = (-S2);
}
talon1s = (S1 + 1.00) * 90;
talon2s = (S2 + 1.00) * 90;
//Talon1.write(talon1s);
// Talon2.write(talon2s);
}
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
// This sample does not use field-oriented drive, so the gyro input is set to zero.
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ());
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
/**
* @brief Sets a cached joystick value.
* @param joy_id Which joystick to set the cached value for.
* @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons.
*/
void Proxy166::SetJoystick(int joy_id, Joystick & stick)
{
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS && joy_id >= 0);
//semTake(JoystickLocks[joy_id], WAIT_FOREVER);
Joysticks[joy_id].X = stick.GetX();
Joysticks[joy_id].Y = stick.GetY();
Joysticks[joy_id].Z = stick.GetZ();
Joysticks[joy_id].throttle = stick.GetThrottle();
for(unsigned i=0;i<NUMBER_OF_JOY_BUTTONS;i++) {
Joysticks[joy_id].button[i] = stick.GetRawButton(i);
}
//semGive(JoystickLocks[joy_id]);
}
void OperatorControl()
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl() && IsEnabled())
{
//MECANUM DRIVE
float yValue = stick->GetY();
float xValue = stick->GetX();
float rotation = stick->GetTwist();
myRobot.MecanumDrive_Cartesian(xValue, yValue, rotation, 0.0);
Wait(0.005);
}
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool reset_yaw_button_pressed = stick.GetRawButton(1);
if ( reset_yaw_button_pressed ) {
ahrs->ZeroYaw();
}
bool rotateToAngle = false;
if ( stick.GetRawButton(2)) {
turnController->SetSetpoint(0.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(3)) {
turnController->SetSetpoint(90.0f);
rotateToAngle = true;
} else if ( stick.GetRawButton(4)) {
turnController->SetSetpoint(179.9f);
rotateToAngle = true;
} else if ( stick.GetRawButton(5)) {
turnController->SetSetpoint(-90.0f);
rotateToAngle = true;
}
double currentRotationRate;
if ( rotateToAngle ) {
turnController->Enable();
currentRotationRate = rotateToAngleRate;
} else {
turnController->Disable();
currentRotationRate = stick.GetTwist();
}
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and the current */
/* calculated rotation rate (or joystick Z axis), */
/* depending upon whether "rotate to angle" is active. */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
currentRotationRate ,ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void DriveJoystickCommand::Execute() {
Joystick* pXboxController =
Robot::instance->pOperatorInterface->pXboxController;
if(!WPILibException::isWPIObjectOK(pXboxController)){
printf("Xbox controller not present!\n");
return;
}
double y = pXboxController->GetY();
// there was a 0.7 turnMax variable in the 2015 bot
y = clampJoystickValue(y, -TURN_MAX, TURN_MAX);
double x = pXboxController->GetX();
// roadkill safety
x = clampJoystickValue(x, -LINEAR_MAX, LINEAR_MAX);
SmartDashboard::PutNumber("DriveJoystick_x", x);
SmartDashboard::PutNumber("DriveJoystick_y", y);
Subsystems::pDriveSubsystem->DriveJoystick(y, x);
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
robotDrive.SetSafetyEnabled(false);
while (IsOperatorControl() && IsEnabled())
{
bool motionDetected = ahrs->IsMoving();
SmartDashboard::PutBoolean("MotionDetected", motionDetected);
try {
/* Use the joystick X axis for lateral movement, */
/* Y axis for forward movement, and Z axis for rotation. */
/* Use navX MXP yaw angle to define Field-centric transform */
robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
stick.GetZ(),ahrs->GetAngle());
} catch (std::exception ex ) {
std::string err_string = "Error communicating with Drive System: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
}
}
void UpdateStatusDisplays(void)
{
// Joystick values
SmartDashboard::PutNumber("stickX", stick.GetX());
SmartDashboard::PutNumber("stickY", stick.GetY());
SmartDashboard::PutBoolean("shift", stick2.GetState(BUTTON_SHIFT) ? kStateClosed : kStateOpen);
// Shooter/Indexer values
SmartDashboard::PutBoolean("indexSwitch", indexSwitch.GetState() ? kStateClosed : kStateOpen);
SmartDashboard::PutNumber("shooterMotor", shooterMotor.Get());
SmartDashboard::PutNumber("indexerMotor", indexerMotor.Get());
// Misc Motor Values
SmartDashboard::PutNumber("collectorMotor", collectorMotor.Get());
SmartDashboard::PutNumber("armMotor", armMotor.Get());
// Arm position via potentiometer voltage (2.5 volts is center position)
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "Voltage: %3.1f", potentiometer.GetVoltage());
SmartDashboard::PutNumber("Potentiometer", potentiometer.GetVoltage());
// Claw lock states
SmartDashboard::PutBoolean("Green Claw State", greenClawLockSwitch.GetState());
SmartDashboard::PutBoolean("Yellow Claw State", yellowClawLockSwitch.GetState());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "Green : %s",
greenClawLockSwitch.GetState() ? "Locked" : "Unlocked");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line5, "Yellow: %s",
yellowClawLockSwitch.GetState() ? "Locked" : "Unlocked");
// Pneumatic shifter count
SmartDashboard::PutNumber("Shift Count", m_shiftCount);
// State viariables
dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "CMR: %s SMR: %s JTR: %s",
m_collectorMotorRunning ? "T" : "F",
m_shooterMotorRunning ? "T" : "F",
m_jogTimerRunning ? "T" : "F");
}
void OperatorControl(void)
{
// Teleoperated Code.
/*double WaitDash = 0.0;
double FireDash = 0.0;
double intPause = 0.0;
SmartDashboard::PutNumber("P", 3.0);
SmartDashboard::PutNumber("W", 0.0);
SmartDashboard::PutNumber("A", 0.0);
WaitDash = SmartDashboard::GetNumber("P");
FireDash = SmartDashboard::GetNumber("W");
intPause = SmartDashboard::GetNumber("A");
SmartDashboard::PutNumber("P", WaitDash);
SmartDashboard::PutNumber("W", FireDash);
SmartDashboard::PutNumber("A", WaitDash);
*/
// Enable and start the compressor.
//compressor->Enabled();
compressor->Start();
// Enable drive motor safety timeout.
myRobot.SetSafetyEnabled(true);
// Enable watchdog and initial feed.
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(1);
GetWatchdog().Feed();
// Set robot in low gear by default. Not active.
//s[0]->Set(false);
GetWatchdog().Feed();
//bool blnShoot = false;
bool blnLowHang = false;
bool blnShift = false;
GetWatchdog().Feed();
bool blnShooterSpd = false;
bool blnReverse = false;
float fltShoot;
float fltSpeed = 1;
int intFail = 0;
timerLowHang.Reset();
timerShift.Reset();
timerFire.Reset();
timerShooter.Reset();
timerDriveCtrl.Reset();
timerCamera.Reset();
timerReverse.Reset();
timerLowHang.Start();
timerShift.Start();
timerFire.Start();
timerShooter.Start();
timerDriveCtrl.Start();
timerCamera.Start();
timerReverse.Start();
GetWatchdog().Feed();
//sd->sendIOPortData();
// Local variables.
//float fltStick1X, fltStick1Y;
while (IsOperatorControl())
{
if(timerReverse.Get() > 0.5)
{
if(stick1->GetRawButton(8) && blnReverse == false )
{
blnReverse = true;
GetWatchdog().Feed();
timerReverse.Reset();
timerReverse.Start();
}
else if(stick1->GetRawButton(9) && blnReverse == true)
{
blnReverse = false;
GetWatchdog().Feed();
timerReverse.Reset();
timerReverse.Start();
}
}
if(blnReverse == false)
{
fltStick1Y = stick1->GetY();
fltStick1X = stick1->GetX();
SmartDashboard::PutBoolean("Reverse",false);
GetWatchdog().Feed();
}
else if(blnReverse == true)
{
fltStick1Y = ((stick1->GetY())*(-1));
fltStick1X = ((stick1->GetX())*(1));
SmartDashboard::PutBoolean("Reverse",true);
GetWatchdog().Feed();
}
myRobot.ArcadeDrive(fltStick1Y,fltStick1X);
//myRobot.ArcadeDrive(stick1);
GetWatchdog().Feed(); // Feed hungary demonic Watchdog.
SmartDashboard::PutBoolean("Touching Tower?",LimitSwitch->Get());
SmartDashboard::PutNumber("Throttle (%)",stick1->GetY()*(-100));
SmartDashboard::PutNumber("Steering (%)",stick1->GetX()*(100));
GetWatchdog().Feed();
//End Stick1 arcade drive code.
GetWatchdog().Feed();
fltShoot = (((-(stick2->GetRawAxis(3)))+1)/2);
GetWatchdog().Feed();
SmartDashboard::PutNumber("Shooter Power (%)", fltShoot);
SmartDashboard::PutNumber("Shooter Set Speed (%)", (fltSpeed*100));
//float fltPressureSwitch = m_pressureSwitch;
//float fltRelay = m_relay;
//SmartDashboard::PutNumber("Demo",3);
GetWatchdog().Feed();
if(timerShift.Get() > 0.2)
{
if(stick1->GetRawButton(7) || stick1->GetTrigger())
{
if(blnShift == false)
{
GetWatchdog().Feed();
s[0]->Set(false);
s[1]->Set(true);
SmartDashboard::PutString("Gear","Low");
blnShift = true;
timerShift.Stop();
timerShift.Reset();
timerShift.Start();
GetWatchdog().Feed();
}
else if (blnShift == true)
{
GetWatchdog().Feed();
s[0]->Set(true);
s[1]->Set(false);
SmartDashboard::PutString("Gear","High");
blnShift = false;
timerShift.Stop();
timerShift.Reset();
timerShift.Start();
GetWatchdog().Feed();
}
}
}
if(stick1->GetRawButton(2) && blnLowHang == false && timerLowHang.Get() > 0.5)
{
blnLowTime = true;
blnLowHang = true;
timerLowHang.Stop();
timerLowHang.Reset();
timerLowHang.Start();
GetWatchdog().Feed();
}
else if(stick1->GetRawButton(2) && blnLowHang == true && timerLowHang.Get() > 0.5)
{
blnLowTime = false;
blnLowHang = false;
timerLowHang.Stop();
timerLowHang.Reset();
timerLowHang.Start();
GetWatchdog().Feed();
}
if(blnLowTime == true)
{
s[3]->Set(true);
GetWatchdog().Feed();
}
else if (blnLowTime == false)
{
s[3]->Set(false);
GetWatchdog().Feed();
}
if(stick1->GetRawButton(3))
{
mtdCameraCode();
GetWatchdog().Feed();
}
if(stick2->GetTrigger() && intFail == 0 && timerFire.Get() > 0.8)
{
s[2]->Set(true);
SmartDashboard::PutString("Shooter Piston","In");
intFail = 1;
GetWatchdog().Feed();
timerFire.Stop();
timerFire.Reset();
timerFire.Start();
GetWatchdog().Feed();
}
else if(stick2->GetTrigger() && intFail == 1 && timerFire.Get() > 0.8)
{
s[2]->Set(false);
intFail = 0;
SmartDashboard::PutString("Shooter Piston","Out");
GetWatchdog().Feed();
timerFire.Stop();
timerFire.Reset();
timerFire.Start();
GetWatchdog().Feed();
}
if(stick2->GetRawButton(2) && blnShooterSpd == false && timerShooter.Get() > 0.5)
{
GetWatchdog().Feed();
myShooter1.Set(-fltSpeed);
myShooter2.Set(-fltSpeed);
SmartDashboard::PutString("Shooter","On");
SmartDashboard::PutNumber("Shooter Speed (%)",(fltSpeed)*(100));
blnShooterSpd = true;
GetWatchdog().Feed();
timerShooter.Stop();
timerShooter.Reset();
timerShooter.Start();
GetWatchdog().Feed();
}
else if(stick2->GetRawButton(2) && blnShooterSpd == true && timerShooter.Get() > 0.5)
{
GetWatchdog().Feed();
myShooter1.Set(0);
myShooter2.Set(0);
SmartDashboard::PutString("Shooter","Off");
SmartDashboard::PutNumber("Shooter Speed (%)",0);
blnShooterSpd = false;
GetWatchdog().Feed();
//Wait(0.2);
timerShooter.Stop();
timerShooter.Reset();
timerShooter.Start();
GetWatchdog().Feed();
}
if(stick2->GetRawButton(10))
{
fltSpeed = 0.6;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(9))
{
fltSpeed = 0.7;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(8))
{
fltSpeed = 0.8;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(7))
{
fltSpeed = 0.9;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(6))
{
fltSpeed = 1;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(11))
{
fltSpeed = fltShoot;
GetWatchdog().Feed();
}
GetWatchdog().Feed();
}
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()//teleop code
{
robotDrive.SetSafetyEnabled(false);
gyro.Reset();
grabEncoder.Reset();
timer.Start();
timer.Reset();
double liftHeight = 0; //variable for lifting thread
int liftHeightBoxes = 0; //another variable for lifting thread
int liftStep = 0; //height of step in inches
int liftRamp = 0; //height of ramp in inches
double grabPower;
bool backOut;
uint8_t toSend[10];//array of bytes to send over I2C
uint8_t toReceive[10];//array of bytes to receive over I2C
uint8_t numToSend = 1;//number of bytes to send
uint8_t numToReceive = 0;//number of bytes to receive
toSend[0] = 1;//set the byte to send to 1
i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
bool isGrabbing = false;//whether or not grabbing thread is running
bool isLifting = false;//whether or not lifting thread is running
bool isBraking = false;//whether or not braking thread is running
float driveX = 0;
float driveY = 0;
float driveZ = 0;
float driveGyro = 0;
bool liftLastState = false;
bool liftState = false; //button pressed
double liftLastTime = 0;
double liftTime = 0;
bool liftRan = true;
Timer switchTimer;
Timer grabTimer;
switchTimer.Start();
grabTimer.Start();
while (IsOperatorControl() && IsEnabled())
{
// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
// This sample does not use field-oriented drive, so the gyro input is set to zero.
toSend[0] = 1;
numToSend = 1;
driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
driveY = driveStick.GetRawAxis(Constants::driveYAxis);
driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;
if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
toSend[0] = 6;
numToSend = 1;
if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
driveY = 0;
driveZ = 0;
}
else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
driveX = 0;
driveZ = 0;
}
else {//if Z is greater than X and Y, then it will only go in the direction of Z
driveX = 0;
driveY = 0;
}
}
if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
toSend[0] = 7;
driveZ = 0;//Stops Z while Z lock is pressed
}
if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
driveGyro = 0;//gyro stops while field lock is enabled
}
driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive
if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
if(grabTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
pickup.setGrabber(0);
grabTimer.Reset();
toSend[0] = 6;
}
if (Constants::grabLiftInverted) {
pickup.setLifter(-Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
else {
pickup.setLifter(Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
}
SmartDashboard::PutNumber("Lift Power", Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree));
SmartDashboard::PutBoolean("Is Lifting", isLifting);
if (Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree) != 0 || isLifting) { //if the robot is lifting
isBraking = false; //stop braking thread
SmartDashboard::PutBoolean("Braking", false);
}
else if(!isBraking) {
isBraking = true; //run braking thread
pickup.lifterBrake(isBraking);//brake the pickup
}
if (grabStick.GetRawButton(Constants::liftFloorButton)) {
liftHeight = 0;
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftTime = timer.Get();
liftState = grabStick.GetRawButton(Constants::liftButton);
if (liftState) { //if button is pressed
if (!liftLastState) {
if (liftTime - liftLastTime < Constants::liftMaxTime) {
if (liftHeightBoxes < Constants::liftMaxHeightBoxes) {
liftHeightBoxes++; //adds 1 to liftHeightBoxes
}
}
else {
liftHeightBoxes = 1;
liftRamp = 0;
liftStep = 0;
}
}
liftLastTime = liftTime;
liftLastState = true;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftRampButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftStep = 0;
}
liftRamp = 1; //prepares to go up ramp
liftLastTime = liftTime;
liftRan = false;
}
else if (grabStick.GetRawButton(Constants::liftStepButton)) {
if (liftTime - liftLastTime > Constants::liftMaxTime) {
liftHeight = 0;
liftRamp = 0;
}
liftStep = 1; //prepares robot for step
liftLastTime = liftTime;
liftRan = false;
}
else {
if (liftTime - liftLastTime > Constants::liftMaxTime && !liftRan) {
liftHeight = liftHeightBoxes * Constants::liftBoxHeight + liftRamp * Constants::liftRampHeight + liftStep * Constants::liftStepHeight; //sets liftHeight
if (liftHeightBoxes > 0) {
liftHeight -= Constants::liftBoxLip;
}
pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
liftRan = true;
}
liftLastState = false;
}
if (grabStick.GetRawButton(Constants::grabToteButton)) {//if grab button is pressed
grabPower = Constants::grabToteCurrent;
backOut = true;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabBinButton)) {//if grab button is pressed
grabPower = Constants::grabBinCurrent;
backOut = false;
if (!isGrabbing) {
pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
}
}
else if (grabStick.GetRawButton(Constants::grabChuteButton)) {//if grab button is presset
SmartDashboard::PutBoolean("Breakpoint -2", false);
SmartDashboard::PutBoolean("Breakpoint -1", false);
SmartDashboard::PutBoolean("Breakpoint 0", false);
SmartDashboard::PutBoolean("Breakpoint 1", false);
SmartDashboard::PutBoolean("Breakpoint 2", false);
SmartDashboard::PutBoolean("Breakpoint 3", false);
SmartDashboard::PutBoolean("Breakpoint 4", false);
//Wait(.5);
if (!isGrabbing) {
//pickup.grabberChute(isGrabbing, grabStick);//start grabber thread
}
}
//determines what the LED's look like based on what the Robot is doing
if (isGrabbing) {
toSend[0] = 5;
numToSend = 1;
}
if (isLifting) {//if the grabbing thread is running
if (Constants::encoderToDistance(liftEncoder.Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < liftHeight) {
toSend[0] = 3;
}
else {
toSend[0] = 4;
}
numToSend = 1;//sends 1 byte to I2C
}
if(!grabOuterLimit.Get()) { //tells if outer limit is hit with lights
if(switchTimer.Get() < 1) {
toSend[0] = 6;
}
}
else {
switchTimer.Reset();
}
if (driveStick.GetRawButton(Constants::sneakyMoveButton)) {
toSend[0] = 0;
numToSend = 1;
}
float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor
float rotations = (float) liftEncoder.Get(); // rotations on encoder
SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash
SmartDashboard::PutNumber("Current", pdp.GetCurrent(Constants::grabPdpChannel));
SmartDashboard::PutNumber("LED Current", pdp.GetCurrent(Constants::ledPdpChannel));
SmartDashboard::PutNumber("Lift Encoder", rotations);
SmartDashboard::PutNumber("Lift Height", liftHeight);
SmartDashboard::PutNumber("Grab Encoder", grabEncoder.Get());
SmartDashboard::PutBoolean("Grab Inner", grabInnerLimit.Get());
SmartDashboard::PutBoolean("Grab Outer", grabOuterLimit.Get());
SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
SmartDashboard::PutNumber("Throttle", grabStick.GetZ());
i2c.Transaction(toSend, 1, toReceive, 0);//send and receive information from arduino over I2C
Wait(0.005); // wait 5ms to avoid hogging CPU cycles
} //end of teleop
isBraking = false;
toSend[0] = 0;
i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
}
/**
* Periodic code for teleop mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in teleop mode.
*/
void RobotDemo::TeleopPeriodic()
{
m_robotDrive.MecanumDrive_Cartesian(m_driveStick.GetX(), m_driveStick.GetY(), m_driveStick2.GetX(), m_gyro.GetAngle());
printf("rate: %d\n", (int) m_encoder.GetRaw());
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot->SetSafetyEnabled(true);
while (IsOperatorControl())
{
bool setLimit;
double cimValue1= -scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1, also invert the cim, since we want it to rotate coutnerclockwise/clockwise
double cimValue2= scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1
//For shooter
/*if (stick.GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
myRobot.ArcadeDrive((stick.GetY()),
(stick.GetX()), false); // inverted drive control
} else {
//front of the robot is moved forward by pushing forward on the joystick
myRobot.ArcadeDrive(-(stick.GetY()),
(stick.GetX()), false); // normal drive control
}
*/
//For manual button speed control, this sets the speed
if (stick->GetRawButton(4) == true) {
cim1->Set(cimValue1); //use the value from the throttle to set cim speed
cim2->Set(cimValue2);//Get speed from throttle, and then scale it
setLimit = true;
//Open Ball Stopper
}
else {
cim1->Set(0.0);
cim2->Set(0.0);
setLimit = false;
//Close Ball Stopper
}
//For precisebelt pickup
if (stick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
if (setLimit == true) {
JagBelt->ArcadeDrive(0.1,
(stick->GetX()), false); // inverted drive control
} else {
JagBelt->ArcadeDrive((stick->GetY()),
(stick->GetX()), false); // inverted drive control
}
} else {
//front of the robot is moved forward by pushing forward on the joystick
if (setLimit == true) {
JagBelt->ArcadeDrive(-0.1,
(stick->GetX()), false); // inverted drive control
} else {
JagBelt->ArcadeDrive(-(stick->GetY()),
(stick->GetX()), false); // inverted drive control
}
}
//For normal belt pickup
/*if (stick->GetRawButton(6) == true) {
JagBelt->Drive(1.0, 0); //opens gripper
} else {
JagBelt->Drive(0.0, 0); //closes gripper
}
*/
//For drive
if (rightstick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
if (rightstick->GetRawButton(10) == true) {
precisionMode= true;
}
else {
precisionMode= false;
}
myRobot->ArcadeDrive((rightstick->GetY()), (rightstick->GetX()), precisionMode); // inverted drive control
} else
{
if (rightstick->GetRawButton(10) == true) {
precisionMode= true;
}
else {
precisionMode= false;
}
//front of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive(-(rightstick->GetY()), (rightstick->GetX()), precisionMode); // normal drive control
}
/*if (stick->GetRawButton(8) == true) {
cim1->Set(-0.37); //run cim 1 at 50% speed counterclockwise??
cim2->Set(0.37); // run cim at 50% speed clockwise
check = true; //indicate if motors are running
} else if (check == true){ // if motors are running a
Wait(2.0);
belt->Set(1); // run the belt
Wait(2.0); // one sec delay
belt->Set(0.0); // turn belt off
check = false; // put new check
}
else if (check == false){ //if false
// 2 sec delay to wait for the first ball to shoot
cim1->Set(0.0); //stop cims
cim2->Set(0.0);
}
else { // Stop everything
cim1->Set(0.0); //stop cims
cim2->Set(0.0);
belt->Set(0.0);
}
*/
//Code for using window motor
if (rightstick->GetRawButton(4)) {
window->Set(Relay::kOn);
window->Set(Relay::kForward); // tell window motor to go forward
} else if (rightstick->GetRawButton(5) == true){
window->Set(Relay::kOn);
window->Set(Relay::kReverse); //tell window motor to go backward
}
else {
//Wait(1.0);
window->Set(Relay::kOff); //turn it off, if the relays aint being used
}
//Code for Banebot Motor for stopping ballz
if (stick->GetRawButton(2) == true) { // press button TWO to close
pickStop->Set(-0.5); //closes ball stopper
Wait(1);
pickStop->Set(0.0);
} else if (stick->GetRawButton(3) == true){ //press button three to open
pickStop->Set(0.5); //opens ball stopper
Wait(1.2); // too slow, so needs more time
pickStop->Set(0.0);
}
else if (stick->GetRawButton(5)== true){ //press 5 to stop imediately, useful for adjusting angles...
//Wait(1.0);
pickStop->Set(0.0);
}
//Code for ... servoooo
if (stick->GetRawButton(10) == true) { //press 10 on the left stick...
bar->SetAngle(60); // set the angles to 60...clockwise?
bar2->SetAngle(60);
} else if (stick->GetRawButton(11) == true) // press 11 on the left stick
{
bar->SetAngle(-60); //set the angles to -60...counterclockwise?
bar2->SetAngle(-60);
}
// Initialize functions...
// RelayServo();
//PreciseBelt();
//UltrasonicRange();
// Accelerometer();
//dash->GetPacketNumber();
// send data back to dashboard
dash->GetPacketNumber(); //not sure why this is here 0_0
//int limitValue= limitSwitch->Get() ? 1 : 0; // retrieve 1 and 0 only.../ look for 0 and 1
float servoAngle = bar->GetAngle();
//dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Limit State: %d", limitValue); //send data back to driver station...
// dsLCD->Printf(DriverStationLCD::kUser_Line2, 2, "Servo Angle: %f", servoAngle); //send data back to driver station...
float gyroVal = gyro -> GetVoltage();//Gets voltage from gyro
float ultraVal = rangeFinder -> GetVoltage(); //Get voltage from ultrasonic sensor
float tempVal = Temperature -> GetVoltage();//Gets temperature
//Do the math to convert data received from the ultrasonic volts->miliVolts->milivolts per inch->inches
float Vm = (ultraVal*1000);
float Ri = (Vm/9.765625);
// Print data back to dashboard
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Ultrasonic Range: %f",Ri);
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Gyro: %f", gyroVal);
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Temperature: %f", tempVal);
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Cim1 Speed: %f%%", (cimValue1*100)); //display speed that the mototrs are reunning at different percentages...
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Cim2 Speed: %f%%", (cimValue2*100));
//Update dashboard
dsLCD->UpdateLCD();
}
}
void OperatorControl (void) {
GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
while (IsOperatorControl() && IsEnabled()) {
GetWatchdog().Feed(); // Feed the Watchdog.
kinectMode = (bool) kinectModeSelector->GetSelected();
demoMode = (bool) demoModeSelector->GetSelected();
speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
if (kinectMode) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
if (!demoMode) {
if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
} else {
motorDriveLeft.Set(0);
motorDriveRight.Set(0);
}
}
if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
motorTurret.Set(TURRET_POWER);
} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
if (joystickDriveLeft.GetThrottle() == 0) {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
motorTurret.Set(TURRET_POWER);
} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
motorTurret.Set(TURRET_POWER * -1);
} else {
motorTurret.Set(0);
}
} else {
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " ");
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
motorShooter.Set(SHOOTER_MOTOR_POWER);
motorFeed.Set(FEED_MOTOR_POWER);
motorPickup.Set(PICKUP_MOTOR_POWER);
} else if (joystickManipulator.GetRawButton(MANIPULATOR_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
motorFeed.Set(FEED_MOTOR_POWER * -1);
motorPickup.Set(PICKUP_MOTOR_POWER * -1);
} else {
motorShooter.Set(0);
motorFeed.Set(0);
motorPickup.Set(0);
}
motorTurret.Set(joystickManipulator.GetX() * -1 * TURRET_POWER);
}
}
dsLCD->UpdateLCD();
}
GetWatchdog().SetEnabled(false); // Teleop is done, so let's turn off Watchdog.
}
/*
* OPERATOR CONTROL using a arcade style drive
*/
void OperatorControl(void) {
GetWatchdog().SetEnabled(true);
while (IsOperatorControl()) //This code will loop continuously as long it is operator control mode
{
GetWatchdog().Feed(); // Feed the watchdog
shoulderPotentiometerReading = (5-(shoulderPotentiometerChannel->GetVoltage())); // reads the potentiometer at channel 1
/* COMMENT GRIPPER CODE BELOW
if (shoulderPotentiometerReading == 0) {
gripperMotor->SetAngle(0); //opens gripper
}
else if (shoulderPotentiometerReading > 3) {
gripperMotor->SetAngle(55); //closes gripper
}
*/
if (leftstick->GetRawButton(4) == true) {
shoulderMotor->Drive(-kMaxShoulderMotorSpeed, 0); //moves shoulderMotor down
shoulderDestinationVoltage = shoulderPotentiometerReading;
} else if (leftstick->GetRawButton(5) == true) {
shoulderMotor->Drive(kMaxShoulderMotorSpeed, 0); //moves shoulderMotor up
shoulderDestinationVoltage = shoulderPotentiometerReading;
} else {
//LEFTSTICK PRESETS
if (leftstick->GetRawButton(2) == true) {
//shoulderDestinationVoltage = kPickUpPosition;
} else if (leftstick->GetRawButton(6) == true) {
shoulderDestinationVoltage = kTopRow;
} else if (leftstick->GetRawButton(7) == true) {
shoulderDestinationVoltage = kMiddleRow;
} else if (leftstick->GetRawButton(8) == true) {
shoulderDestinationVoltage = kBottomRow;
} else if (leftstick->GetRawButton(9) == true) {
shoulderDestinationVoltage = kBottomRowSecondColumn;
} else if (leftstick->GetRawButton(10) == true) {
shoulderDestinationVoltage = kMiddleRowSecondColumn;
} else if (leftstick->GetRawButton(11) == true) {
shoulderDestinationVoltage = kTopRowSecondColumn;
}
MoveShoulderTo(shoulderDestinationVoltage);
}
//----------------------------------------------------------------
//GRIPPER OPEN AND CLOSE
if (leftstick->GetRawButton(1) == true) {
gripperMotor->SetAngle(0); //opens gripper
} else {
gripperMotor->SetAngle(55); //closes gripper
}
//MINIBOT DEPLOYMENT
if (leftstick->GetRawButton(3) == true) {
minibotDeployMotor->SetAngle(60); //releases four-bar
myRobot->Drive(0.0, 0); // stop robot since controls are going to be inverted momentarily - don't want to go from full forward to full reverse instantly
}
//RIGHTSTICK PRESETS
if (rightstick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive((rightstick->GetY()),
(rightstick->GetX()), false); // inverted drive control
} else {
//front of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive(-(rightstick->GetY()),
(rightstick->GetX()), false); // normal drive control
}
//MINIBOT CLOSING/CLIMBING POLE
if (rightstick->GetRawButton(3) == true) {
//minibot closes only after its four-bar is dropped
minibotCloseMotor1->SetAngle(45); //closes minibot so it starts climbing pole
minibotCloseMotor2->SetAngle(45); //closes minibot so it starts climbing pole
} else if (rightstick->GetRawButton(4) == true) {
//MANUAL ARM CONTROL
armMotor->Drive(-kMaxArmMotorSpeed/2.0, 0); //turn armMotor counter clockwise
} else if (rightstick->GetRawButton(5) == true) {
//MANUAL ARM CONTROL
armMotor->Drive(kMaxArmMotorSpeed/1.5, 0); //turn armMotor cw
}
}
}
void SwerveDrive::Update(Joystick &stick, Joystick &stick2, float gyroValue)
{
float zInput = stick.GetZ(); // Rotation Clockwise
float xInput = stick.GetX(); // Strafe
float yInput = stick.GetY(); // Forward
float temp = yInput * cos(gyroValue * M_PI / 180) + xInput * sin(gyroValue * M_PI / 180); // This block of commands SHOULD make this thing field oriented
xInput = -yInput * sin(gyroValue * M_PI / 180) + xInput * cos(gyroValue * M_PI / 180);
yInput = temp;
int *controlType;
controlType = (int *)a_ControlTypeChooser.GetSelected();
if (controlType == NULL)
{
std::cout << "error reading control type" << std::endl;
return;
}
float A = xInput - zInput * (a_RobotLength / a_ChassisRadius);
float B = xInput + zInput * (a_RobotLength / a_ChassisRadius);
float C = yInput - zInput * (a_RobotWidth / a_ChassisRadius);
float D = yInput + zInput * (a_RobotWidth / a_ChassisRadius);
float max = -9 * pow(10,4);
double frSpeed = 0.0;
double frAngle = 0.0;
double flSpeed = 0.0;
double flAngle = 0.0;
double blSpeed = 0.0;
double blAngle = 0.0;
double brSpeed = 0.0;
double brAngle = 0.0;
switch (*controlType)
{
case CONTROL_TYPE_SWERVE:
frSpeed = sqrt(pow(B,2) + pow(C,2));
flSpeed = sqrt(pow(B,2) + pow(D,2));
blSpeed = sqrt(pow(A,2) + pow(D,2));
brSpeed = sqrt(pow(A,2) + pow(C,2));
max = frSpeed;
if(flSpeed > max) {
max = flSpeed;
}
if(blSpeed > max) {
max = blSpeed;
}
if(brSpeed > max) {
max = brSpeed;
}
if(max > 1) { // This is done so that if a speed greater than 1 is calculated, all are reduced proportionally
frSpeed /= max;
flSpeed /= max;
blSpeed /= max;
brSpeed /= max;
}
// atan2 outputs values in a manner similar to what is shown on the below diagram
////////////////////
// 0 //
// // //
// // //
// // //
//-90///////////90//
// // //
// // //
// // //
// -180 or 180 //
////////////////////
frAngle = (atan2(C,B) * 180.0 / M_PI);
flAngle = (atan2(D,B) * 180.0 / M_PI);
blAngle = (atan2(D,A) * 180.0 / M_PI);
brAngle = (atan2(C,A) * 180.0 / M_PI);
// If on the left side, add 360 to normalize values to the below diagram
////////////////////
// 360 or 0 //
// // //
// // //
// // //
//270///////////90//
// // //
// // //
// // //
// 180 //
////////////////////
if(frAngle < 0) {
frAngle += 360;
}
if(flAngle < 0) {
flAngle += 360;
}
if(brAngle < 0) {
brAngle += 360;
}
if(blAngle < 0) {
blAngle += 360;
}
break;
case CONTROL_TYPE_CRAB:
float setAngle = atan2(yInput,xInput) * 180.0 / M_PI; // find the angle the stick is pointed to, use that for all wheels
frAngle = setAngle;
flAngle = setAngle;
blAngle = setAngle;
brAngle = setAngle;
float setSpeed = sqrt(pow(xInput,2) + pow(yInput,2)); // find the r of the joystick vector, if you think about it in polar coordinates
frSpeed = setSpeed;
flSpeed = setSpeed;
blSpeed = setSpeed;
brSpeed = setSpeed;
break;
}
a_FrontRight.Set(frAngle, frSpeed);
a_FrontLeft.Set(flAngle, flSpeed);
a_BackLeft.Set(blAngle, blSpeed);
a_BackRight.Set(brAngle, brSpeed);
}
/**
* unchanged from SimpleDemo:
*
* Runs the motors under driver control with either tank or arcade
* steering selected by a jumper in DS Digin 0.
*
* added for vision:
*
* Adjusts the servo gimbal based on the color tracked. Driving the
* robot or operating an arm based on color input from gimbal-mounted
* camera is currently left as an exercise for the teams.
*/
void OperatorControl(void)
{
char funcName[] = "OperatorControl";
DPRINTF(LOG_DEBUG, "OperatorControl");
//GetWatchdog().Feed();
TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);
/* for controlling loop execution time */
float loopTime = 0.05;
double currentTime = GetTime();
double lastTime = currentTime;
double savedImageTimestamp = 0.0;
bool foundColor = false;
bool staleImage = false;
while (IsOperatorControl())
{
setServoPositions(rightStick->GetX(), rightStick->GetY());
/* calculate gimbal position based on color found */
if (FindColor
(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, &par,
&cReport))
{
foundColor = true;
if (par.imageTimestamp == savedImageTimestamp)
{
// This image has been processed already,
// so don't do anything for this loop
staleImage = true;
}
else
{
staleImage = false;
savedImageTimestamp = par.imageTimestamp;
// compute final H & V destination
horizontalDestination = par.center_mass_x_normalized;
verticalDestination = par.center_mass_y_normalized;
}
}
else
{
foundColor = false;
}
PrintReport(&cReport);
if (!staleImage)
{
if (foundColor)
{
/* Move the servo a bit each loop toward the
* destination. Alternative ways to task servos are
* to move immediately vs. incrementally toward the
* final destination. Incremental method reduces the
* need for calibration of the servo movement while
* moving toward the target.
*/
ShowActivity
("** %s found: Servo: x: %f y: %f",
td.name, horizontalDestination, verticalDestination);
}
else
{
ShowActivity("** %s not found", td.name);
}
}
dashboardData.UpdateAndSend();
// sleep to keep loop at constant rate
// elapsed time can vary significantly due to debug printout
currentTime = GetTime();
lastTime = currentTime;
if (loopTime > ElapsedTime(lastTime))
{
Wait(loopTime - ElapsedTime(lastTime));
}
}
while (IsOperatorControl())
{
// determine if tank or arcade mode; default with no jumper is
// for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0)
{
// drive with tank style
myRobot->TankDrive(leftStick, rightStick);
}
else
{
// drive with arcade style (use right stick)
myRobot->ArcadeDrive(rightStick);
}
}
} // end operator control
void OperatorControl(void)
{
bool button1Bool;
bool button2Bool;
bool button3Bool;
bool button4Bool;
bool button5Bool;
bool button6Bool;
bool button7Bool;
bool button8Bool;
bool button9Bool;
bool button10Bool;
bool button11Bool;
bool button12Bool;
float LeftaxisYValue;
float LeftaxisXValue;
float RightaxisXValue;
float RightaxisYValue;
float TriggerValue;
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
button1Bool = button1.Get();
SmartDashboard::PutNumber("button1",button1Bool);
button2Bool = button2.Get();
SmartDashboard::PutNumber("button2",button2Bool);
button3Bool = button3.Get();
SmartDashboard::PutNumber("button3",button3Bool);
button4Bool = button4.Get();
SmartDashboard::PutNumber("button4",button4Bool);
button5Bool = button5.Get();
SmartDashboard::PutNumber("button5",button5Bool);
button6Bool = button6.Get();
SmartDashboard::PutNumber("button6",button6Bool);
button7Bool = button7.Get();
SmartDashboard::PutNumber("button7",button7Bool);
button8Bool = button8.Get();
SmartDashboard::PutNumber("button8",button8Bool);
button9Bool = button9.Get();
SmartDashboard::PutNumber("button9",button9Bool);
button10Bool = button10.Get();
SmartDashboard::PutNumber("button10",button10Bool);
button11Bool = button11.Get();
SmartDashboard::PutNumber("button11",button11Bool);
button12Bool = button12.Get();
SmartDashboard::PutNumber("button12",button12Bool);
LeftaxisXValue = stick.GetX();
SmartDashboard::PutNumber("Joystick1 X Axis",LeftaxisXValue);
LeftaxisYValue = 0 - stick.GetY();
SmartDashboard::PutNumber("Joystick1 Y Axis",LeftaxisYValue);
RightaxisXValue = stick.GetTwist();
SmartDashboard::PutNumber("Joystick2 X Axis", RightaxisXValue);
RightaxisYValue = 0 - stick.GetRawAxis(5);
SmartDashboard::PutNumber("Joystick2 Y Axis", RightaxisYValue);
TriggerValue = stick.GetThrottle();
SmartDashboard::PutNumber("Trigger value", TriggerValue);
Wait(0.005); // wait for a motor update time
}
}
void TeleopPeriodic() {
if(tick==10) if (ds->IsSysBrownedOut()) {
ds->ReportError("[ERROR] BROWNOUT DETECTED!!");
}
if(tick == 15) if (!ds->IsNewControlData()) {
ds->ReportError(
"[ERROR] NO DATA FROM DRIVER STATION IN THIS TICK!");
}
if(tick==20) if (!ds->IsDSAttached()) {
ds->ReportError("[ERROR] DRIVER STATION NOT DETECTED!");
}
if (stick.GetRawButton(10))
zeroSanics();
if (stick.GetRawButton(8)) {
leftIRZero = 0;
rightIRZero = 0;
}
tick++;
if (liftStick.GetRawButton(2)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(canScale);
} else if (liftStick.GetRawButton(3)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(-canScale);
} else
canGrabber.SetSpeed(0);
double speed;
//Calculate scalar to use for POV/Adjusted drive
double scale = stick.GetRawAxis(3);
scale += 1;
scale = 2 - scale;
scale /= 2;
//Use pov/hat switch for movement if enabled
if (stick.GetRawButton(1) && stick.GetRawButton(2)) {
AutomaticLineup();
} else if (stick.GetRawButton(1)) {
double leftVolts = leftIR.GetAverageVoltage() - leftIRZero;
double rightVolts = rightIR.GetAverageVoltage() - leftIRZero;
if (rightVolts + VOLTAGE_TOLERANCE > leftVolts
&& rightVolts - VOLTAGE_TOLERANCE < leftVolts) {
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
} else if (rightVolts > leftVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, 0.2);
else if (leftVolts > rightVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, -0.2);
} else if (stick.GetRawButton(6)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, scale);
} else if (stick.GetRawButton(5)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, -scale);
} else if (stick.GetPOV(0) != -1) {
//If POV moved, move polar (getPOV returns an angle in degrees)
robotDrive.MecanumDrive_Polar(scale, -stick.GetPOV(0), 0);
} else if (stick.GetRawButton(2)) {
//Drive with scalar
robotDrive.MecanumDrive_Cartesian(-stick.GetRawAxis(0) * scale,
stick.GetRawAxis(1) * scale, stick.GetRawAxis(2) * scale);
} else {
//Drive normally
robotDrive.MecanumDrive_Cartesian(-stick.GetX(), stick.GetY(),
stick.GetZ());
}
speed = -liftStick.GetY();
//bool canGoUp = maxUp.Get();
bool canGoUp = true;
//bool canGoDown = maxDown.Get();
bool canGoDown = true;
//If at a limit switch and moving in that direction, stop
if (speed > 0 && !canGoUp)
speed = 0;
if (speed < 0 && !canGoDown)
speed = 0;
chainLift.SetSpeed(speed);
if (tick >50) {
if (SmartDashboard::GetBoolean("Smart Dashboard Enabled")) {
//Smart Dash outputs
//SmartDashboard::PutNumber("X Acceleration: ", accel.GetX());
//SmartDashboard::PutNumber("Y Acceleration: ", accel.GetY());
//SmartDashboard::PutNumber("Z Acceleration: ", accel.GetZ());
SmartDashboard::PutBoolean("Switch 1: (up)", maxUp.Get());
SmartDashboard::PutBoolean("Switch 2: (down)", maxDown.Get());
SmartDashboard::PutBoolean("Switch 3: (mid)", midPoint.Get());
SmartDashboard::PutBoolean("Auto switch A: ",
autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ",
autoSwitch2.Get());
//SmartDashboard::PutBoolean("RobotDrive Alive?",
// robotDrive.IsAlive());
//SmartDashboard::PutBoolean("ChainLift Alive?",
// robotDrive.IsAlive());
SmartDashboard::PutNumber("Left Sensor",
leftIR.GetAverageVoltage());
SmartDashboard::PutNumber("Right Sensor",
rightIR.GetAverageVoltage());
SmartDashboard::PutNumber("Left w zero",
leftIR.GetAverageVoltage() - leftIRZero);
SmartDashboard::PutNumber("Rigt w zero",
rightIR.GetAverageVoltage() - rightIRZero);
SmartDashboard::PutNumber("PDP 14 Current", pdp.GetCurrent(14));
SmartDashboard::PutNumber("PDP 15 Current", pdp.GetCurrent(15));
}
tick = 0;
}
}
/**
* Periodic code for teleop mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in teleop mode.
*/
void RobotDemo::TeleopPeriodic() {
/*ALTERNATE OPTION: use constants at the top of the code to pass in buttons associated
* with various actions.
*
*FOR ALTERNATE LAYOUTS:
*have one set of layouts commented out?
*take input from Driver Station I/O tab to determine which set is used?
* */
//driver joystick: Extreme 3D
bool compressor = stickDrive->GetRawButton(10);
//drive-related
bool stopDrive = stickDrive->GetRawButton(1);
bool slowSpeed = stickDrive->GetRawButton(2);
//arm controls--solenoids involved
bool extendAll_driver = stickDrive->GetRawButton(9);
bool retractAll_driver = stickDrive->GetRawButton(11);
//spinner controls
bool leftDrop_driver = stickDrive->GetRawButton(3);
bool leftPick_driver = stickDrive->GetRawButton(5);
bool rightDrop_driver = stickDrive->GetRawButton(4);
bool rightPick_driver = stickDrive->GetRawButton(6);
//other joystick: Attack 3
//arm controls--solenoids involved
bool extendAll_other = stickOther->GetRawButton(3);
bool retractAll_other = stickOther->GetRawButton(2);
bool leftArm_other = (stickOther->GetRawButton(6) || stickOther->GetRawButton(10));
bool rightArm_other = (stickOther->GetRawButton(7) || stickOther->GetRawButton(11));
//spinner controls
bool leftDrop_other = (stickOther->GetRawButton(4) && !stickOther->GetRawButton(1));
bool leftPick_other = (stickOther->GetRawButton(4) && stickOther->GetRawButton(1));
bool rightDrop_other = (stickOther->GetRawButton(5) && !stickOther->GetRawButton(1));
bool rightPick_other = (stickOther->GetRawButton(5) && stickOther->GetRawButton(1));
SmartDashboard::PutBoolean("Left-Spinner", spinnerStatusLeft);
SmartDashboard::PutBoolean("Right-Spinner", spinnerStatusLeft);
SmartDashboard::PutBoolean("S1-Status", s1Status);
SmartDashboard::PutBoolean("S2-Status", s2Status);
SmartDashboard::PutBoolean("S3-Status", s3Status);
SmartDashboard::PutNumber("Compressor-Pressure", this->compressor->GetPressureSwitchValue());
if(compressor) {
toggleCompressor();
}
++periodicCounter;
float x = stickDrive->GetX();
float y = stickDrive->GetY();
float z = (stickDrive->GetRawAxis(3)) / 1.5; //actually twist--capped at 2/3 for speed control
if(abs(x) < .125) x =0;
if(abs(y) < .125) y =0;
if(abs(z) < .125) z =0;
if(!slowSpeed) {
//myRobot.MecanumDrive_Cartesian(x, y, z, 0.0);
MecanumDrive(x, y, z, 0.0);
}
else if(slowSpeed) {
if (x < 0) x = x/3;
else x = x/3;
if (y < 0) y = y/3;
else y = y/3;
if (z < 0) z = z/3;
else z = z/3;
//myRobot.MecanumDrive_Cartesian(x, y, z, 0.0);
MecanumDrive(x, y, z, 0.0);
}
//stop robot
if (stopDrive){
stop();
return;
}
//extend all arms
if(extendAll_driver){
extend(s1,s1Status); //spinner arms go out first
extend(s3,s3Status);
Wait(.2);
extend(s2,s2Status);
return;
}
//retract all arms
if(retractAll_driver){
retract(s2,s2Status); //catcher arms come in first
Wait(.2);
retract(s3,s3Status);
retract(s1,s1Status);
return;
}
//toggle left set of spinners
if(leftDrop_driver){
toggleLeftDrop(SPINNER_SPEED);
return;
}
if(leftPick_driver){
toggleLeftPick(SPINNER_SPEED);
return;
}
//toggle right set of spinners
if(rightDrop_driver){
toggleRightDrop(SPINNER_SPEED);
return;
}
if(rightPick_driver){
toggleRightPick(SPINNER_SPEED);
return;
}
//attack3 joystick code below
//extend all arms
if(extendAll_other){
extend(s1,s1Status); //spinner arms go out first
extend(s3,s3Status);
Wait(.2);
extend(s2,s2Status);
Wait(0.5);
return;
}
//retract all arms
if(retractAll_other){
retract(s2,s2Status); //catcher arms come in first
Wait(.2);
retract(s3,s3Status);
retract(s1,s1Status);
Wait(0.5);
return;
}
if(leftDrop_other){
spinnerL1->Set(SPINNER_SPEED); //may need to
spinnerL2->Set(SPINNER_SPEED); //reverse these
return;
}
if(leftPick_other){
spinnerL1->Set(-SPINNER_SPEED); //may need to
spinnerL2->Set(-SPINNER_SPEED); //reverse these
return;
}
//toggle right set of spinners
if(rightDrop_other){
spinnerR1->Set(SPINNER_SPEED);
spinnerR2->Set(SPINNER_SPEED);
return;
}
if(rightPick_other){
spinnerR1->Set(-SPINNER_SPEED);
spinnerR2->Set(-SPINNER_SPEED);
return;
}
if (!(leftDrop_driver || leftPick_driver || leftDrop_other || leftPick_other))
{
stopSpinnersLeft();
}
if (!(rightDrop_driver || rightPick_driver || rightDrop_other || rightPick_other))
{
stopSpinnersRight();
}
//toggle left spinner arm
if(leftArm_other) {
toggle(s1,s1Status);
return;
}
//toggle right spinner arm
if(rightArm_other) {
toggle(s3,s3Status);
return;
}
}