//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; }
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); } } }
/** * Return the actual angle in degrees that the robot is currently facing. * * The angle is based on the current accumulator value corrected by the oversampling rate, the * gyro type and the A/D calibration values. * The angle is continuous, that is can go beyond 360 degrees. This make algorithms that wouldn't * want to see a discontinuity in the gyro output as it sweeps past 0 on the second time around. * * @param slot The slot the analog module is connected to * @param channel The analog channel the gyro is plugged into * @return the current heading of the robot in degrees. This heading is based on integration * of the returned rate from the gyro. */ float GetGyroAngle(UINT32 slot, UINT32 channel) { Gyro *gyro = AllocateGyro(slot, channel); if (gyro) return gyro->GetAngle(); return 0.0; }
bool GyroDrive(float desiredDriveAngle, float speed, int desiredDistance) { bool driving = true; double encoderInchesTraveled = fabs(leftEnco->GetDistance());//absolute value distance 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; } float my_speed = 0.0; float turn = 0.0; if(speed > 0) //30.0 is the number you have to change to adjust properly turn = -((myAngle + desiredDriveAngle) / 30.0); //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be else turn = (myAngle + desiredDriveAngle) / 30.0; //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be if (encoderInchesTraveled < desiredDistance) my_speed = speed; else { my_speed = 0.0; driving = false; } myRobot->Drive(my_speed, turn); return driving; }
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; } }
/** * 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); }
void Autonomous() { Timer timer; float power = 0; bool isLifting = false; bool isGrabbing = false; double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip; double grabPower = Constants::grabAutoCurrent; bool backOut; uint8_t toSend[1];//array of bytes to send over I2C uint8_t toReceive[0];//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] = 2;//set the byte to send to 1 i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C bool isSettingUp = true; //pickup.setGrabber(-1); //open grabber all the way pickup.setLifter(0.8); while (isSettingUp && IsEnabled() && IsAutonomous()) { isSettingUp = false; /*if (grabOuterLimit.Get() == false) { pickup.setGrabber(0); //open until limit } else { isSettingUp = true; }*/ if (liftLowerLimit.Get()) { pickup.setLifter(0); //down till bottom } else { isSettingUp = true; } } gyro.Reset(); liftEncoder.Reset(); grabEncoder.Reset(); if (grabStick.GetZ() > .8) { timer.Reset(); timer.Start(); while (timer.Get() < 1) { robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back if(power>-.4){ power-=0.005; Wait(.005); } } robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle()); // STOP!!! timer.Stop(); timer.Reset(); Wait(1); } power = 0; while (isLifting && IsEnabled() && IsAutonomous()) { Wait(.005); } backOut = Constants::autoBackOut; pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick); Wait(.005); while (isGrabbing && IsEnabled() && IsAutonomous()) { Wait(.005); } liftHeight = 3*Constants::liftBoxHeight; Wait(.005); pickup.lifterPosition(liftHeight, isLifting, grabStick); Wait(.005); while (isLifting && IsEnabled() && IsAutonomous()) { Wait(.005); } while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous()); // while the nearest object is closer than 2 feet timer.Start(); while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet if (power < .45) { //ramp up the power slowly power += .00375; } robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash 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("Gyro Angle", gyro.GetAngle()); SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches); Wait(.005); } timer.Reset(); while(timer.Get() < Constants::autoBrakeTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet robotDrive.MecanumDrive_Cartesian(0,Constants::autoBrakePower,0); ///Brake } float turn = 0; while (fabs(turn) < 85 && IsEnabled() && IsAutonomous()) { //turn 90(ish) degrees robotDrive.MecanumDrive_Cartesian(0,0,.1); turn = gyro.GetAngle(); if (turn > 180) { turn -= 360; } } robotDrive.MecanumDrive_Cartesian(0,0,0); ///STOP!!! timer.Stop(); toSend[0] = 8; i2c.Transaction(toSend, numToSend, toReceive, numToReceive); while(IsAutonomous() && IsEnabled()); 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()); }
void TeleopPeriodic() { //camera->GetImage(frame); //imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f); //CameraServer::GetInstance()->SetImage(frame); printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle()); drive->ArcadeDrive(driveStick); drive->SetMaxOutput((1-driveStick->GetThrottle())/2); //printf("%f\n", (1-stick->GetThrottle())/2); //leftMotor->Set(0.1); //rightMotor->Set(0.1); if (shootStick->GetRawAxis(3) > 0.5) { launch1->Set(1.0); launch2->Set(1.0); } else if (shootStick->GetRawAxis(2) > 0.5) { printf("Power Counter: %i\n", powerCounter); if (powerCounter < POWER_MAX) { powerCounter++; launch1->Set(-0.8); launch2->Set(-0.8); } else { launch1->Set(-0.6); launch2->Set(-0.6); } } else { launch1->Set(0.0); launch2->Set(0.0); powerCounter = 0.0; } //use this button to spin only one winch, to lift up. if (shootStick->GetRawButton(7)) { otherWinch->Set(0.5); } else if (shootStick->GetRawButton(8)) { otherWinch->Set(-0.5); } else { otherWinch->Set(0.0); } if (shootStick->GetRawButton(5)) { winch->Set(-0.7); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { // otherWinch->Set(-0.5); } } else if (shootStick->GetRawButton(6)) { winch->Set(0.7); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { // otherWinch->Set(0.5); } } else { winch->Set(0.0); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { //, otherWinch->Set(0.0); } } if (shootStick->GetRawButton(1)) { launchPiston->Set(1); } else { launchPiston->Set(0); } if (shootStick->GetRawButton(3)) { Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer); } if (shootStick->GetRawButton(3) && debounce == false) { debounce = true; if (defenseUp) { defensePiston->Set(DoubleSolenoid::Value::kReverse); defenseUp = false; } else { defenseUp =true; defensePiston->Set(DoubleSolenoid::Value::kForward); } } else if (!shootStick->GetRawButton(3)){ debounce = false; } }
void AutonomousPeriodic() { drive->SetMaxOutput(1.0); printf("Distance: %f\n", rightEnc->GetDistance()); // if (!launcherDown) { // if (timer->Get() > 1.0) { // winch->Set(0.5); // otherWinch->Set(0.5); // } else if (timer->Get() < 3.0) { // winch->Set(0.5); // otherWinch->Set(0.0); // } else { // winch->Set(0.0); // otherWinch->Set(0.0); // launcherDown = true; // } // } if(done) { winch->Set(0.0); otherWinch->Set(0.0); drive->ArcadeDrive(0.0,0.0); if (autoCounter > 10) { launchPiston->Set(0); launch1->Set(0.0); launch2->Set(0.0); } else { autoCounter++; if (shoot == "Yes") { launch1->Set(1.0); launch2->Set(1.0); } } } else { if (autoSelected == "Approach Only") { done = Autonomous::approachOnly(); launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); } else if (!defenseCrossed) { if(Autonomous::crossFunctions.find(autoSelected) != Autonomous::crossFunctions.end()) { bool (*crossFunction)() = Autonomous::crossFunctions.at(autoSelected); defenseCrossed = crossFunction(); launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); } else { done = true; launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); drive->ArcadeDrive(0.0,0.0); } timer->Reset(); } else { if (autoSelected == "Spy Bot") { rotation = 90; } //after we cross... float difference = gyro->GetAngle() - rotation; if (difference > 10) { launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); drive->ArcadeDrive(0.0,difference * 0.3); timer->Reset(); } else { if (goal == "Yes") { if (!Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer)) { launchPiston->Set(0); } else { launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); drive->ArcadeDrive(0.0,0.0); launchPiston->Set(1); done = true; } } else { launch1->Set(0.0); launch2->Set(0.0); winch->Set(0.0); otherWinch->Set(0.0); done = true; } } } } }
/**************************************** * Runs the motors with arcade steering.* ****************************************/ void OperatorControl(void) { //TODO put in servo for lower camera--look in WPI to set // Watchdog baddog; // baddog.Feed(); myRobot.SetSafetyEnabled(true); //SL Earth.Start(); // turns on Earth // SmartDashboard *smarty = SmartDashboard::GetInstance(); //DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory) //char debugout [100]; compressor.Start(); gyro.Reset(); // resets gyro angle int rpmForShooter; while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop" { // baddog.Feed(); //myRobot.SetSafetyEnabled(true); //myRobot.SetExpiration(0.1); float leftYaxis = driver.GetY(); float rightYaxis = driver.GetTwist();//RawAxis(5); myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1 float random = gamecomponent.GetY(); float lazysusan = gamecomponent.GetZ(); //bool elevator = Frodo.Get(); float angle = gyro.GetAngle(); bool balance = Smeagol.Get(); SmartDashboard::PutNumber("Gyro Value",angle); int NumFail = -1; //bool light = Pippin.Get(); //SL float speed = Earth.GetRate(); //float number = shooter.Get(); //bool highspeed = button1.Get() //bool mediumspeed = button2.Get(); //bool slowspeed = button3.Get(); bool finder = autotarget.Get(); //bool targetandspin = autodistanceandspin.Get(); SmartDashboard::PutString("Targeting Activation",""); //dslcd->Clear(); //sprintf(debugout,"Number=%f",angle); //dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout); //SL sprintf(debugout,"Number=%f",speed); //SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout); //sprintf(debugout,"Number=%f",number); //dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout); //sprintf(debugout,"Finder=%u",finder); //dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout); //dslcd->UpdateLCD(); // update the Driver Station with the information in the code // sprintf(debugout,"Number=%u",maxi); // dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout) bool basketballpusher = julesverne.Get(); bool bridgetipper = joystickbutton.Get(); if (bridgetipper) // if joystick button 7 is pressed (is true) { solenoid.Set(true); // then the first solenoid is on } else { //Wait(0.5); // and then the first solenoid waits for 0.5 seconds solenoid.Set(false); //and then the first solenoid turns off } if (basketballpusher) // if joystick button 6 is pressed (is true) { shepard.Set(true); // then shepard is on the run //Wait(0.5); // and shepard waits for 0.5 seconds } else { shepard.Set(false); // and then shepard turns off } //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2 //} //cheetah.Set(0.3*lazysusan); // smarty->PutDouble("pre-elevator",lynx.Get()); lynx.Set(random); // smarty->PutDouble("elevator",lynx.Get()); // smarty->PutDouble("joystick elevator",random); if (balance) // this is the start of the balancing code { angle = gyro.GetAngle(); myRobot.Drive(-0.03*angle, 0.0); Wait(0.005); } /*if (light) //button 5 turns light on oand off on game controller flashring.Set(Relay::kForward); else flashring.Set(Relay::kOff); */ if (finder) { flashring.Set(Relay::kForward); if (button_H.Get()==true) { targeting.SetLMHTarget(BOGEY_H); SmartDashboard::PutString("Targeting","High Button Pressed"); } if (button_M.Get()==true) { targeting.SetLMHTarget(BOGEY_M); SmartDashboard::PutString("Targeting","Medium Button Pressed"); } if (button_L.Get()==true) { targeting.SetLMHTarget(BOGEY_L); SmartDashboard::PutString("Targeting","Low Button Pressed"); } if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true) { if (targeting.ProcessOneImage()) { NumFail = 0; SmartDashboard::PutString("Targeting Activation","YES"); targeting.ChooseBogey(); targeting.MoveTurret(); #ifdef USE_HARDWIRED_RPM shooter.setTargetRPM(HARDWIRED_RPM); #else rpmForShooter = targeting.GetCalculatedRPM(); shooter.setTargetRPM(rpmForShooter); #endif targeting.InteractivePIDSetup(); } else { NumFail++; if (NumFail > 10) targeting.StopPID(); } SmartDashboard::PutNumber("Numfail", NumFail); shooter.setTargetRPM(rpmForShooter); } else { SmartDashboard::PutString("Targeting Activation","NO"); shooter.setTargetRPM(0); targeting.StopPID(); } } else { flashring.Set(Relay::kOff); targeting.StopPID(); turret.Set(lazysusan); // the lazy susan would turn right & left based on how far the person moves the right joystick#2 side to side //targeting.StopPID(); //if (elevator) //shooter would move at full speed if button is pressed //TODO Change RPM values //TODO Disable calculation of RPM values SmartDashboard::PutNumber("CurrentRPM",shooter.GetCurrentRPM()); if (button_H.Get() == true) shooter.setTargetRPM((int)2100); //From front of free throw line, should hit the backboard and go in //used to be 2700 RPMs else if (button_M.Get() == true) shooter.setTargetRPM((int)1900); //From front of free throw line, should go in the net--can shoot the next ball on the overshoot? //Used to be 2250 RPMs else if (button_L.Get() == true) shooter.setTargetRPM((int)1350); //From fender, should hit the backboard //Used to be 2000 RPMs //shooter.Set(0.5); else shooter.setTargetRPM(0); // else if (mediumspeed) //shooter.setTargetRPM((int)0); //else if (slowspeed) //shooter.setTargetRPM((int)0); /*if (targetandspin) //code for autotargeting and speed will go here { shooter.setTargetRPM((int)1800); } else {*/ //} myRobot.TankDrive(leftYaxis,rightYaxis); } //Wait(0.005); } }
/** * 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(); }