/** * 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; }
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); } } }
void DemoSceneManager::draw(double deltaT) { _time += deltaT; float angle = _time * .1; // .1 radians per second glClearColor(0.25f, 0.25f, 0.25f, 1.0f); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LEQUAL); glCullFace(GL_BACK); glEnable(GL_CULL_FACE); Gyro *gyro = Gyro::getInstance(); gyro->read(); vmml::mat4f translation = vmml::create_translation(vmml::vec3f(_scrolling.x(), -_scrolling.y(), 0)); vmml::mat4f scaling = vmml::create_scaling(vmml::vec3f(.2f)); vmml::mat3f rotation = vmml::create_rotation(gyro->getRoll() * -M_PI_F - .3f, vmml::vec3f::UNIT_Y) * vmml::create_rotation(gyro->getPitch() * -M_PI_F + .3f, vmml::vec3f::UNIT_X); _eyePos = rotation * vmml::vec3f(0.0f, 0.0f, 0.0f); vmml::vec3f eyeUp = vmml::vec3f::DOWN; _viewMatrix = lookAt(_eyePos, vmml::vec3f::UP, rotation * eyeUp); _modelMatrix = translation * scaling; // drawModel2("quad"); drawModel("tunnel3"); // drawModel2("quad"); _eyePos = rotation * vmml::vec3f(0.0f, 2.0f, 0.0f); _viewMatrix = lookAt(_eyePos, vmml::vec3f::UP, rotation * eyeUp); }
/** * Set the gyro type based on the sensitivity. * This takes the number of volts/degree/second sensitivity of the gyro and uses it in subsequent * calculations to allow the code to work with multiple gyros. * * @param slot The slot the analog module is connected to * @param channel The analog channel the gyro is plugged into * @param voltsPerDegreePerSecond The type of gyro specified as the voltage that represents one degree/second. */ void SetGyroSensitivity(UINT32 slot, UINT32 channel, float voltsPerDegreePerSecond) { Gyro *gyro = AllocateGyro(slot, channel); if (gyro) gyro->SetSensitivity(voltsPerDegreePerSecond); }
/** * 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 }
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(); }
/** * 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(); }
//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 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); }
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; } }
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(); }
int main() { //Handle Ctrl-C quit signal(SIGINT, sig_handler); Shield *shield = new Shield(); //two motor setup Motor left_motor(15, 0); Motor right_motor(4, 2); Gyro gyro; IR medA = IR(2, 6149.816568, 4.468768853); IR medB = IR(1, 2391.189039, -0.079559138); //was 0.015, 0, 0.4 //also 0.05, 0, 0.2 //for medA: PIDDrive driveA(&left_motor, &right_motor, shield, 0.00001, 0.0001, 0.2); PIDDrive driveB(&left_motor, &right_motor, shield, 0.00001, 0.0001, 0.1); int straight = 0; int turning = 0; double curAngle = gyro.get_angle(); /*double startDist = medA.getDistance(); double dist[5] = {startDist, startDist, startDist, startDist, startDist}; double avg = startDist; double sum = 0; */ while (running) { /*sum = 0; for (int i = 1; i <= 4; i++) { dist[i-1] = dist[i]; } dist[4] = medA.getDistance(); for (int i = 0; i < 5; i++) { sum += dist[i]; } avg = sum / 5.; */ std::cout << "sensor A: " << medA.getDistance() << "\t"; std::cout << "sensor B: " << medB.getDistance() << std::endl; //std::cout << "avg: " << avg << std::endl; /*if (medA.getDistance() < 20 && medB.getDistance() > 30) { turning = 0; if (straight == 0) { curAngle = gyro.get_angle(); } straight++; driv.drive(curAngle, gyro.get_angle(), 0.25); //keep driving straight std::cout << "straight\t" << "angle: " << curAngle << std::endl; } else if ((medA.getDistance() < 20 && medB.getDistance() < 30) || (medA.getDistance() > 30)) { straight = 0; if (turning == 0) { curAngle = gyro.get_angle(); } turning++; //drive.drive(curAngle + 10, gyro.get_angle(), .2); //turn away from wall drive.stop(); straight = 0; sleep(.2); std::cout << "turning " << "angle: " << curAngle << std::endl; } else { drive.drive(gyro.get_angle(), gyro.get_angle(), 0.2); }*/ if (medB.getDistance() < 15) { driveA.stop(); driveB.stop(); while (medB.getDistance() < 30) { left_motor.setSpeed(shield, 0.2); right_motor.setSpeed(shield, -0.2); std::cout << "B" << std::endl; } } else if (medA.getDistance() > 80) { left_motor.setSpeed(shield, 0.2); right_motor.setSpeed(shield, -0.2); usleep(300000); } else { turning = 0; driveA.drive(15, medA.getDistance(), 0.2); std::cout << "A" << std::endl; usleep(100000); } /* if (medA.getDistance() < 14) { left_motor.setSpeed(shield, 0.2); right_motor.setSpeed(shield, -0.15); } else if (medA.getDistance() > 16) { left_motor.setSpeed(shield, -0.15); right_motor.setSpeed(shield, 0.2); } else { left_motor.setSpeed(shield, 0.2); right_motor.setSpeed(shield, 0.2); } */ /*if (avg < 30) { drive.drive(gyro.get_angle(), gyro.get_angle(), -.25); } else { drive.drive(gyro.get_angle(), gyro.get_angle(), 0.25); }*/ usleep(50000); //sleep(1); } }
/** * 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); }
/** * 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(); }
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; } }
/**************************************** * 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); } }
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; } } } } }
int main() { //Handle Ctrl-C quit signal(SIGINT, sig_handler); Shield *shield = new Shield(); //two motor setup Motor left_motor(15, 2); Motor right_motor(4, 8); Gyro gyro; IR medA = IR(1, 6149.816568, 4.468768853); //was 0.015, 0, 0.4 PIDDrive drive(&left_motor, &right_motor, shield, 0.05, 0, 0.2); int straight = 0; int turning = 0; double curAngle = gyro.get_angle(); /*double startDist = medA.getDistance(); double dist[5] = {startDist, startDist, startDist, startDist, startDist}; double avg = startDist; double sum = 0; */ while (running) { /*sum = 0; for (int i = 1; i <= 4; i++) { dist[i-1] = dist[i]; } dist[4] = medA.getDistance(); for (int i = 0; i < 5; i++) { sum += dist[i]; } avg = sum / 5.; */ std::cout << "sensor: " << medA.getDistance() << std::endl; //std::cout << "avg: " << avg << std::endl; if (medA.getDistance() < 30) { if (turning == 0) { curAngle = gyro.get_angle(); } turning++; drive.drive(curAngle + 90, gyro.get_angle(), .25); //turn away from wall straight = 0; sleep(.75); std::cout << "turning " << "angle: " << curAngle << std::endl; } else { turning = 0; if (straight == 0) { curAngle = gyro.get_angle(); } straight++; drive.drive(curAngle, gyro.get_angle(), 0.25); //keep driving straight std::cout << "straight\t" << "angle: " << curAngle << std::endl; //sleep(0.25); } /*if (avg < 30) { drive.drive(gyro.get_angle(), gyro.get_angle(), -.25); } else { drive.drive(gyro.get_angle(), gyro.get_angle(), 0.25); }*/ usleep(30000); } }
/** * 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(); }
static void SetUpTestCase() { // The gyro object blocks for 5 seconds in the constructor, so only // construct it once for the whole test case m_gyro = new Gyro(TestBench::kCameraGyroChannel); m_gyro->SetSensitivity(0.013); }
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; } } } }
int main(void) { int enc_now[2] = {}, enc_old[2] = {}, push_time[3] = {}, _build_mode = 0; float yaw_now = 0, yaw_old = 0; bool Start = false; //Initialise Systick MillisecondTimer::initialise(); Nvic::initialise(); MainV3 mainBoard; SpiMotor spimotor( *mainBoard.spi3v1, 1); MillisecondTimer::delay(100); PS3Con *ps3con = new PS3Con(); mainBoard.can.AddListenerNode(ps3con); mainBoard.mpu6050.setTimeout(20); debug<<"[INFO]Testing MPU6050...\r\n"; while(!mainBoard.mpu6050.test()); debug<<"[INFO]MPU6050 test passed.\r\n"; debug<<"[INFO]Setting up MPU6050...\r\n"; mainBoard.mpu6050.setup(); mainBoard.mpu6050.setGyrRange(mainBoard.mpu6050.GYR_RANGE_500DPS); debug<<"[INFO]Setup complete!\r\n"; SensorTimer sensor_timer(&mainBoard); Machine machine; Gyro gyro; Builder build(&mainBoard,&machine); //Reset build.Reset(); while (1) { int rotation_gyro = 0, rotation = 0, D_out = 0, SPI_out = 0, X_100 = 0, Y_100 = 0; float A_out = 0, B_out = 0, C_out = 0; mainBoard.can.Update(); mainBoard.buzzer.stop(); //Safety start if (Start == false || ps3con->getButtonPress(CONNECTED) == 0) { if (ps3con->getButtonPress(START)){ Start = true; debug << "[INFO]Safety launch success!\r\n"; }else{ debug << "[WARN]Please push START button.\r\n"; } if (Start == true) mainBoard.buzzer.set(-1, 6); else mainBoard.buzzer.flash(); mainBoard.led.flash(); build.changeMode(NO_CAHNGE); D_out = build.pwmArm((int)mainBoard.ad[2]->get()); SPI_out = build.pwmPlate((int)mainBoard.ad[3]->get()); setLimit(D_out,300); setLimit(SPI_out,200); mainBoard.motorD.setOutput((float) D_out / 500.0); spimotor.setOutput((float) SPI_out / 500.0); MillisecondTimer::delay(50); continue; }else if(Start == true && ps3con->getButtonPress(START)){//Emergency stop mainBoard.buzzer.set(-1, 4); debug << "[WARN]Emergency stop!!\r\n"; Start = false; build.Reset(); //PWM output mainBoard.motorA.setOutput(0 / 500.0); mainBoard.motorB.setOutput(0 / 500.0); mainBoard.motorC.setOutput(0 / 500.0); mainBoard.motorD.setOutput(0 / 500.0); spimotor.setOutput(0/500.0); MillisecondTimer::delay(200); continue; } mainBoard.led.On(); enc_now[X] = mainBoard.encoders.getCounter1(); enc_now[Y] = mainBoard.encoders.getCounter2(); machine.extendEncValue(enc_now[X],enc_old[X],X); machine.extendEncValue(enc_now[Y],enc_old[Y],Y); sensor_timer.ahrs.getYaw(yaw_now); yaw_now *= RAD_TO_DEG; gyro.set(yaw_now,yaw_old); rotation_gyro = gyro.correct(); setLimit(rotation_gyro,R_Gyro_LIMIT); //Rotate (angle of 90) if (ps3con->getButtonPress(R1) && push_time[0] == 0){ mainBoard.buzzer.set(-1, 5); gyro.setAngle90(90); push_time[0] = 1; } else if (ps3con->getButtonPress(L1) && push_time[0] == 0){ mainBoard.buzzer.set(-1, 5); gyro.setAngle90(-90); push_time[0] = 1; } //Rotate (Normal) if(ps3con->getButtonPress(L2)||ps3con->getButtonPress(R2)){ gyro.reset(rotation_gyro); rotation = (ps3con->getAnalog(ANALOG_R2) - ps3con->getAnalog(ANALOG_L2)) / 2; } machine.antiSlip(rotation,ROTATE,NO_CAHNGE); setLimit(rotation,R_LIMIT); //change manual build mode if(ps3con->getButtonPress(SELECT) && push_time[1] == 0){ mainBoard.buzzer.set(-1, 5); push_time[1] = 1; if(_build_mode == AUTO){ _build_mode = MANUAL; build.changeMode(-2); } else{ _build_mode = AUTO; build.changeMode(-6); } } //Build capital (push_time) if(ps3con->getButtonPress(CIRCLE) && push_time[2] == 0){ mainBoard.buzzer.set(-1, 5); push_time[2] = 1; if(_build_mode == AUTO){ build.changeMode(); }else if(_build_mode == MANUAL){ if(build.getMode() == -2) build.changeMode(1);//Get else build.changeMode(-2);//Release } build.resetPause(); }else if(ps3con->getButtonPress(CROSS) && push_time[2] == 0 && _build_mode == AUTO){ mainBoard.buzzer.set(-1, 4); push_time[2] = 1; build.changeMode(build.getMode() - 1); build.resetPause(); } build.changeMode(NO_CAHNGE); for(int i = 0;i < 3;i++){ if(push_time[i] != 0){ push_time[i]++; if(push_time[i] > 10) push_time[i] = 0; } } X_100 = ps3con->convertValue(ps3con->getAnalog(ANALOG_L_X),build.getGain()); Y_100 = ps3con->convertValue(ps3con->getAnalog(ANALOG_L_Y),build.getGain()); //Adjust bit if(ps3con->getButtonPress(TRIANGLE)){ if (ps3con->getButtonPress(RIGHT)){ if(X_100 < SPEED_Linearly) X_100 += 20;} else if (ps3con->getButtonPress(LEFT)){ if(X_100 > -SPEED_Linearly) X_100 -= 20;} else if (ps3con->getButtonPress(UP)){ if(Y_100 > -SPEED_Linearly) Y_100 -= 20;} else if (ps3con->getButtonPress(DOWN)){ if(Y_100 < SPEED_Linearly) Y_100 += 20;} else{ X_100 = 0; Y_100 = 0; } } //Automatically adjust the distance between the object if (ps3con->getButtonPress(SQUARE) && ps3con->getButtonPress(UP)) { Y_100 = machine.adjustDistance((int)mainBoard.ad[0]->get(),build.getMode(),Y); if(Y_100 == 0) mainBoard.buzzer.set(-1, 6); } //Automatically adjust the distance between the wall if (ps3con->getButtonPress(SQUARE) && ps3con->getButtonPress(LEFT)) { X_100 = machine.adjustDistance((int)mainBoard.ad[1]->get(),build.getMode(),X); if(X_100 == 0) mainBoard.buzzer.set(-1, 6); } //Automatically run to target // if (ps3con->getButtonPress(CROSS)) { // X_100 = machine.runToTarget(X); // Y_100 = machine.runToTarget(Y); // // if(X_100 == 0 && Y_100 == 0) mainBoard.buzzer.set(-1, 6); // } X_100 = machine.antiSlip(X_100, X,build.getMode()); Y_100 = machine.antiSlip(Y_100, Y,build.getMode()); //motor A_out = X_100 / 2 - Y_100 * sqrt(3) / 2 + rotation + rotation_gyro; B_out = X_100 / 2 + Y_100 * sqrt(3) / 2 + rotation + rotation_gyro; C_out = -X_100 + rotation + rotation_gyro; //switch Arm & Plate mode if(_build_mode == AUTO){ D_out = build.pwmArm((int)mainBoard.ad[2]->get()); SPI_out = build.pwmPlate((int)mainBoard.ad[3]->get()); }else{ int dust = 0;//no function dust = build.pwmArm((int)mainBoard.ad[2]->get()); dust = build.pwmPlate((int)mainBoard.ad[3]->get()); D_out = -ps3con->convertValue(ps3con->getAnalog(ANALOG_R_Y),5); } //debug //SPI_out = -ps3con->convertValue(ps3con->getAnalog(ANALOG_R_Y),2); //D_out = -ps3con->convertValue(ps3con->getAnalog(ANALOG_R_Y),5); setLimitFloat(A_out,PWM_LIMIT); setLimitFloat(B_out,PWM_LIMIT); setLimitFloat(C_out,PWM_LIMIT); setLimit(D_out,PWM_LIMIT); setLimit(SPI_out,110); //PWM output mainBoard.motorA.setOutput(A_out / 500.0); mainBoard.motorB.setOutput(B_out / 500.0); mainBoard.motorC.setOutput(C_out / 500.0); mainBoard.motorD.setOutput((float)D_out / 500.0); spimotor.setOutput((float)SPI_out/500.0); yaw_old = yaw_now; enc_old[X] = enc_now[X]; enc_old[Y] = enc_now[Y]; //debug char str[128]; sprintf(str, "[DEBUG]Mode:%d,%d Arm,Plate:%s,%s PS3con:%d,%d omniPWM:%.0f,%.0f,%.0f Gyro:%d Rotate:%d Arm:%d Plate:%d\r\n" ,build.getMode(),_build_mode,build.getComp(ARM)?"O":"X",build.getComp(PLATE)?"O":"X" ,X_100,Y_100,A_out,B_out,C_out,rotation_gyro,rotation,(int)mainBoard.ad[2]->get(),(int)mainBoard.ad[3]->get()); //sprintf(str,"[TEST]%d %d\r\n",(int)mainBoard.ad[3]->get(),SPI_out); //sprintf(str,"[TEST]Front:%d Left:%d\r\n",(int)mainBoard.ad[0]->get(),(int)mainBoard.ad[1]->get()); debug << str; MillisecondTimer::delay(50); } }