/**
* Initialization code for autonomous mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters autonomous mode.
*/
void RA14Robot::AutonomousInit() {
Config::LoadFromFile("config.txt");
auto_case = (int) Config::GetSetting("auto_case", 1);
alreadyInitialized = true;
auto_timer->Reset();
auto_timer->Start();
missionTimer->Start();
myDrive->ResetOdometer();
myCamera->Set(Relay::kForward);
myCollection->ExtendArm();
cout<<"Reseting Gyro"<<endl;
gyro->Reset();
//myOdometer->Reset();
//myDrive->ShiftUp();
myDrive->ShiftDown();
//shift to high gear
if (!fout.is_open()) {
cout << "Opening logging.csv..." << endl;
fout.open("logging.csv");
logheaders();
}
auto_state = 0;
#ifndef DISABLE_SHOOTER
myCam->Reset();
myCam->Enable();
#endif //Ends DISABLE_SHOOTER
}
void DisabledInit() {
//Config loading
try {
cameraLight->Set(Relay::kOff);
if (!Config::LoadFromFile("config.txt")) {
cout << "Something happened during the load." << endl;
}
Config::Dump();
myDrive->DisablePID();
myDrive->ResetPID();
if(fout.is_open() && !freshStart && !ds->IsFMSAttached()){
fout.close();
myShooter->ResetShooterProcess();
lc->holdState(false);
}
} catch (exception ex) {
cout << "Disabled exception. Trying again." << endl;
cout << "Exception: " << ex.what() << endl;
}
//ResetShooterMotors();
/*
SmartDashboard::PutNumber("Target Info S",1741);
cout<<SmartDashboard::GetNumber("Target Info S");
*/
}
void OperatorControl()
{
tank.StartDrive();
kick.StartKicker();
while (IsOperatorControl())
{
comp.checkCompressor();
tank.Drive(PrimaryController.GetRawAxis(LEFT_JOYSTICK), PrimaryController.GetRawAxis(RIGHT_JOYSTICK), (int)PrimaryController.GetRawButton(BUTTON_HIGH_DRIVE_SHIFT), (int)PrimaryController.GetRawButton(BUTTON_LOW_DRIVE_SHIFT));
kick.StateMachine(PrimaryController.GetRawButton(BUTTON_KICK));
collect.runCollector(PrimaryController.GetRawButton(BUTTON_ROLLER_ON));
}
}
Robot() :
// these must be initialized in the same order
// as they are declared above.
//driveTrain(),
m_stick(Port::joystickChannel),
m_gamepad(Port::gamepadChannel),
m_controller(&m_stick, &m_gamepad)
{
driveTrain.SetExpiration(0.1);
// invert the left side motors
driveTrain.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
// you may need to change or remove this to match your robot
driveTrain.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
}
void TeleopInit() {
//Open logging
if(!fout.is_open()) {
fout.open("logging.csv");
logheaders();
}
cameraLight->Set(Relay::kOff);
myDrive->SetPID(KP,KI,KD);
myDrive->EnablePID();
manualAngle = 0;
compressor->Start();
myDrive->setGyroDrive(true);
lc->setMode(0);
freshStart = false;
}
/**
* Initialization code for teleop mode should go here.
*
* Use this method for initialization code which will be called each time
* the robot enters teleop mode.
*/
void RA14Robot::TeleopInit() {
Config::LoadFromFile("config.txt");
myCompressor->Start();
alreadyInitialized = true;
missionTimer->Start();
myDrive->ResetOdometer();
myDrive->ShiftUp();
//myOdometer->Reset();
if (!fout.is_open()) {
cout << "Opening logging.csv..." << endl;
fout.open("logging.csv");
logheaders();
}
#ifndef DISABLE_SHOOTER
myCam->Reset();
myCam->Enable();
#endif //Ends DISABLE_SHOOTER
}
void RA14Robot::logheaders() {
fout << "MissionTimer,";
#ifndef DISABLE_SHOOTER
myCam->logHeaders(fout);
#endif //Ends DISABLE_SHOOTER
myDrive->logHeaders(fout);
fout << "CAMLeftCurrent,CAMRightCurrent,DriveLeftCurrent,DriveRightCurrent,AutoCase,GyroHeading,DropSensor,BatteryVoltage,";
fout << "TargetValid,TargetHot,TargetDistance,TargetX,TargetY,TargetIsLeft,TargetIsRight,MatchTime,AutoInternalState,";
fout << endl;
}
/*
* Checks if the string from the Auto Modes meets any of the constants above. If so, it runs that Auto Mode.
* Right now has testing modes, but autonomous->driveOverDefense(); will do the checking for us.
*/
void AutonomousPeriodic(){
//autonomous->driveOverDefense();
const int kModesEqual=0;
//autonomous->test();
if (autoSelected.compare(DoNothing)==kModesEqual){
autonomous->doNothing();
SmartDashboard::PutString("DN TEST", "Done");
}
else if (autoSelected.compare(autoNameOver)==kModesEqual){
autonomous->driveOverDefense();
}
else if (autoSelected.compare(autoNameRW)==kModesEqual){
autonomous->rockWallOrMoat();
}
else if (autoSelected.compare(autoNameLeft)==kModesEqual){
autonomous->leftLowGoal();
}
else if (autoSelected.compare(autoNameRight)==kModesEqual){
autonomous->rightLowGoal();
}
else if (autoSelected.compare(autoNameP1)==kModesEqual){
autonomous->p1High();
}
else if (autoSelected.compare(autoNameP2)==kModesEqual){
autonomous->p2High();
}
else if (autoSelected.compare(autoNameP3)==kModesEqual){
autonomous->p3High();
}
else if (autoSelected.compare(autoNameP4)==kModesEqual){
autonomous->p4High();
}
else if (autoSelected.compare(autoNameP5)==kModesEqual){
autonomous->p5High();
}
Drive->stayAtTheTop();
}
/**
* Runs the motors with Mecanum drive.
*/
void OperatorControl()
{
driveTrain.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.
// TODO change stick to m_controller
//driveTrain.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ());
// wait 5ms to avoid hogging CPU cycles
Wait(0.005);
}
}
void AutonomousInit(){
SmartDashboard::PutString("AutonomousInit", "Autonomous Init Reached");
autonomous->setSelected(*((std::string*)chooser->GetSelected()));
//autoSelected = SmartDashboard::GetString("Auto Selector", *((std::string*)chooser->GetSelected()));
autoSelected = *((std::string*)chooser->GetSelected());
SmartDashboard::PutString("Auto Selected", autoSelected);
//testSelected= SmartDashboard::GetString("Auto Selector", *((std::string*)chooser2->GetSelected()));
testSelected = *((std::string*)chooser2->GetSelected());
Drive->AutonomousInit();
autonomous->startTimer();
autonomous->reset();
}
//logging functions
void logheaders()
{
fout << "Time,Mode,Battery,Gyro,";
autoLogHeaders();
myDrive->logHeaders(fout);
myShooter->logHeaders(fout);
//myClimber->LogHeaders(fout);
log_gamepad_headers(fout, "Driver");
log_gamepad_headers(fout, "Gamepad");
//autoLogHeaders();
fout << "FancyLights,";
fout << "Light1,";
fout << "Light2,";
fout<<endl;
}
void logdata()
{
//log data
fout << logTimer->Get() << ',' << CurrentMode() << ',' << ds->GetBatteryVoltage() << ',';
fout << magicBox->getGyroAngle() << ',';
autoLog();
myDrive->log(fout);
myShooter->log(fout);
//myClimber->Log(fout);
log_gamepad(fout, driverGamepad);
log_gamepad(fout, operatorGamepad);
fout<<LEDTimer<<",";
fout<<LEDPercent(LEDTimer)<<",";
fout<<LEDPercent(LEDTimer + 30)<<",";
fout<<endl;
}
void RA14Robot::logging() {
if (fout.is_open()) {
fout << missionTimer->Get() << ",";
#ifndef DISABLE_SHOOTER
myCam->log(fout);
#endif //Ends DISABLE_SHOOTER
myDrive->log(fout);
CurrentSensorSlot * slots[4] = { camMotor1Slot, camMotor2Slot,
driveLeftSlot, driveRightSlot };
DriverStation * ds = DriverStation::GetInstance();
for (int i = 0; i < 4; ++i) {
fout << slots[i]->Get() << ",";
}
fout << auto_case << "," << gyro->GetAngle() << "," << dropSensor->GetPosition() << "," << ds->GetBatteryVoltage() << ",";
fout << target->IsValid() << "," << target->IsHot() << "," << target->GetDistance() << "," << target->GetX() << ",";
fout << target->GetY() << "," << target->IsLeft() << "," << target->IsRight() << ",";
fout << ds->GetMatchTime() << "," << auto_state << ",";
fout << endl;
}
}
/*Initializes objects necessary for teleop
* Adds options to sendable chooser on SmartDashboard
*/
void RobotInit(){
/*if (fork()==0){
system("/home/lvuser/start_vision &");
}*/
//creates DriveTrain and sendable choosers
Drive= new DriveTrain();
chooser = new SendableChooser();
chooser2 = new SendableChooser();
//adds options to both
chooser->AddDefault(DoNothing, (void*)&DoNothing);
chooser->AddObject(autoNameLeft, (void*)&autoNameLeft);
chooser->AddObject(autoNameRight, (void*)&autoNameRight);
chooser->AddObject(autoNameOver, (void*)&autoNameOver);
chooser->AddObject(autoNameP1, (void*)&autoNameP1);
chooser->AddObject(autoNameP2, (void*)&autoNameP2);
chooser->AddObject(autoNameP3, (void*)&autoNameP3);
chooser->AddObject(autoNameP4, (void*)&autoNameP4);
chooser->AddObject(autoNameP5, (void*)&autoNameP5);
chooser->AddObject(autoNameRW, (void*)&autoNameRW);
SmartDashboard::PutData("Auto Modes", chooser);
chooser2->AddDefault(autoNameTest1, (void*)&autoNameTest1);
chooser2->AddObject(autoNameTest2, (void*)&autoNameTest2);
chooser2->AddObject(autoNameTest3, (void*)&autoNameTest3);
chooser2->AddObject(autoNameTest4, (void*)&autoNameTest4);
SmartDashboard::PutData("Auto Modes 2", chooser2);
Drive->AutonomousInit();
autonomous = new Autonomous(Drive, *((std::string*)chooser->GetSelected()));
//passes driveTrain and sendable chooser option to Autonomous class and creates it!
}
//completes all necessary functions for teleop driving/operating
void TeleopPeriodic(){
Drive->Drive();
Drive->stayAtTheTop();
}
void TeleopPeriodic() {
//Target * target = GetLatestTarget();
//Begin DRIVER input section
if(driverGamepad->GetDPad()==Gamepad::kUp)
lc->setMode(1);
else if(driverGamepad->GetDPad()==Gamepad::kDown)
lc->setMode(6);
else
lc->setMode(0);
if(driverGamepad->GetDPad() == Gamepad::kRight)
{
cameraLight->Set(Relay::kOff);
}
else if(driverGamepad->GetDPad() == Gamepad::kLeft)
{
cameraLight->Set(Relay::kOn);
}
if(driverGamepad->GetB())
{
cout << "Resetting the gyro! :D" << endl;
//driveGyro->Reset();
magicBox->resetGyro();
}
bool modifySpeed = driverGamepad->GetRightBumper();
bool halfSpeed = driverGamepad->GetLeftBumper();
/*
try {
std::string temp = SmartDashboard::GetString("TargetInfo");
cout << "temp: " << endl;
} catch (exception ex) {
cout << "bah! " << ex.what() << endl;
}*/
//Target t = GetLatestTarget();
/*
if (t.IsValid()) {
cout << "Valid target (" << (t.IsCenter() ? "center" : "side") << "):" << t.Distance() << " ft, " << t.X() << "," << t.Y() << endl;
} else {
//cout << "No valid target." << endl;
} */
if(driverGamepad->GetBack())
{
cout << "Now driving in relation to the robot" << endl;
myDrive->setGyroDrive(false);
}
if(driverGamepad->GetStart())
{
cout << "Now driving in relation to the field (gyro)" << endl;
myDrive->setGyroDrive(true);
}
// If the driver is holding the X button
/*
if (driverGamepad->GetX()) {
// Stop using the joysticks to drive the robot, and let this function do it
PointAtTarget(t);
} else { */
// We aren't auto-targeting. Reset the sequence.
target_state = RA13Robot::READY;
// Drive based on joysticks.
myDrive->Drive(driverGamepad->GetLeftX(), driverGamepad->GetLeftY(),
driverGamepad->GetRightX(), magicBox->getGyroAngle(), modifySpeed,halfSpeed);
//}
//End DRIVER input section
Gamepad::DPadDirection dir = operatorGamepad->GetDPad();
if (dir != lastdir) {
switch(dir) {
case Gamepad::kUp:
manualAngle += 1;
break;
case Gamepad::kDown:
manualAngle -= 1;
break;
case Gamepad::kRight:
manualAngle += 0.25;
break;
case Gamepad::kLeft:
manualAngle -= 0.25;
break;
default:
break;
}
}
//cout << "Current target angle: " << manualAngle << endl;
//Begin OPERATOR input section
if(operatorGamepad->GetBack())
{
myShooter->ResetShooterProcess(); // Re-homes the shooter
}
if(operatorGamepad->GetLeftTrigger())
{
myShooter->SetSpeeds(SHOOTER_FRONTWHEEL_SPEED,SHOOTER_REARWHEEL_SPEED); // (RPM,RPM)
}
else if (operatorGamepad->GetLeftBumper())
{
myShooter->SetSpeeds(Config::GetSetting("shooter_main_speed_pyramid", 1500),
Config::GetSetting("shooter_secondary_speed_pyramid", 2000));
}
else
{
myShooter->SetSpeeds(0,0); // (RPM,RPM)
}
if(operatorGamepad->GetRightTrigger())
{
// TODO: Make this selectable somehow. This is probably too powerful for
// distant shots.
myShooter->ChamberFrisbee(Config::GetSetting("loader_power",-1));
}
else
{
myShooter->StopLoader();
}
if(operatorGamepad->GetY())
{
myShooter->SetAngle(manualAngle);
}
if(operatorGamepad->GetX())
{
myShooter->SetAngle(Config::GetSetting("shooter_loading_angle",0)); //Angle for reloading
manualAngle = Config::GetSetting("shooter_loading_angle",0);
}
if(operatorGamepad->GetA()) //Shooting from load zone
{
myShooter->SetAngle(Config::GetSetting("load_zone_shooting_angle",13.25));
manualAngle = Config::GetSetting("load_zone_shooting_angle",13.25);
}
if(operatorGamepad->GetB())
{
myShooter->SetAngle(Config::GetSetting("pyramid_shooting_angle",23.25));
manualAngle = Config::GetSetting("pyramid_shooting_angle",23.25);
}
if(operatorGamepad->GetRightBumper())
{
//myShooter->SetAngle(Config::GetSetting("funnel_release_angle",35));
myShooter->SetAngle(Config::GetSetting("pyramid_basket_angle", 45));
manualAngle = Config::GetSetting("pyramid_basket_angle", 45);
}
if (operatorGamepad->GetLeftPush()) {
myShooter->ResetShooterProcess();
}
// Do something similar, but don't touch the AngleJag.
if (operatorGamepad->GetRightPush()) {
myShooter->ResetShooterMotors();
}
//pneumatics section
if(!driverGamepad->GetLeftTrigger())
{
pneumatics->extendEndgame();
}
else
{
pneumatics->retractEndgame();
myShooter->SetAngle(3);
}
if(driverGamepad->GetRightTrigger())
{
pneumatics->extendCollector();
}
else
{
pneumatics->retractCollector();
}
//end pneumatics section
lastdir = dir;
myShooter->Process();
//cout << "Shooter rear wheel" << myShooter->getRearWheelSpeed() << endl;
//End OPERATOR input section
/*Manual control of the angle motor position using PID
* THIS ASSUMES THE SHOOTER HAS BEEN ZERO'ED TO THE HOME POSITION SOMEHOW
if(operatorGamepad->GetLeftTrigger())
{
myShooter->ManualAngleControl(16); // (theta)
}
else
{
myShooter->ManualAngleControl(0);
}
*/
/*if (target != NULL) {
cout << "Target located. (" << target->X() << ',' << target->Y() << ") range is " << target->Distance() << " feet" << endl;
}
*/
// switch(myShooter->getFiringState())
// {
// case 1:
// lc->setMode(4);
//// cout << "Spinning up!!" << endl;
// break;
// case 2:
// lc->setMode(5);
//// cout << "Firing!!" << endl;
// break;
// default:
// lc->setMode(1);
// break;
// }
lc->useLights();
BuildAndSendPacket();
logdata();
}
/**
* Periodic code for autonomous mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in autonomous mode.
*/
void RA14Robot::AutonomousPeriodic() {
StartOfCycleMaintenance();
target->Parse(server->GetLatestPacket());
float speed = Config::GetSetting("auto_speed", .5);
cout<<"Auto Speed: "<<Config::GetSetting("auto_speed", 0)<<endl; // original .1
float angle = gyro->GetAngle();
float error = targetHeading - angle;
float corrected = error * Config::GetSetting("auto_heading_p", .01);
//float corrected = error * Config::GetSetting("auto_heading_p", .01);
cout <<"Gyro angle: "<<angle<<endl;
cout <<"Error: " << error << endl;
//float lDrive = Config::GetSetting("auto_speed", -0.3) + (error * Config::GetSetting("auto_heading_p", .01));
//float rDrive = Config::GetSetting("auto_speed", -0.3) - (error * Config::GetSetting("auto_heading_p", .01));
// Reading p value from the config file does not appear to be working. When we get p from config, the math is not correct.
float lDrive = Config::GetSetting("auto_speed", -0.3) + (error*0.01);
float rDrive = Config::GetSetting("auto_speed", -0.3) - (error*0.01);
cout << "Left: " << lDrive << endl;
cout << "Right: " << rDrive << endl;
#ifndef DISABLE_AUTONOMOUS
switch(auto_case)
{
case 0:
// start master autonomous mode
switch (auto_state) {
case 0: // start
auto_timer->Reset();
auto_timer->Start();
myCam->Process(false, false, false);
break;
case 1:
myCam->Process(false, false, false);
if (target->IsValid()) {
auto_state = 2;
} else if (auto_timer->Get() >= Config::GetSetting("auto_target_timeout", 1)) {
auto_state = 10;
}
break;
case 2:
myCam->Process(false, false, false);
if (target->IsHot()) {
auto_state = 10;
} else {
if (auto_timer->Get() >= Config::GetSetting("auto_target_hot_timeout", 5)) {
auto_state = 10;
}
}
break;
case 10:
myCam->Process(true, false, false);
if (myCam->IsReadyToRearm()) {
auto_state = 11;
}
break;
case 11:
myCam->Process(false, false, false);
myDrive->DriveArcade(corrected, speed);
if (myDrive->GetOdometer() >= Config::GetSetting("auto_drive_distance", 100))
{
myDrive->Drive(0,0);
}
break;
case 12:
myDrive->Drive(0,0);
break;
default:
cout << "Unknown state #" << auto_state << endl;
break;
}
// end master autonomous mode
break;
case 1:
if( target->IsHot() && target->IsValid() )
{
cout << "Target is HOTTT taking the shot" << endl;
//Drive forward and shoot right away
//if( target->IsLeft() || target->IsRight() )
//{
if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
{
myDrive->Drive(corrected,speed);
}
else
{
myDrive->Drive(0,0);
cout << "FIRING" << endl;
#ifndef DISABLE_SHOOTER
myCam->Process(1,0,0);
#endif //Ends DISABLE_SHOOTER
}
/*}
else
{
cout<<"Error"<<endl;
}*/
}
else if(target->IsValid())
{
cout << "Target is valid, but cold. Driving and waiting" << endl;
//Drive forward and wait to shoot
if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
{
myDrive->Drive(corrected, speed);
}
else
{
// now at the firing spot.
myDrive->Drive(0,0);
if( target->IsHot() )
{
cout << "FIRING" << endl;
#ifndef DISABLE_SHOOTER
myCam->Process(1,0,0);
#endif //Ends DISABLE_SHOOTER
}
}
}
else
{
//Not valid
cout << "Not valid target." << endl;
}
break;
case 2:
#ifndef DISABLE_SHOOTER
myCam->Process(false,false,false);
if(auto_timer->Get() >= 4.0)
{
myCam->Process(true,false,false);
}
#endif //Ends DISABLE_SHOOTER
/*if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
{
cout<<"Distance traveled: "<<myDrive->GetOdometer()<<" inches"<<endl;
myDrive->Drive(corrected, speed);
}
else
{
myDrive->Drive(0,0);
cout << "FIRING" << endl;
}*/
if(auto_timer->Get() < 5.0)
{
cout<<"Waiting....."<<endl;
}
else
{
cout<<"Distance traveled: "<<myDrive->GetOdometer()<<" inches"<<endl;
myDrive->Drive(-.5, -.5);
}
break;
case 3:
#ifndef DISABLE_SHOOTER
switch(auto_state) {
case 0:
// Home/rearm
// fire, rearm, eject
myCam->Process(false, false, false);
if (myCam->IsReadyToFire()) {
auto_state = 1;
}
break;
case 1:
// fire
myCam->Process(true, false, false);
if (myCam->IsReadyToRearm()) {
auto_state = 2;
}
break;
case 2:
// rearm
myCam->Process(false, true, false);
if (myCam->IsReadyToFire()) {
auto_state = 3;
auto_timer->Reset();
auto_timer->Start();
}
break;
case 3:
myCam->Process(false, false, false);
myCollection->SpinMotor(Config::GetSetting("intake_roller_speed", .7));
if (auto_timer->HasPeriodPassed( Config::GetSetting("auto_collection_delay", 1.0) )) {
auto_state = 4;
}
break;
case 4:
// fire again!
myCam->Process(true, false, false);
auto_state = 5;
break;
case 5:
myCam->Process(false, false, false);
if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
{
myDrive->Drive(corrected, speed);
} else {
// now at the firing spot.
auto_state = 6;
myDrive->Drive(0,0);
}
break;
case 6:
myCollection->RetractArm();
break;
default:
cout << "Error, unrecognized state " << auto_state << endl;
}
#endif //Ends DISABLE_SHOOTER
break;
case 4: /* One ball Autonomous - Drive forward specific distance, stop then shoot.*/
#ifndef DISABLE_SHOOTER
switch(auto_state) {
case 0: //Reset odometer, lower the arm and set launcher to ready to fire
myDrive->ShiftUp();
myDrive->ResetOdometer();
myCollection->ExtendArm();
myCam->Process(false,true,false);
auto_state = 1;
break;
case 1: // Start driving forward
// myDrive->Drive(-.5, -.5);
//myDrive->Drive(lDrive, rDrive);
myDrive->DriveArcade(corrected, speed);
auto_state = 2;
break;
case 2: // Continue driving until required distance
if(myDrive->GetOdometer() >= Config::GetSetting("auto_drive_distance", 96)) {
myDrive->Drive(0, 0);
auto_state = 3;
}
break;
case 3: // Fire launcher
myCam->Process(true,false,false);
auto_state = 4;
break;
case 4: // Idle state
break;
}
#endif //Ends DISABLE_SHOOTER
break;
case 5: // Two Ball Autonomous - Drag ball 2 while driving forward specific distance, stop then shoot, load shoot next ball
#ifndef DISABLE_SHOOTER
// By Hugh Meyer - April 1, 2014
switch(auto_state) {
cout << "Executing mr m's auton" << endl;
case 0: // Set low gear, reset odometer, extend pickup arm, set launcher ready to fire, set wait timer
//myDrive->ShiftUp(); // Shift to low gear
myDrive->ShiftDown();
myDrive->ResetOdometer(); // Reset odometer to zero
myCollection->ExtendArm(); // Extend arm to pickup position
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while pickup extends
auto_state = 1; // Go on to next state
break;
case 1: // Wait for timer to expire - Let arm get extended and stabalized
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_extend_delay", 1.0) )) {
auto_state = 2;
}
break;
case 21: //reset gyro and reset timer
gyro->Reset();
auto_timer->Reset();
auto_timer->Start();
auto_state = 22;
break;
case 22:
if(auto_timer->HasPeriodPassed(Config::GetSetting("auton5_gyro_reset_delay", 2) )){
auto_state = 2;
}
break;
case 2: // Activate pickup roller motor to drag speed, start driving forward
myCollection->SpinMotor(Config::GetSetting("auton5_drag_speed", 0.3)); // Start motor to drag ball 2
//myDrive->DriveArcade(corrected, speed); // Drive straight
myDrive->DriveArcade(0.0, Config::GetSetting("auton5_drive_speed", -0.7)); //Start driving forwards
auto_state = 3;
break;
case 3: // Continue driving until required distance, stop driving, stop pickup roller motor
if(myDrive->GetOdometer() >= Config::GetSetting("auton5_drive_distance", 96.0))
{
myCollection->SpinMotor(0); // Stop collector pickup motor
myDrive->Drive(0, 0); // Stop driving
auto_state = 31; // On to next state
}
break;
case 31: // slightly un-eject herded ball to avoid contact with launch ball
myCollection->SpinMotor(Config::GetSetting("auton5_eject_speed", 0.3));
auto_timer->Reset();
auto_timer->Start(); // setup timer to time un-eject
auto_state = 32;
break;
case 32: // once timer has run out, stop ejection and fire
if (auto_timer->HasPeriodPassed(Config::GetSetting("auton5_uneject_time", 0.25))) {
myCollection->SpinMotor(0); // stop spinner
auto_state = 4;
}
break;
case 4: // Fire launcher to shoot first ball, set wait timer
myCam->Process(true,false,false); // Fire ball # 1
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 5; // On to next state
break;
case 5: // Wait for timer to expire after ball one launches
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_ball_1_launch_delay", 1.0) )) {
auto_state = 6; // On to next state
}
break;
case 6: // set launcher ready to fire for ball 2, set wait timer
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 7; // On to next state
break;
case 7: // Wait for timer to expire
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_ball_2_ready2fire_delay", 1.0) )) {
auto_state = 8; // Wait for launcher to get ready to accept ball 2
}
break;
case 8: // Activate pickup roller motor to load ball 2, set wait timer
myCollection->SpinMotor(Config::GetSetting("auton5_intake_roller_speed", 0.7));
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball 2 loads
auto_state = 9; // On to next state
break;
case 9: // Wait for timer to expire while ball 2 gets collected into launcher
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton5_ball_2_settle_delay", 1.0) )) {
auto_state = 10; // Wait for ball 2 to be collected and settle
}
break;
case 19: // Drive backwards a little bit, set wait timer
myDrive->DriveArcade(-1*corrected, -1*speed); // Drive straight
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 20; // Wait for ball 2 to be collected and settle
break;
case 20: // Wait for timer to expire while ball 2 gets collected into launcher
if (myDrive->GetOdometer() <=
Config::GetSetting("auton5_drive_distance", 96)
- Config::GetSetting("auton5_backup_distance", 6)) {
myDrive->Drive(0,0);
auto_state = 10;
}
break;
case 10: // Fire launcher to shoot second ball and stop roller
myCam->Process(true,false,false); // Fire ball # 2
myCollection->SpinMotor(0); // Stop spinning the roller
auto_state = 11; // All done so go to idle state
break;
case 11: // Idle state
auto_state = 11;
break;
/* case 12: // More states if we need them for changes.
auto_state = 13;
break;
case 13: //
auto_state = 14;
break;
case 14: //
auto_state = 15;
break;
case 15: //
auto_state = 15;
break;
*/
}
#endif //Ends DISABLE_SHOOTER
break;
case 6: // Two Ball Autonomous - Drive forward specific distance, stop then shoot, backup get second ball, drive, shoot
#ifndef DISABLE_SHOOTER
// By Hugh Meyer - April 1, 2014
switch(auto_state) {
case 0: // Set low gear, reset odometer, extend pickup arm, set launcher ready to fire
myDrive->ShiftDown(); // Shift to low gear
myDrive->ResetOdometer(); // Reset odometer to zero
myCollection->ExtendArm(); // Extend arm to pickup position
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_state = 1; // Go on to next state
break;
case 1: // Drive forward
myDrive->DriveArcade(corrected, speed);
auto_state = 2; // On to next state
break;
case 2: // Continue driving forward until the specific distance is traveled
if(myDrive->GetOdometer() >= Config::GetSetting("auton6_drive_forward_distance", 96.0)) {
myDrive->Drive(0, 0);
auto_state = 3;
}
break;
case 3: // Fire launcher to launch first ball, set timer
myCam->Process(true,false,false); // Launch ball # 1
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 4; // On to next state
break;
case 4: // Wait for timer while ball launcher fires
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton6_ball_1_fire_delay", 1.0) )) {
auto_state = 5; // On to next state
}
break;
case 5: // Reset Odometer, Drive backwards, set launcher to ready to fire position, turn on pickup
cout<<"Corrected Values: "<<corrected<<endl;
cout<<"Speed: "<< -1 * speed<<endl;
myDrive->ResetOdometer(); // Reset odometer to zero
myDrive->DriveArcade(-1* corrected, -1 * speed); // Drive backwards
myCam->Process(false,true,false); // Set launcher to ready to fire position
myCollection->SpinMotor(-1 * Config::GetSetting("auton6_intake_roller_speed", 0.7)); // Turn on pickup
auto_state = 6; // On to next state
break;
case 6: // Continue driving backwards a specific distance
if(myDrive->GetOdometer() <= -1 * Config::GetSetting("auton6_drive_forward_distance", -96.0)) {
auto_state = 7; // On to next state
}
break;
case 7: // Set timer
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 8; // On to next state
break;
case 8: // Wait for timer while ball loads and settles
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton6_ball_2_load_delay", 1.0) )) {
auto_state = 9; // On to next state
myDrive->ResetOdometer();
}
break;
case 9: // Drive forwards
myDrive->DriveArcade(/*corrected*/ 0.01, speed);
auto_state = 10; // On to next state
break;
case 10: // Continue driving forward until the specific distance is traveled
cout << "I am driving forward: " << myDrive->GetOdometer() << endl;
myDrive->DriveArcade(0.01, speed);
if(myDrive->GetOdometer() >= Config::GetSetting("auton6_drive_forward_distance", 96.0)) {
myDrive->Drive(0, 0);
auto_state = 11; // On to next state
}
break;
case 11: // Fire launcher to launch second ball
myCam->Process(true,false,false); // Launch ball # 2
auto_state = 15; // On to next state
break;
/* case 12: //
auto_state = 13; // On to next state
break;
case 13: //
auto_state = 14; // On to next state
break;
case 14: //
auto_state = 15; // All done so go to idle state
break;
*/
case 15: // Idle state
auto_state = 15;
break;
}
cout << myDrive->GetOdometer() << endl;
#endif //Ends DISABLE_SHOOTER
break;
case 7: // Two Ball Autonomous - Drag ball 2 while driving forward specific distance, stop then shoot, load shoot next ball
#ifndef DISABLE_SHOOTER
// By Hugh Meyer - April 1, 2014
switch(auto_state) {
cout << "Executing mr m's auton" << endl;
case 0: // Set low gear, reset odometer, extend pickup arm, set launcher ready to fire, set wait timer
myDrive->ShiftUp(); // Shift to high gear
//myDrive->ShiftDown();
myDrive->ResetOdometer(); // Reset odometer to zero
myCollection->ExtendArm(); // Extend arm to pickup position
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while pickup extends
auto_state = 1; // Go on to next state
break;
case 1: // Wait for timer to expire - Let arm get extended and stabalized
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_extend_delay", 1.0) )) {
auto_state = 2;
}
break;
case 21: //reset gyro and reset timer
gyro->Reset();
auto_timer->Reset();
auto_timer->Start();
auto_state = 22;
break;
case 22:
if(auto_timer->HasPeriodPassed(Config::GetSetting("auton7_gyro_reset_delay", 2) )){
auto_state = 2;
}
break;
case 2: // Activate pickup roller motor to drag speed, start driving forward
myCollection->SpinMotor(Config::GetSetting("auton7_drag_speed", 0.3)); // Start motor to drag ball 2
//myDrive->DriveArcade(corrected, speed); // Drive straight
myDrive->DriveArcade(0.05, speed);
auto_state = 3;
break;
case 3: // Continue driving until required distance, stop driving, stop pickup roller motor
if(myDrive->GetOdometer() >= Config::GetSetting("auton7_drive_distance", 96.0))
{
myCollection->SpinMotor(0); // Stop collector pickup motor
myDrive->Drive(0, 0); // Stop driving
auto_state = 31; // On to next state
}
break;
case 31: // slightly un-eject herded ball to avoid contact with launch ball
myCollection->SpinMotor(Config::GetSetting("auton7_eject_speed", 0.3));
auto_timer->Reset();
auto_timer->Start(); // setup timer to time un-eject
auto_state = 32;
break;
case 32: // once timer has run out, stop ejection and fire
if (auto_timer->HasPeriodPassed(Config::GetSetting("auton7_uneject_time", 0.25))) {
myCollection->SpinMotor(0); // stop spinner
auto_state = 4;
}
break;
case 4: // Fire launcher to shoot first ball, set wait timer
myCam->Process(true,false,false); // Fire ball # 1
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 5; // On to next state
break;
case 5: // Wait for timer to expire after ball one launches
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_ball_1_launch_delay", 1.0) )) {
auto_state = 6; // On to next state
}
break;
case 6: // set launcher ready to fire for ball 2, set wait timer
myCam->Process(false,true,false); // Set launcher to ready to fire position
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 7; // On to next state
break;
case 7: // Wait for timer to expire
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_ball_2_ready2fire_delay", 1.0) )) {
auto_state = 8; // Wait for launcher to get ready to accept ball 2
}
break;
case 8: // Activate pickup roller motor to load ball 2, set wait timer
myCollection->SpinMotor(Config::GetSetting("auton7_intake_roller_speed", 0.7));
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball 2 loads
auto_state = 9; // On to next state
break;
case 9: // Wait for timer to expire while ball 2 gets collected into launcher
if (auto_timer->HasPeriodPassed( Config::GetSetting("auton7_ball_2_settle_delay", 1.0) )) {
auto_state = 10; // Wait for ball 2 to be collected and settle
}
break;
case 19: // Drive backwards a little bit, set wait timer
myDrive->DriveArcade(-1*corrected, -1*speed); // Drive straight
auto_timer->Reset(); // Set timer to zero
auto_timer->Start(); // Start the timer for a short delay while ball launches
auto_state = 20; // Wait for ball 2 to be collected and settle
break;
case 20: // Wait for timer to expire while ball 2 gets collected into launcher
if (myDrive->GetOdometer() <=
Config::GetSetting("auton7_drive_distance", 96)
- Config::GetSetting("auton7_backup_distance", 6)) {
myDrive->Drive(0,0);
auto_state = 10;
}
break;
case 10: // Fire launcher to shoot second ball and stop roller
myCam->Process(true,false,false); // Fire ball # 2
myCollection->SpinMotor(0); // Stop spinning the roller
auto_state = 11; // All done so go to idle state
break;
case 11: // Idle state
auto_state = 11;
break;
/* case 12: // More states if we need them for changes.
auto_state = 13;
break;
case 13: //
auto_state = 14;
break;
case 14: //
auto_state = 15;
break;
case 15: //
auto_state = 15;
break;
*/
}
#endif //Ends DISABLE_SHOOTER
break;
/*
case 7: // Extra structure for more changes
#ifndef DISABLE_SHOOTER
// By Hugh Meyer - April 1, 2014
switch(auto_state) {
case 0: // Set low gear, reset odometer, extend pickup arm, set launcher ready to fire, drive foward
myDrive->ShiftDown(); // Shift to low gear
myDrive->ResetOdometer(); // Reset odometer to zero
myCollection->ExtendArm(); // Extend arm to pickup position
myCam->Process(false,true,false); // Set launcher to ready to fire position
myDrive->Drive(lDrive, rDrive); // Drive straight
auto_state = 1; // Go on to next state
break;
case 1: //
auto_state = 2; // On to next state
break;
case 2: //
auto_state = 3; // On to next state
break;
case 3: //
auto_state = 4; // On to next state
break;
case 4: //
auto_state = 5; // On to next state
break;
case 5: //
auto_state = 6; // On to next state
break;
case 6: //
auto_state = 7; // On to next state
break;
case 7: //
auto_state = 8; // On to next state
break;
case 8: //
auto_state = 9; // On to next state
break;
case 9: //
auto_state = 10; // On to next state
break;
case 10: //
auto_state = 11; // On to next state
break;
case 11: //
auto_state = 12; // On to next state
break;
case 12: //
auto_state = 13; // On to next state
break;
case 13: //
auto_state = 14; // On to next state
break;
case 14: //
auto_state = 15; // All done so go to idle state
break;
case 15: // Idle state
auto_state = 15;
break;
}
#endif //Ends DISABLE_SHOOTER
break;
*/
default:
cout<<"Error in autonomous, unrecognized case: "<<auto_case<<endl;
}
//#else
// if (myDrive->GetOdometer() <= (4 * acos(-1) ) ) //216 is distance from robot to goal
// {
// float speed = Config::GetSetting("auto_speed", .3);
// cout << myDrive->GetOdometer() << endl;
// myDrive->Drive(speed, speed);
// } else {
// cout << "Finished driving";
// myDrive->Drive(0, 0);
// }
#endif //Ends DISABLE_AUTONOMOUS
EndOfCycleMaintenance();
}
/**
* 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 RA14Robot::TeleopPeriodic() {
StartOfCycleMaintenance();
//Input Acquisition
DriverLeftY = DriverGamepad->GetLeftY();
DriverRightY = DriverGamepad->GetRightY();
DriverLeftBumper = DriverGamepad->GetLeftBumper(); // Reads state of left bumper
DriverRightBumper = DriverGamepad->GetRightBumper(); // Gets state of right bumper
RingLightButton = OperatorGamepad->GetY();
ShouldFireButton = DriverGamepad->GetRightTrigger();
BallCollectPickupButton = DriverGamepad->GetBack();
DriverDPad = DriverGamepad->GetDPad();
OperatorDPad = OperatorGamepad->GetDPad();
//End Input Acquisition
//Current Sensing
//myCurrentSensor->Toggle(DriverGamepad->GetStart());
//End Current Sensing
//Target Processing
target->Parse(server->GetLatestPacket());
/*
if (target->IsValid()) {
cout << "\tx = " << target->GetX() << ", y = " << target->GetY() << ", distance = " << target->GetDistance() << "ft";
cout << "\tside = " << (target->IsLeft() ? "LEFT" : "RIGHT") << ", hot = " << (target->IsHot() ? "HOT" : "NOT") << endl;
}
else {
cout << "No Target Received..." << endl;
}
*/
//End Target Processing
//Ball Collection
/*
if(BallCollectPickupButton)
{
myCollection->Collect();
}
else
{
myCollection->ResetPosition();
}
*/
float spinSpeed = Config::GetSetting("intake_roller_speed", 1);
int armPosition = 0;
int rollerPosition = 0;
switch (OperatorDPad) {
case Gamepad::kCenter:
armPosition = 0;
rollerPosition = 0;
break;
case Gamepad::kUp:
armPosition = -1;
break;
case Gamepad::kUpLeft:
armPosition = -1;
rollerPosition = -1;
break;
case Gamepad::kUpRight:
armPosition = -1;
rollerPosition = 1;
break;
case Gamepad::kDown:
armPosition = 1;
break;
case Gamepad::kDownLeft:
armPosition = 1;
rollerPosition = -1;
break;
case Gamepad::kDownRight:
armPosition = 1;
rollerPosition = 1;
break;
case Gamepad::kLeft:
rollerPosition = -1;
break;
case Gamepad::kRight:
rollerPosition = 1;
break;
}
if (DriverGamepad->GetLeftTrigger()) {
armPosition = 0;
rollerPosition = -1;
}
if(DriverGamepad->GetB())
{
armPosition = -1;
rollerPosition = 1;
}
spinSpeed *= rollerPosition;
myCollection->SpinMotor(spinSpeed);
if (armPosition > 0)
myCollection->ExtendArm();
else if (armPosition < 0)
myCollection->RetractArm();
//End Ball Collection
//Fire Control
#ifndef DISABLE_SHOOTER
myCam->Process(ShouldFireButton, (OperatorGamepad->GetX() || DriverGamepad->GetX() ), DriverGamepad->GetB());
//myCam->Debug(cout);
#endif //Ends DISABLE_SHOOTER
//End Fire Control
//Ring Light
if (RingLightButton) {
RingLightButtonRecentlyPressed = true;
}
if (!RingLightButton && RingLightButtonRecentlyPressed) {
if (myCamera->Get() == Relay::kOff)
myCamera->Set(Relay::kForward);
else
myCamera->Set(Relay::kOff);
RingLightButtonRecentlyPressed = false;
}
//End Ring Light
//Drive Processing
if(DriverDPad == Gamepad::kUp)
{
cout << "Forward" << endl;
myDrive->reverseDirectionForward();
}
else if(DriverDPad == Gamepad::kDown)
{
cout << "Backward"<<endl;
myDrive->reverseDirectionReverse();
}
if (DriverLeftBumper) {
myDrive->ShiftUp();
} else if (DriverRightBumper) {
myDrive->ShiftDown();
}
//myDrive->Drive(DriverLeftY, DriverRightY);
myDrive->DriveArcade(DriverGamepad->GetRightX(), DriverGamepad->GetRightY());
myDrive->Debug(cout);
//End Drive Processing
EndOfCycleMaintenance();
}
/*
* resets encoders, step, navX, and starts the spike relay
*/
void TeleopInit(){
Drive->TeleopInit();
autonomous->reset();
}