void AutonomousContinuous(void) {
printf("Running in autonomous continuous...\n");
GetWatchdog().Feed();
if (kicker->HasBall())
{
//We have a ball, thus stop moving and kick the ball
drivetrain->Drive(0.0, 0.0);
kicker->SetKickerMode(KICKER_MODE_KICK);
} else {
//We do not have a ball
if (kicker->IsKickerInPosition())
{
//Move forward!
drivetrain->ArcadeDrive(autonomousForwardPower, 0.0);
} else {
//If not in position, wait for it to be there...
drivetrain->ArcadeDrive(0.0, 0.0);
kicker->SetKickerMode(KICKER_MODE_ARM);
}
}
//Run the kicker
kicker->Act();
}
void TeleopDrive()
{
if (fabs(m_driver->GetRawAxis(LEFT_Y)) > 0.2 || fabs(m_driver->GetRawAxis(RIGHT_X)) > 0.2)
m_robotDrive->ArcadeDrive(-m_driver->GetRawAxis(LEFT_Y),-m_driver->GetRawAxis(RIGHT_X));
else
m_robotDrive->ArcadeDrive(0.0,0.0);
}
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 during test mode
// Test
// *
void Test()
{
shifters.Set(DoubleSolenoid::kForward);
leftDriveEncoder.Start();
leftDriveEncoder.Reset();
int start = leftDriveEncoder.Get();
while (IsTest()) {
if (rightStick.GetRawButton(7)) {
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
else {
robotDrive.ArcadeDrive(rightStick.GetY()/2, -rightStick.GetX()/2);
}
if (gamepad.GetEvent(4) == kEventClosed) {
start = leftDriveEncoder.Get();
}
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "lde: %d", leftDriveEncoder.Get() - start);
dsLCD->UpdateLCD();
gamepad.Update();
}
}
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 OperatorControl()
{
float xJoy, yJoy, gyroVal, angle = 0, turn = 0, angleDiff, turnPower;
gyro.Reset();
gyro.SetSensitivity(9.7);
while (IsEnabled() && IsOperatorControl()) // loop as long as the robot is running
{
yJoy = xbox.getAxis(Xbox::L_STICK_V);
xJoy = xbox.getAxis(Xbox::R_STICK_H);
gyroVal = gyro.GetAngle()/0.242*360;
turn = 0.15;
angle = angle - xJoy * xJoy * xJoy * turn;
angleDiff = mod(angle - gyroVal, 360);
turnPower = - mod(angleDiff / 180.0 + 1.0, 2) + 1.0;
SmartDashboard::PutString("Joy1", vectorToString(xJoy, yJoy));
SmartDashboard::PutNumber("Heading", mod(gyroVal, 360));
SmartDashboard::PutNumber("Turn Power", turnPower);
SmartDashboard::PutBoolean("Switch is ON:", dumperSwitch.Get());
if (!xbox.isButtonPressed(Xbox::R))
{
drive.ArcadeDrive(yJoy * yJoy * yJoy, turnPower * fabs(turnPower), false);
}
}
}
/**
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis.
*/
void OperatorControl(void)
{
Victor armMotor(5); // create arm motor instance
while (IsOperatorControl())
{
GetWatchdog().Feed();
// determine if tank or arcade mode; default with no jumper is for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0) {
myRobot->TankDrive(leftStick, rightStick); // drive with tank style
} else{
myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick)
}
// Control the movement of the arm using the joystick
// Use the "Y" value of the arm joystick to control the movement of the arm
float armStickDirection = armStick->GetY();
// if at a limit and telling the arm to move past the limit, don't drive the motor
if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
armStickDirection = 0;
} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
armStickDirection = 0;
}
// Set the motor value
armMotor.Set(armStickDirection);
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
AnalogTrigger trig1(2);
trig1.SetLimitsVoltage(1.5, 2.5);
AnalogTrigger trig2(3);
trig2.SetLimitsVoltage(1.5, 2.5);
Encoder encoder(
trig1.CreateOutput(AnalogTriggerOutput::kRisingPulse),
trig2.CreateOutput(AnalogTriggerOutput::kRisingPulse),
false, Encoder::k2X);
double tm = GetTime();
GetWatchdog().SetEnabled(true);
while (IsOperatorControl())
{
GetWatchdog().Feed();
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
if (GetTime() - tm > 0.25)
{
printf("Encoder: %d\r", encoder.Get());
tm = GetTime();
}
Wait(0.005); // wait for a motor update time
}
}
/**
* This method is called every 20ms to update the robots components during teleop.
* There should be no blocking calls in this method (connection requests, large data collection, etc),
* failing to comply with this could result in inconsistent robot behavior
*/
void TeleopPeriodic()
{
//Make the robot move like an arcade stick controlled device
rd->ArcadeDrive(j1);
//If button 1 on the joystick is pressed, spin the shooter wheels up,
//otherwise stop them
if(j1->GetRawButton(1))
{
shoot1->Set(1);
shoot2->Set(2);
}
else
{
shoot1->Set(0);
shoot2->Set(0);
}
//If button 2 on the joystick is pressed, spin the kicker, otherwise stop the kicker
if(j1->GetRawButton(2))
{
kicker->Set(-1);
}
else
{
kicker->Set(0);
}
}
void TeleopPeriodic()
{
myRobot.ArcadeDrive(-gPad.GetRawAxis(GP_RSTICK_YAXIS)*ROBOT_START_SPEED, -gPad.GetRawAxis(GP_RSTICK_XAXIS)*ROBOT_START_SPEED ); // drive with arcade style (use right stick)
if(gPad.GetRawButton(GP_Y_BUTTON) == true && yButton == false)
{
if(driveSpeed + 0.25 < 1 && driveSpeed - 0.25 > 0)
{
yButton = true;
driveSpeed = driveSpeed + 0.25;
}
else
{
yButton = false;
}
}
if(gPad.GetRawButton(GP_A_BUTTON) == true && aButton == false)
{
if(driveSpeed + 0.25 < 1 && driveSpeed - 0.25 > 0)
{
aButton = true;
driveSpeed = driveSpeed + 0.25;
}
else
{
aButton = false;
}
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
while (IsOperatorControl() && IsEnabled())
{
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
Wait(0.005); // wait for a motor update time
}
}
void TeleopPeriodic()
{
drive_mode_t new_mode = drive_mode_chooser.GetSelected();
SmartDashboard::PutString("current mode", new_mode == TANK_DRIVE ? "Tank" : "Arcade");
if (new_mode != drive_mode)
SetDriveMode(new_mode);
if (drive_mode == TANK_DRIVE) {
left_speed = accel(left_speed, pilot->LeftY(), TICKS_TO_ACCEL);
right_speed = accel(right_speed, pilot->RightY(), TICKS_TO_ACCEL);
drive->TankDrive(left_speed, right_speed);
}
else {
rot_speed = accel(rot_speed, pilot->RightX(), TICKS_TO_ACCEL);
SmartDashboard::PutNumber("rotation speed", rot_speed);
rot_speed = pilot->RightX();
move_speed = accel(move_speed, pilot->LeftY(), TICKS_TO_ACCEL);
drive->ArcadeDrive(move_speed * MOVE_SPEED_LIMIT, -rot_speed * MOVE_SPEED_LIMIT, false);
}
SmartDashboard::PutBoolean("clamp open", clamp->isOpen());
SmartDashboard::PutBoolean("sword in", clamp->isSwordIn());
SmartDashboard::PutData("gyro", gyro);
// for (uint8 i = 0; i <= 15; ++i)
// SmartDashboard::PutNumber(std::string("current #") + std::to_string(i), pdp->GetCurrent(i));
SmartDashboard::PutNumber("Current", pdp->GetTotalCurrent());
if (pilot->ButtonState(GamepadF310::BUTTON_A)) {
clamp->open();
}
else if (pilot->ButtonState(GamepadF310::BUTTON_B)){
clamp->close();
}
clamp->update();
SmartDashboard::PutNumber("accelerometer Z", acceler->GetZ());
SmartDashboard::PutNumber("Encoder", encoder->Get());
flywheel->Set(pilot->RightTrigger());
if (pilot->LeftTrigger() != 0)
flywheel->Set(-pilot->LeftTrigger());
SmartDashboard::PutNumber("Left Trigger:", pilot->LeftTrigger());
if (pilot->ButtonState(GamepadF310::BUTTON_X)) {
cameraFeeds-> changeCam(cameraFeeds->kBtCamFront);
}
if (pilot->ButtonState(GamepadF310::BUTTON_Y)){
cameraFeeds-> changeCam(cameraFeeds->kBtCamBack);
}
cameraFeeds->run();
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
Wait(0.005); // wait for a motor update time
}
}
void Autonomous()
{
double forwardTime = SmartDashboard::GetNumber("ForwardTime",2.0);
double forwardSpeed = SmartDashboard::GetNumber("ForwardSpeed",0.5);
double twistTime = SmartDashboard::GetNumber("TwistTime",2.0);
double twistSpeed = SmartDashboard::GetNumber("TwistSpeed",0.5);
Timer *timer = new Timer();
timer->Start();
// int Loop = SmartDashboard::GetNumber("ForwardTime",400);
// autoLoopCounter = 0;
while(timer->Get() < forwardTime && IsEnabled())
{
robotDrive.ArcadeDrive(0.0, forwardSpeed);
// autoLoopCounter++;
// SmartDashboard::PutNumber("Counter",autoLoopCounter);
Wait(.005);
}
timer->Reset();
// int Loop2 = SmartDashboard::GetNumber("TwistTime",400);
// autoLoopCounter2 = 0;
while(timer->Get() < twistTime && IsEnabled())
{
robotDrive.ArcadeDrive(twistSpeed, 0.0);
autoLoopCounter2++;
Wait(.005);
}
timer->Stop();
robotDrive.ArcadeDrive(0.0,0.0);
stateDisarmed = false;
stateArming1 = false;
stateArming2 = false;
stateArmed = true;
stateFiring1 = false;
stateFiring2 = false;
free(timer);
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
while (IsOperatorControl() && IsEnabled())
{
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
//camera.GetImage(Image); //get the most recent image from the camera
Wait(0.005); // wait for a motor update time
}
}
void TeleopContinuous(void) {
printf("Running in teleop continuous...\n");
GetWatchdog().Feed();
//Drive the robot
drivetrain->ArcadeDrive(driverJoystick->GetRawAxis(2) * -1,driverJoystick->GetRawAxis(4) * -1);
//Run the kicker
kicker->Act();
}
/**
* Code to be run during the remaining 2:20 of the match (after Autonomous())
*/
void OperatorControl()
{
/* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was
* the only way we could get out robot code to work (reliably). Should this be set to false?
*/
robotDrive.SetSafetyEnabled(true);
while (IsOperatorControl())
{
robotDrive.ArcadeDrive(rightStick);
Wait(0.005);
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
myRobot.ArcadeDrive(stick.getAxisLeftY(), stick.getAxisLeftX()); // drive with arcade style (use left stick)
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Move: %f4", stick.getAxisLeftY());
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Rotate: %f4", stick.getAxisLeftX());
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
cout << "Hello operator!" << endl;
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl())
{
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
Wait(0.005); // wait for a motor update time
}
cout << "Bye operator!" << endl;
}
void HandleDriverInputsManual(void)
{
myRobot.ArcadeDrive(stick);
if(kEventClosed == stick2.GetEvent(BUTTON_SHIFT))
{
// Shift into high gear.
shifter.Set(DoubleSolenoid::kForward);
}
else if(kEventOpened == stick2.GetEvent(BUTTON_SHIFT))
{
// Shift into low gear.
shifter.Set(DoubleSolenoid::kReverse);
}
}
void stateCentering() {
stateTimer++;
if (stateTimer == 1) {
turnController->SetSetpoint(0.0);
turnController->Enable();
return;
} else if (stateTimer < 10) {
return;
} else if (stateTimer < 120) {
if (!turnPIDSource->acquired()) {
robotState = kOperatorControl;
turnController->Disable();
turnPIDSource->reset();
LOGGER(ERROR) << "[stateCentering] No Target Found";
return;
}
drive->ArcadeDrive(0.0, -turnPIDOutput.correction, false);
LOGGER(DEBUG) << "[stateCentering] Timer: " << stateTimer << " SetPoint: " << turnController->GetSetpoint()
<< " Offset: " << turnPIDSource->PIDGet() << " Correction: " << turnPIDOutput.correction;
if ((fabs(turnPIDOutput.correction) < 0.10) && (fabs(turnPIDSource->PIDGet()) < 3)) {
drive->ArcadeDrive(0.0, 0.0, false);
turnController->Disable();
robotState = kAiming;
stateTimer = 0;
turnPIDSource->reset();
}
} else {
turnController->Disable();
turnPIDSource->reset();
drive->ArcadeDrive(0.0, 0.0, false);
robotState = kOperatorControl;
LOGGER(ERROR) << "[stateCentering] FAILED, Timer: " << stateTimer << " SetPoint: " << turnController->GetSetpoint()
<< " Offset: " << turnPIDSource->PIDGet() << " Correction: " << turnPIDOutput.correction;
}
}
// HandleDriverInputs
// * Drive motors according to joystick values
// * Shift (Button 7 on left joystick)
// ----> ASSUMES kForward = high gear
void HandleDriverInputs()
{
if(kEventOpened == leftStick.GetEvent(BUTTON_SHIFT))
{
// Shift into high gear.
shifters.Set(DoubleSolenoid::kForward);
}
else if(kEventClosed == leftStick.GetEvent(BUTTON_SHIFT))
{
// Shift into low gear.
shifters.Set(DoubleSolenoid::kReverse);
}
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
/**
* unchanged from SimpleDemo:
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0.
*/
void OperatorControl(void)
{
DPRINTF(LOG_DEBUG, "OperatorControl");
GetWatchdog().Feed();
while (1)
{
// determine if tank or arcade mode; default with no jumper is for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0) {
myRobot->TankDrive(leftStick, rightStick); // drive with tank style
} else{
myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick)
}
}
} // end operator control
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl() && IsEnabled())
{
myRobot.ArcadeDrive(joystick); // drive with arcade style (use right stick)
if(joystick.GetRawButton(1)) {
crochets.Set(-0.6);
}
else if(joystick.GetRawButton(2)) {
crochets.Set(0.6);
}
Wait(0.005); // wait for a motor update time
}
}
// LaunchCatapult
// * If in the correct state to launch (loaded), launches the catapult.
void LaunchCatapult()
{
if (loaded)
{
winch.Set(WINCH_FWD);
for (int i = 0; i < 75; i++)
{
if (IsOperatorControl())
{
robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
}
Wait (0.01);
}
// Wait(CATAPULT_SHOOT_WAIT);
winch.Set(0.0);
loaded = false;
}
}
void OperatorControl(void)
{
GetWatchdog().SetEnabled(true);
printf("Entered OperatorControl\n");
while (IsOperatorControl())
{
GetWatchdog().Feed();
// use the trigger to start recording.. at the moment,
// it just gets ignored if you call it more than once
if (stick.GetTrigger())
recorder.StartRecording();
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
// always call the recording routine
recorder.Record();
}
}
/**
* 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
/*
* 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 TeleopPeriodic()
{
myRobot.ArcadeDrive(stick);
}
void stateOperatorControl() {
// DRIVING
move = stick->GetRawAxis(1) * -1.0;
rotate = stick->GetRawAxis(4) * -1.0;
// Deadband
if (fabs(move) < 0.1) {
move = 0.0;
}
if (fabs(rotate) < 0.15) {
rotate = 0.0;
}
drive->ArcadeDrive(move, rotate, false);
// Joystick Buttons
bool button4 = stick->GetRawButton(4);
bool button1 = stick->GetRawButton(1);
bool button5 = stick->GetRawButton(5);
bool button6 = stick->GetRawButton(6);
bool button3 = stick->GetRawButton(3);
// Manual Gatherer
if (stick->GetRawAxis(2) != 0) {
gatherSpeed = stick->GetRawAxis(2);
LOGGER(DEBUG) << "[stateOperatorControl] Gather Angle:" << gatherPIDSource.PIDGet();
}
else if (stick->GetRawAxis(3) != 0) {
gatherSpeed = stick->GetRawAxis(3) * -1;
LOGGER(DEBUG) << "[stateOperatorControl] Gather Angle:" << gatherPIDSource.PIDGet();
}
else {
gatherSpeed = 0.0;
}
gatherer->Set(gatherSpeed);
// Launch Angle
double launcherAngle = launchPIDSource.PIDGet();
if (button5 && !button6 && (launcherAngle < kLaunchMaxAngle)) {
elevator->Set(-0.5); // Up
LOGGER(DEBUG) << "[stateOperatorControl] Launcher Angle:" << launcherAngle;
} else if (button6 && !button5 && (launcherAngle > kLaunchMinAngle)) {
LOGGER(DEBUG) << "[stateOperatorControl] Launcher Angle:" << launcherAngle;
elevator->Set(0.5); // Down
} else {
elevator->Set(0.0);
}
// Auto-Gather
if (button3 && !lastButton3) {
wheelsGathererIn = !wheelsGathererIn;
gatherController->SetSetpoint(kGatherAngle);
gatherController->Enable();
}
if (wheelsGathererIn) {
gathererWheels->Set(1.0);
gatherer->Set(gatherPIDOutput.correction);
LOGGER(DEBUG) << "[stateOperatorControl] Gather Correction:" << gatherPIDOutput.correction
<< " Gather Angle: " << gatherPIDSource.PIDGet();
} else {
gathererWheels->Set(0.0);
gatherController->Disable();
}
if (button4 && !lastButton4) {
stateTimer = 0;
robotState = kCentering;
shootingHigh = true;
}
if (button1 && !lastButton1) {
stateTimer = 0;
robotState = kLaunching;
shootingHigh = true;
}
lastButton4 = button4;
lastButton1 = button1;
lastButton3 = button3;
}