/** * This autonomous (along with the chooser code above) shows how to select between different autonomous modes * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box * below the Gyro * * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings. * If using the SendableChooser make sure to add them to the chooser code above as well. */ void AutonomousInit() { autoSelected = *((std::string*)chooser->GetSelected()); //std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault); std::cout << "Auto selected: " << autoSelected << std::endl; rotation = 0.0; //*((double*)posChooser->GetSelected()); //goal = *((std::string*)goalChooser->GetSelected()); shoot = "No"; //*((std::string*)shootChooser->GetSelected()); defenseCrossed = false; done = false; std::cout << "Here" << std::endl; drive->SetMaxOutput(1.0); std::cout << "there" << std::endl; //Make sure to reset the encoder! leftEnc->Reset(); rightEnc->Reset(); gyro->Reset(); autoCounter = 0; timer->Reset(); }
RobotDemo() { //Initialize the objects for the drive train myRobot = new RobotDrive(1, 2); leftEnco = new Encoder(LEFT_ENCO_PORT_1, LEFT_ENCO_PORT_2); rightEnco = new Encoder(RIGHT_ENCO_PORT_1, RIGHT_ENCO_PORT_2); leftEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV); leftEnco->Start(); rightEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV); rightEnco->Start(); leftStick = new Joystick(1); rightStick = new Joystick(2); //Initialize the gyro gyro = new Gyro(GYRO_PORT); gyro->Reset(); //Initialize the manipulators intake = new Intake(INTAKE_ROLLER_PORT, BALL_SENSOR_PORT, LEFT_SERVO_PORT, RIGHT_SERVO_PORT); catapult = new Catapult(LOADING_MOTOR_PORT, HOLDING_MOTOR_PORT, LOADED_LIMIT_1_PORT, LOADED_LIMIT_2_PORT, HOLDING_LIMIT_PORT); //Initialize the objects needed for camera tracking rpi = new RaspberryPi("17140"); LEDLight = new Relay(1); LEDLight->Set(Relay::kForward); //Set the autonomous modes autonMode = ONE_BALL_AUTON; autonStep = AUTON_ONE_SHOOT; //Initialize the lcd lcd = DriverStationLCD::GetInstance(); }
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); } } }
/** * 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 TeleopInit() { leftEnc->Reset(); rightEnc->Reset(); gyro->Reset(); powerCounter = 0; }
virtual void SetUp() override { m_tilt = new Servo(TestBench::kCameraTiltChannel); m_pan = new Servo(TestBench::kCameraPanChannel); m_spiAccel = new ADXL345_SPI(SPI::kOnboardCS0); m_tilt->Set(kTiltSetpoint45); m_pan->SetAngle(0.0f); Wait(kServoResetTime); m_gyro->Reset(); }
void Test() { robotDrive.SetSafetyEnabled(false); 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] = 7; //send 0 to arduino i2c.Transaction(toSend, numToSend, toReceive, numToReceive); bool isSettingUp = true; pickup.setGrabber(-1); pickup.setLifter(1); while (isSettingUp) { isSettingUp = false; if (grabOuterLimit.Get() == false) { pickup.setGrabber(0); } else { isSettingUp = true; } if (liftLowerLimit.Get()) { pickup.setLifter(0); } else { isSettingUp = true; } } gyro.Reset(); liftEncoder.Reset(); grabEncoder.Reset(); toSend[0] = 8; i2c.Transaction(toSend, numToSend, toReceive, numToReceive); while(IsTest() && IsEnabled()); toSend[0] = 0; i2c.Transaction(toSend, numToSend, toReceive, numToReceive); }
void RobotInit(void) { //Pegs 1-6 dseio.SetDigitalConfig(1,DriverStationEnhancedIO::kInputPullDown); dseio.SetDigitalConfig(2,DriverStationEnhancedIO::kInputPullDown); dseio.SetDigitalConfig(3,DriverStationEnhancedIO::kInputPullDown); dseio.SetDigitalConfig(4,DriverStationEnhancedIO::kInputPullDown); dseio.SetDigitalConfig(5,DriverStationEnhancedIO::kInputPullDown); dseio.SetDigitalConfig(6,DriverStationEnhancedIO::kInputPullDown); //pickup,retrieve,stow,stop dseio.SetDigitalConfig(7,DriverStationEnhancedIO::kInputPullDown); dseio.SetDigitalConfig(8,DriverStationEnhancedIO::kInputPullDown); dseio.SetDigitalConfig(9,DriverStationEnhancedIO::kInputPullDown); dseio.SetDigitalConfig(10,DriverStationEnhancedIO::kInputPullDown); LP1=false; LP2 =false; LP3=false; LP4 =false; LP5=false; LP6=false; LPStow=false; LPRetrieve=false; LPPickUp=false; LPStopArm=true; myarm->MoveClaw(-1); //start compressor robot_compressor->Start(); mygyro->Reset(); }
/** * 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); }
/** * Reset the gyro. * Resets the gyro to a heading of zero. This can be used if there is significant * drift in the gyro and it needs to be recalibrated after it has been running. * @param slot The slot the analog module is connected to * @param channel The analog channel the gyro is plugged into */ void ResetGyro(UINT32 slot, UINT32 channel) { Gyro *gyro = AllocateGyro(slot, channel); if (gyro) gyro->Reset(); }
void Autonomous() { GetWatchdog().SetEnabled(false); Timer* hotGoalTimer = new Timer(); Timer* reloadTimer = new Timer(); Timer* intakeTimer = new Timer(); Timer* intakeDropTimer = new Timer(); bool goalFound = false; //bool rightSideHot = false; hotGoalTimer->Reset(); hotGoalTimer->Start(); gyro->Reset(); leftEnco->Reset(); rightEnco->Reset(); LEDLight->Set(Relay::kForward); //Find out the type of autonomous we are using int autonType = (int)SmartDashboard::GetNumber(NUM_BALL_AUTO); if(autonType == 2)//Set the auton mode to two if we are doing a two ball auto { autonMode = TWO_BALL_AUTON; autonStep = AUTON_TWO_WAIT_FOR_INTAKE; } else//Set the auton to one if the value on SD is set to 1 or another random value { autonMode = ONE_BALL_AUTON; autonStep = AUTON_ONE_FIND_HOT; } //Bring the intake down intake->DropIntake(); intakeDropTimer->Reset(); intakeDropTimer->Start(); while(IsAutonomous() && !IsDisabled()) { rpi->Read(); lcd->Printf(DriverStationLCD::kUser_Line1, 1, "L: %f", leftEnco->GetDistance()); lcd->Printf(DriverStationLCD::kUser_Line2, 1, "R: %f", rightEnco->GetDistance()); lcd->Printf(DriverStationLCD::kUser_Line3, 1, "T: %f", hotGoalTimer->Get()); lcd->Printf(DriverStationLCD::kUser_Line4, 1, "i: %i", rpi->GetMissingPacketcount()); lcd->Printf(DriverStationLCD::kUser_Line5, 1, "%i", rpi->GetXPos()); lcd->Printf(DriverStationLCD::kUser_Line6, 1, "%i", rpi->GetYPos()); lcd->UpdateLCD(); LEDLight->Set(Relay::kForward); if(autonMode == ONE_BALL_AUTON) { switch(autonStep) { case AUTON_ONE_FIND_HOT: //Reload the catapult and find the hot goal rpi->Read(); if(!goalFound) { //This is put into an if statement to protect against the //rpi finding the hot goal and then losing it goalFound = ((rpi->GetXPos() != RPI_ERROR_VALUE) || (rpi->GetYPos() != RPI_ERROR_VALUE)); } //Wait for the goal to be hot and the intake to move down if(((goalFound) || (hotGoalTimer->Get() >= 6.75)) && intakeDropTimer->Get() >= INTAKE_DROP_WAIT) { autonStep = AUTON_ONE_SHOOT; catapult->StartRelease(); } break; case AUTON_ONE_SHOOT: //Shoot the catapult if(!catapult->ReleaseHold()) { //Move on to the next step when the catapult is released autonStep = AUTON_ONE_WAIT; //Start the wait timer reloadTimer->Reset(); reloadTimer->Start(); } break; case AUTON_ONE_WAIT: if(reloadTimer->Get() >= CATAPULT_WAIT_TIME) { autonStep = AUTON_ONE_DRIVE_FORWARDS; reloadTimer->Stop(); //Start reloading the catapult catapult->StartLoad(); gyro->Reset(); } break; case AUTON_ONE_DRIVE_FORWARDS: //Drive forwards into the alliance zone and reload the catapult if((!GyroDrive(0, 0.75, 36)) && (!(bool)catapult->Load())) { autonStep = AUTON_END; } break; case AUTON_END: break; } } else if(autonMode == TWO_BALL_AUTON) { switch(autonStep) { /*case AUTON_TWO_RELOAD: //Determine if the hot goal is on the right if((rpi->GetXPos() != RPI_ERROR_VALUE) && (rpi->GetYPos() != RPI_ERROR_VALUE)) { rightSideHot = true; } //Reload the catapult if(!((bool)catapult->Load())) { autonStep = AUTON_TWO_FIRST_SHOOT; catapult->StartShoot(); } if((goalFound)) { //If the goal is found, the right side is hot and we can go to the next step rightSideHot = true; autonStep = AUTON_TWO_FIRST_SHOOT; } else if(hotGoalTimer->Get() >= 0.5) { //If the timer runs of, the right side is not hot and we can go to the next step rightSideHot = false; autonStep = AUTON_TWO_FIRST_TURN; } break;*/ case AUTON_TWO_WAIT_FOR_INTAKE: //wait for the intake to drop down if(intakeDropTimer->Get() >= INTAKE_DROP_WAIT) { autonStep = AUTON_TWO_FIRST_SHOOT; catapult->StartShoot(); } break; case AUTON_TWO_FIRST_SHOOT: if(catapult->GetLoadingState() == LOAD_RELEASE_TENSION) { intake->RollIn(); } if(!((bool)catapult->Shoot())) { autonStep = AUTON_TWO_INTAKE; intakeTimer->Reset(); intakeTimer->Start(); } break; case AUTON_TWO_INTAKE: intake->RollIn(); if(intakeTimer->Get() >= 1.5) { intake->Stop(); intakeTimer->Stop(); autonStep = AUTON_TWO_SECOND_SHOOT; catapult->StartRelease(); } break; case AUTON_TWO_SECOND_SHOOT: if(!catapult->ReleaseHold()) { autonStep = AUTON_TWO_WAIT; reloadTimer->Reset(); reloadTimer->Start(); } break; case AUTON_TWO_WAIT: if(reloadTimer->Get() >= 0.5) { reloadTimer->Stop(); leftEnco->Reset(); rightEnco->Reset(); catapult->StartLoad(); gyro->Reset(); autonStep = AUTON_TWO_DRIVE_FORWARDS; } break; case AUTON_TWO_DRIVE_FORWARDS: if((!GyroDrive(0, 0.9, 36)) && (!((bool)catapult->Load()))) { autonStep = AUTON_END; } break; /*case AUTON_TWO_FIRST_TURN: //Turn to the left 5* if the right goal is not hot if(!rightSideHot) { if(!GyroTurn(-5, 0.5)) { autonStep = AUTON_TWO_FIRST_SHOOT; } } else { autonStep = AUTON_TWO_FIRST_SHOOT; } break; case AUTON_TWO_FIRST_SHOOT: //Release the catapult to shoot if(!catapult->ReleaseHold()) { autonStep = AUTON_TWO_FIRST_WAIT; reloadTimer->Reset(); reloadTimer->Start(); } break; case AUTON_TWO_FIRST_WAIT: if(reloadTimer->Get() >= CATAPULT_WAIT_TIME) { autonStep = AUTON_TWO_SECOND_TURN; catapult->StartLoad(); reloadTimer->Stop(); } break; case AUTON_TWO_SECOND_TURN: //Turn the robot so it's facing forwards and reload the catapult if((!GyroTurn(0, 0.5)) && (!(bool)catapult->Load())) { autonStep = AUTON_TWO_GET_SECOND_BALL; } break; case AUTON_TWO_GET_SECOND_BALL: //Start up the intake and drive back to pick up the second ball intake->RollIn(); if(!GyroDrive(0, -0.5, -12)) { autonStep = AUTON_TWO_THIRD_TURN; leftEnco->Reset(); rightEnco->Reset(); } break; case AUTON_TWO_THIRD_TURN: //If the right goal was originally hot, turn left if(rightSideHot) { if(!GyroTurn(-5, 0.5)) { autonStep = AUTON_TWO_SECOND_SHOOT; } } else { autonStep = AUTON_TWO_SECOND_SHOOT; } break; case AUTON_TWO_SECOND_SHOOT: intake->Stop(); if(!catapult->ReleaseHold()) { autonStep = AUTON_TWO_SECOND_WAIT; reloadTimer->Reset(); reloadTimer->Start(); } break; case AUTON_TWO_SECOND_WAIT: if(reloadTimer->Get() >= CATAPULT_WAIT_TIME) { autonStep = AUTON_TWO_DRIVE_FORWARDS; catapult->StartLoad(); reloadTimer->Stop(); } break; case AUTON_TWO_DRIVE_FORWARDS: if(!GyroDrive(0, 0.75, 60) && (!(bool)catapult->Load())) { autonStep = AUTON_END; } break;*/ case AUTON_END: break; } } } }
/**************************************** * 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(); }