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();
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
myRobot.Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
myRobot.SetSafetyEnabled(false);
myRobot.Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
}
Robot() :
robotDrive(frontLeftChannel, rearLeftChannel,
frontRightChannel, rearRightChannel), // initialize variables in same
stick(joystickChannel) // order as declared above.
{
rotateToAngleRate = 0.0f;
robotDrive.SetExpiration(0.1);
robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // invert left side motors
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // change to match your robot
try {
/* Communicate w/navX MXP via the MXP SPI Bus. */
/* Alternatively: I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
ahrs = new AHRS(SPI::Port::kMXP);
} catch (std::exception ex ) {
std::string err_string = "Error instantiating navX MXP: ";
err_string += ex.what();
DriverStation::ReportError(err_string.c_str());
}
turnController = new PIDController(kP, kI, kD, kF, ahrs, this);
turnController->SetInputRange(-180.0f, 180.0f);
turnController->SetOutputRange(-1.0, 1.0);
turnController->SetAbsoluteTolerance(kToleranceDegrees);
turnController->SetContinuous(true);
/* Add the PID Controller to the Test-mode dashboard, allowing manual */
/* tuning of the Turn Controller's P, I and D coefficients. */
/* Typically, only the P value needs to be modified. */
LiveWindow::GetInstance()->AddActuator("DriveSystem", "RotateController", turnController);
if ( ahrs ) {
LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
}
}
//Choose which auto to use
void AutonomousInit() {
chainLift.SetSpeed(0);
canGrabber.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "STARTING AUTO");
robotDrive.SetSafetyEnabled(false);
chainLift.SetSafetyEnabled(false);
SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get());
//Select auto type
if (autoSwitch1.Get()) {
if (autoSwitch2.Get())
AutonomousType4();
else
//1 on 2 grab n back
AutonomousType8();
} else {
if (autoSwitch2.Get())
//1 off, 2 on: grab n turn
AutonomousType10();
else {
SmartAutoPicker();
}
//Do Nothing
}
}
/**
* 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
}
}
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();
}
//robot turns to desired position with a deadband of 2 degrees in each direction
bool GyroTurn (float desiredTurnAngle, float turnSpeed)
{
bool turning = true;
float myAngle = gyro->GetAngle();
//normalizes angle from gyro to [-180,180) with zero as straight ahead
while(myAngle >= 180)
{
GetWatchdog().Feed();
myAngle = myAngle - 360;
}
while(myAngle < -180)
{
GetWatchdog().Feed();
myAngle = myAngle + 360;
}
if(myAngle < (desiredTurnAngle - 2))// if robot is too far left, turn right a bit
{
myRobot->Drive(turnSpeed, -turnSpeed); //(right,left)
}
if(myAngle > (desiredTurnAngle + 2))// if robot is too far right, turn left a bit
{
myRobot->Drive(-turnSpeed, turnSpeed); //(right,left)
}
else
{
myRobot->Drive(0, 0);
turning = false;
}
return turning;
}
DragonBotDriveTrain(void) {
drive = new RobotDrive(
new Victor(FRONT_LEFT_PWM),
new Victor(BACK_LEFT_PWM),
new Victor(FRONT_RIGHT_PWM),
new Victor(BACK_RIGHT_PWM)
);
drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
//makingSmoke = false;
smoke_cannon = new Victor(SMOKE_CANNON_PWM);
left_eye_x = new Servo(LEFT_EYE_X_PWM);
//left_eye_y = new Servo(LEFT_EYE_Y_PWM);
right_eye_x = new Servo(RIGHT_EYE_X_PWM);
//right_eye_y = new Servo(RIGHT_EYE_Y_PWM);
gamepad = new Gamepad(1);
gamepad2 = new Gamepad(2);
smoke_machine = new DigitalOutput(SMOKE_MACHINE_RELAY);
lcd = DriverStationLCD::GetInstance();
jaw_motor = new Victor(JAW_MOTOR_PWM);
head_motor = new Victor(HEAD_MOTOR_PWM);
tophead_limit = new DigitalInput(TOPHEAD_LIMIT_PIN);
bottomjaw_limit = new DigitalInput(BOTTOMJAW_LIMIT_PIN);
crash_limit = new DigitalInput(CRASH_LIMIT_PIN);
default_eye_position = 15.0f; //TODO: figure this out
making_smoke_timer = new Timer();
firing_smoke_timer = new Timer();
}
// 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();
}
}
/**
* This method is called every 20ms to update the robots components during autonomous.
* 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 AutonomousPeriodic()
{
//In the first two seconds of auto, drive forwardat half power
if(Timer::GetFPGATimestamp() - autoTimer >= 0 && Timer::GetFPGATimestamp() < 2)
{
rd->SetLeftRightMotorOutputs(.5, .5);
}
else if(Timer::GetFPGATimestamp() - autoTimer >= 2 && Timer::GetFPGATimestamp() < 4)
{
//2 to 4 seconds, stop drivetrain and spin shooter wheels
rd->SetLeftRightMotorOutputs(0, 0);
shoot1->Set(1);
shoot2->Set(1);
}
else if(Timer::GetFPGATimestamp() - autoTimer >= 4 && Timer::GetFPGATimestamp() < 8 && !kickerSwitch->Get())
{
//4 to 8 seconds and while kicker switch is not true, spin shooter wheels and kicker
shoot1->Set(1);
shoot2->Set(1);
kicker->Set(-1);
}
else
{
//After all that, stop everything
rd->SetLeftRightMotorOutputs(0, 0);
shoot1->Set(0);
shoot2->Set(0);
kicker->Set(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 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);
}
}
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);
}
//Drive base control
void DriveBaseControl(void){
//Local declarations
float driveThreshold = 0.005;
//Get the y-axis of the joystick
float yAxis1 = 1 * leftStick.GetY();
float yAxis2 = 1 * rightStick.GetY();
//std::cout << "yAxisVal: " << yAxisVal << std::endl;
//Convert input to [-1.0, 1.0] range
//Don't need to - already in the correct range
//Drive the drive motors when any input is within +-driveThreshold of 0.0
//NOTE - currently this doesn't scale up the input from 0.0 after the deadband region -- it just uses the raw value.
if(yAxis1 >= driveThreshold || yAxis2 >= driveThreshold || yAxis1 <= -driveThreshold || yAxis2 <= -driveThreshold )
{
robotDrive.TankDrive(-yAxis1,-yAxis2); // drive Forwards
} else {
robotDrive.TankDrive(0.0, 0.0); // stop robot
}
}
void FunctionBot::driveX(int x)
{
m_RawBot->resetEncoders();
correct=m_RawBot->m_Gyro->GetAngle();
correct/=30;
correct*=-1;
float traveled;
float traveledmode;
traveled=m_RawBot->averageEncoders();
traveledmode=m_RawBot->averageEncoders()+abs(x);
//traveledmode=(traveledmode * 8 * 3.14) + abs(x);
if(x<0)
{
while(abs(x)-m_RawBot->averageEncoders()>1&&!IsOperatorControl())
{
drive->MecanumDrive_Cartesian(0,-.15,0,0);//numbers might be wrong get checked
traveled=m_RawBot->averageEncoders();
}
}
else
{
while(abs(x)-m_RawBot->averageEncoders()>1&&!IsOperatorControl())
{
drive->MecanumDrive_Cartesian(0,.15,0,0);//numbers might be wrong get checked
traveled=m_RawBot->averageEncoders();
}
}
drive->MecanumDrive_Cartesian(0,0,0,0);
}
void TeleopPeriodic(void )
{
/*
* Code placed in here will be called only when a new packet of information
* has been received by the Driver Station. Any code which needs new information
* from the DS should go in here
*/
//Start compressor
compressor->Start();
driveTrainValues();
deadzone();
//Drivetrain.....
//When button eight is pressed robot drives at 25% speed
printf("right: %f and left: %f\n", useright, useleft);
if (gamepad->GetRawButton(8))
{
drivetrain->TankDrive((-0.5*(useleft)), (-0.5*(useright)));
//Negative for switched wires
}
else
{
drivetrain->SetLeftRightMotorOutputs(-useleft, -useright);
//Normal driving
//Negative for switched wires
}
}
void AutonomousType4() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 4");
//Lift, turn, drive
chainLift.SetSpeed(0.5);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
if (WaitF(2.5))
return;
robotDrive.MecanumDrive_Polar(0.25, 0, 0);
if (WaitF(5.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
if (WaitF(2))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
//chainLift.SetSpeed(-0.4);
//while (maxDown.Get() && IsAutonomous()) {
//}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 4 COMPLETE");
}
void Drive (float speed, int dist)
{
leftDriveEncoder.Reset();
leftDriveEncoder.Start();
int reading = 0;
dist = abs(dist);
// The encoder.Reset() method seems not to set Get() values back to zero,
// so we use a variable to capture the initial value.
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "initial=%d\n", leftDriveEncoder.Get());
dsLCD->UpdateLCD();
// Start moving the robot
robotDrive.Drive(speed, 0.0);
while ((IsAutonomous()) && (reading <= dist))
{
reading = abs(leftDriveEncoder.Get());
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "reading=%d\n", reading);
dsLCD->UpdateLCD();
}
robotDrive.Drive(0.0, 0.0);
leftDriveEncoder.Stop();
}
Robot() :
timer(),
robotDrive(Constants::driveFrontLeftPin, Constants::driveRearLeftPin, Constants::driveFrontRightPin, Constants::driveRearRightPin),//tells the robot where everything is plugged in
i2c(I2C::kOnboard,Constants::ledAddress),
driveStick(Constants::driveStickChannel),
grabStick(Constants::grabStickChannel),
prox(Constants::driveUltrasonicPin),
grabTalon(Constants::grabTalonPin),
grabInnerLimit(Constants::grabInnerLimitPin),
grabOuterLimit(Constants::grabOuterLimitPin),
liftTalon(Constants::liftTalonPin),
liftEncoder(Constants::liftEncoderAPin, Constants::liftEncoderBPin),
liftUpperLimit(Constants::liftUpperLimitPin),
liftLowerLimit(Constants::liftLowerLimitPin),
grabEncoder(Constants::grabEncoderAPin, Constants::grabEncoderBPin),
pdp(),
gyro(Constants::driveGyroRate),
pickup(grabTalon, grabInnerLimit, grabOuterLimit, liftTalon, liftEncoder, liftUpperLimit, liftLowerLimit, pdp)
{
robotDrive.SetExpiration(0.1); // safety feature
robotDrive.SetInvertedMotor(RobotDrive::kFrontRightMotor, true); // make the motors go the right way
robotDrive.SetInvertedMotor(RobotDrive::kRearRightMotor, true);
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
liftEncoder.SetReverseDirection(Constants::liftEncoderReverse);
}
/*this fuction will need to be updated for the final robot*/
void FunctionBot::configDrive() {
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
//drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor,true);
drive->SetSafetyEnabled(false);
//not sure on this value at all
drive->SetMaxOutput(333);
}
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();
}
Robot() :
robotDrive(new Talon(frontLeftChannel), new Talon(rearLeftChannel),
new Talon(frontRightChannel), new Talon(rearRightChannel)), stick(
STICK_CHANNEL), liftStick(LIFTSTICK_CHANNEL), chainLift(
CHAINLIFT_PWM), maxUp(MAXUP_DIO), maxDown(MAXDOWN_DIO), midPoint(
MIDPOINT_DIO), autoSwitch1(AUTOSWITCH1_DIO), autoSwitch2(
AUTOSWITCH2_DIO), leftIR(LEFTIR_LOC), rightIR(RIGHTIR_LOC), canGrabber(
CANGRAB_PWM) {
SmartDashboard::init();
ds = DriverStation::GetInstance();
SmartDashboard::PutString("STATUS:", "INITIALIZING");
robotDrive.SetExpiration(0.1);
robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
autoMode = new SendableChooser();
autoMode->AddDefault("00: Do Nothing", new AutonomousIndex(0));
autoMode->AddObject("01: Just Drive Forward", new AutonomousIndex(1));
autoMode->AddObject("02: Stack bin on tote, turn 90, go",
new AutonomousIndex(2));
autoMode->AddObject("03: Lift up and go forward",
new AutonomousIndex(3));
autoMode->AddObject("04: Lift up, turn 90, go", new AutonomousIndex(4));
autoMode->AddObject("05: Stack 3 bins w/ correction constant",
new AutonomousIndex(5));
autoMode->AddObject("06: Stack 3 bins w/ acclerometer",
new AutonomousIndex(6));
autoMode->AddObject("07: Grab first bin from landfill",
new AutonomousIndex(7));
autoMode->AddObject("08: Grab yellow bin and back up TO RAMP",
new AutonomousIndex(8));
autoMode->AddObject("09: Grab two bins from landfill, slide sideways",
new AutonomousIndex(9));
autoMode->AddObject("10: Grab from landfill, over by turning",
new AutonomousIndex(10));
autoMode->AddObject("11: Grab yellow bin, back up to AUTOZONE, drop",
new AutonomousIndex(11));
autoMode->AddObject("12: Grab yellow bin, back up to AUTOZONE, stop",
new AutonomousIndex(12));
autoMode->AddObject("13: Steal two green cans. Gottta go fast",
new AutonomousIndex(13));
autoMode->AddObject("99: CUPID SHUFFLE", new AutonomousIndex(99));
SmartDashboard::PutString("Both Off", "Pick from radio list");
SmartDashboard::PutString("On-Off", "Grab yellow and back up");
SmartDashboard::PutString("Off-On",
"Grab, turn right, drive, turn left");
SmartDashboard::PutString("Both On",
"Lift up, turn 90, drive to auto zone");
SmartDashboard::PutData("BOTH SWITCHES ON: Pick One:", autoMode);
SmartDashboard::PutData(Scheduler::GetInstance());
SmartDashboard::PutString("STATUS:", "READY");
SmartDashboard::PutBoolean("Smart Dashboard Enabled", false);
tick = 0;
leftIRZero = 0;
rightIRZero = 0;
}
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);
}
Wheels()
{
drive = new RobotDrive(0,2,4,3); //frontLeft, rearLeft, frontRight, rearRight
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // 0 is front left wheel
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // 2 is back left wheel
drive->SetExpiration(0.1);
drive->SetSafetyEnabled(true);
driveSafety = new MotorSafetyHelper(drive);
}
void AutonomousType1() { //Just drive forward
SmartDashboard::PutString("STATUS:", "STARTING AUTO 1");
//<TODO>-0.2 y for 3 seconds =43 inches --> 1 second at full speed is 71.66667 inches
robotDrive.MecanumDrive_Cartesian(0, -0.36, 0);
if (WaitF(1.75))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 1 COMPLETE");
}
void TeleopInit() override {
drive->SetExpiration(200000);
drive->SetSafetyEnabled(false);
liftdown->Set(false);
intake_hold = false;
lastLiftPos = 0;
manual = true;
}
/**
* 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
}
}
/**
* Drive left & right motors for 2 seconds, enabled by a jumper (jumper
* must be in for autonomous to operate).
*/
void Autonomous(void) {
myRobot->SetSafetyEnabled(false);
if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1) // only run the autonomous program if jumper is in place
{
myRobot->Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot->Drive(0.0, 0.0); // stop robot\
}
myRobot->SetSafetyEnabled(true);
}
void AutonomousPeriodic()
{
if(autoLoopCounter < 100) //Check if we've completed 100 loops (approximately 2 seconds)
{
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
autoLoopCounter++;
} else {
myRobot.Drive(0.0, 0.0); // stop robot
}
}