void Autonomous(void) { dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix"); dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Autonomous Mode"); dsLCD->UpdateLCD(); }
/** * Runs the motors with arcade steering. */ void OperatorControl(void) { HSLImage *Himage; Threshold targetThreshold(247, 255, 60, 140, 10, 50); BinaryImage *matchingPixels; vector<ParticleAnalysisReport> *pReport; //myRobot->SetSafetyEnabled(true); Saftey->SetEnabled(false); AxisCamera &mycam = AxisCamera::GetInstance("10.15.10.11"); mycam.WriteResolution(AxisCamera::kResolution_640x480); mycam.WriteCompression(20); mycam.WriteBrightness(25); Wait(3.0); dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); float X[2]; float Y[2]; float Z[2]; while(IsOperatorControl()) { X[1] = Stick1->GetX(); X[2] = Stick2->GetX(); Y[1] = Stick1->GetY(); Y[2] = Stick2->GetY(); Z[1] = Stick1->GetZ(); Z[2] = Stick2->GetZ(); Jaguar1->Set(Y[1]); Jaguar2->Set(Y[2]); Wait(0.005); if (mycam.IsFreshImage()) { Himage = mycam.GetImage(); matchingPixels = Himage->ThresholdHSL(targetThreshold); pReport = matchingPixels->GetOrderedParticleAnalysisReports(); for (unsigned int i = 0; i < pReport->size(); i++) { printf("Index: %d X Center: %d Y Center: %d \n", i, (*pReport)[i].center_mass_x, (*pReport)[i].center_mass_y); } delete Himage; delete matchingPixels; delete pReport; } } //myRobot->ArcadeDrive(stick); // drive with arcade style (use right stick) //Wait(0.005); // wait for a motor update time }
DriverStationLCDTextExample() { dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); control = new LCDControl(); dsLCD->UpdateLCD(); }
void OperatorControl() { // Loop counter to ensure that the program is running (debug helper // that can be removed when things get more stable) int sanity, bigSanity = 0; gamepad.Update(); while (IsOperatorControl() && IsEnabled()) { controls = Controls::GetInstance(); controls->SetSpeed(LEFT_DRIVE_PWM, -1.0 * gamepad.GetRightY()); controls->SetSpeed(RIGHT_DRIVE_PWM, -1.0 * gamepad.GetRightY()); gamepad.Update(); dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix"); dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Teleop Mode"); dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "bigSanity: %d", sanity); dsLCD->UpdateLCD(); sanity++; if (0 == sanity % 20) { bigSanity++; } Wait(0.05); // wait for a motor update time } }
void RobotInit(void) { DriverStationLCD *dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); dsLCD->UpdateLCD(); //blnShift = true; }
void Zed::updateDriverStation() { DriverStationLCD* lcd = DriverStationLCD::GetInstance(); lcd->Clear(); lcd->PrintfLine(DriverStationLCD::kUser_Line1, 0, "Shooter Speed: %f", shooterSpeed); lcd->UpdateLCD(); }
RobotDemo(void) : signal(3), signalControlVoltage(7) { dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "SonarTest"); dsLCD->UpdateLCD(); }
Hohenheim(void) { driverStation = DriverStation::GetInstance(); dsLCD = DriverStationLCD::GetInstance(); pneumaticsControl = PneumaticsControl::getInstance(); shooterControl = ShooterControl::getInstance(); dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hohenheim 2014 V 3.1"); dsLCD->UpdateLCD(); GetWatchdog().SetEnabled(false); }
RobotDemo(void): myRobot(1, 2), // these must be initialized in the same order stick(1) // as they are declared above. { dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "XboxController2"); dsLCD->UpdateLCD(); myRobot.SetExpiration(0.1); }
SebastianRobot(void) { dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Sebastian V24.2"); dsLCD->UpdateLCD(); GetWatchdog().SetEnabled(false); led0 = new Relay(2); led1 = new Relay(3); flashCount = 0; led0->Set(Relay::kOff); led1->Set(Relay::kOff); isFlashing=false; }
//should move this to helper function void robot::feed() { DriverStationLCD *lcd = DriverStationLCD::GetInstance(); lcd->Clear(); float th = gyro.getangle(); a = qmod(th * dt + a, -180, 180); lcd->PrintfLine(DriverStationLCD::kUser_Line1, "BUILD: %i", BUILD); lcd->PrintfLine(DriverStationLCD::kUser_Line2, "%f", gyrob.GetAngle()); lcd->PrintfLine(DriverStationLCD::kUser_Line3, "%f", arma.GetVoltage()); lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%f", armb.GetVoltage()); //lcd->PrintfLine(DriverStationLCD::kUser_Line5, "%f", aa); lcd->UpdateLCD(); }
void Disabled() { while(IsDisabled()) { LEDLight->Set(Relay::kForward); rpi->Read(); lcd->Clear(); lcd->Printf(DriverStationLCD::kUser_Line3, 1, "R: %i", rpi->GetMissingPacketcount()); lcd->Printf(DriverStationLCD::kUser_Line1, 1, "x: %i", rpi->GetXPos()); lcd->Printf(DriverStationLCD::kUser_Line2, 1, "y: %i", rpi->GetYPos()); lcd->UpdateLCD(); } }
void Print () { if (PrintTime.Get() > PRINT_TIME) { lcd->Clear(); lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Left Speed = %5.4f", PrimaryController.GetRawAxis(LEFT_JOYSTICK)); lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Right Speed = %5.4f", PrimaryController.GetRawAxis(RIGHT_JOYSTICK)); lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Charge State = %d", (int)Shooter.chargestate); //lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Collector speed= %d", Collector.CollectorSpeed()); lcd->UpdateLCD(); PrintTime.Reset(); PrintTime.Start(); } }
void OperatorControl(void) { XboxController *xbox = XboxController::getInstance(); bool isEndGame = false; GetWatchdog().SetEnabled(true); _driveControl.initialize(); //_poleVaultControl.initialize(); shooterControl.InitializeOperator(); while (IsEnabled() && IsOperatorControl()) { GetWatchdog().Feed(); dsLCD->Clear(); if (xbox->isEndGame()) { isEndGame = !isEndGame; } if (!isEndGame) { shooterControl.Run(); //_poleVaultControl.act(); _driveControl.act(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Normal"); led0->Set((shooterControl.getLED1())?Relay::kOn: Relay::kOff); led1->Set((shooterControl.getLED2())?Relay::kOn: Relay::kOff); } else { shooterControl.RunEndGame(); //_poleVaultControl.actEndGame(); _driveControl.actEndGame(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "End Game"); flashCount--; if (flashCount<=0){ isFlashing = !isFlashing; flashCount=FLASHTIME; } led0->Set((isFlashing)?Relay::kOn: Relay::kOff); led1->Set((isFlashing)?Relay::kOn: Relay::kOff); } // dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "Flash: %i", flashCount); dsLCD->UpdateLCD(); Wait(WAIT_TIME); // wait for a motor update time } GetWatchdog().SetEnabled(false); }
void OperatorControl(void) { GetWatchdog().SetEnabled(true); dsLCD->Clear(); dsLCD->UpdateLCD(); driveControl.initialize(); pneumaticsControl->initialize(); shooterControl->initialize(); while (IsOperatorControl() && IsEnabled()) { GetWatchdog().Feed(); driveControl.run(); shooterControl->run(); dsLCD->UpdateLCD(); Wait(0.005); // wait for a motor update time } pneumaticsControl->disable(); }
RobotDemo(void): gamepad(3) { dsLCD = DriverStationLCD::GetInstance(); // Output the program name and build date/time in the hope that this will help // us catch cases where we are downloading a program other than the one // we think we are downloading. Keep in mind that if this source file // does not change (and you don't do a complete rebuild) the timestamp // will not change. dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Menu"); dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__); dsLCD->UpdateLCD(); }
FRC2994_2014(): leftFrontDrive(LEFT_FRONT_DRIVE_PWM), leftRearDrive(LEFT_REAR_DRIVE_PWM), rightFrontDrive(RIGHT_FRONT_DRIVE_PWM), rightRearDrive(RIGHT_REAR_DRIVE_PWM), robotDrive(&leftFrontDrive, &leftRearDrive, &rightFrontDrive, &rightRearDrive), rightStick(RIGHT_DRIVE_STICK) { // Print an I'M ALIVE message before anything else. NOTHING ABOVE THIS LINE. dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, ROBOT_NAME); dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " " __TIME__); dsLCD->UpdateLCD(); robotDrive.SetExpiration(0.1); }
/** * Runs during test mode ``````` */ void Test() { TestMode tester(m_ds); driveDistanceRight.Reset(); driveDistanceRight.Start(); ballGrabber.resetSetPoint(); shooter.motorShutOff(); while (IsTest() && IsEnabled()){ lcd->Clear(); tester.PerformTesting(&gamePad, &driveDistanceRight, lcd, &rightJoyStick, &leftJoyStick, &testSwitch, &testTalons, &frontUltrasonic, &backUltrasonic, &ballGrabber.ballDetector, &analogTestSwitch, &shooter, &ballGrabber ); lcd->UpdateLCD(); Wait(0.1); } driveDistanceRight.Stop(); }
RobotDemo(void): leftDriveMotor(LEFT_DRIVE_PWM), rightDriveMotor(RIGHT_DRIVE_PWM), myRobot(&leftDriveMotor, &rightDriveMotor), // these must be initialized in the same order stick(1), // as they are declared above. stick2(2), gamepad(3), collectorMotor(PICKUP_PWM), indexerMotor(INDEX_PWM), shooterMotor(SHOOTER_PWM), armMotor (ARM_PWM), leftDriveEncoder(LEFT_DRIVE_ENC_A, LEFT_DRIVE_ENC_B), shifter(SHIFTER_A,SHIFTER_B), greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED), yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED), potentiometer(ARM_ROTATION_POT), indexSwitch(INDEXER_SW), greenClawLockSwitch(CLAW_1_LOCK_SENSOR), yellowClawLockSwitch(CLAW_2_LOCK_SENSOR), compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE), jogTimer(), shooterTimer() { m_collectorMotorRunning = false; m_shooterMotorRunning = false; m_jogTimerRunning = false; m_shiftCount = MAX_SHIFTS; dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME); dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__); dsLCD->UpdateLCD(); myRobot.SetExpiration(0.1); shifter.Set(DoubleSolenoid::kReverse); leftDriveEncoder.SetDistancePerPulse(DRIVE_ENCODER_DISTANCE_PER_PULSE); leftDriveEncoder.SetMaxPeriod(1.0); leftDriveEncoder.SetReverseDirection(true); // change to true if necessary leftDriveEncoder.Start(); }
FRC2994_2014(): leftFrontDrive(LEFT_FRONT_DRIVE_PWM), leftRearDrive(LEFT_REAR_DRIVE_PWM), rightFrontDrive(RIGHT_FRONT_DRIVE_PWM), rightRearDrive(RIGHT_REAR_DRIVE_PWM), leftCenterDrive(CENTER_LEFT_DRIVE_PWM), rightCenterDrive(CENTER_RIGHT_DRIVE_PWM), intake(INTAKE_MOTOR_PWM), rightWinch(RIGHT_WINCH_MOTOR_PWM), leftWinch(LEFT_WINCH_MOTOR_PWM), robotDrive(leftFrontDrive, leftRearDrive, leftCenterDrive, rightCenterDrive, rightFrontDrive, rightRearDrive), rightStick(RIGHT_DRIVE_STICK), leftStick(LEFT_DRIVE_STICK), gamepad(GAMEPAD_PORT), shifters(SHIFTER_A, SHIFTER_B), arm(ARM_A, ARM_B), eject(EJECT_A, EJECT_B), winchSwitch(WINCH_SWITCH), leftDriveEncoder(LEFT_ENCODER_A, LEFT_ENCODER_B), rightDriveEncoder(RIGHT_ENCODER_A, RIGHT_ENCODER_B), compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE), // Robot starts off in a loaded state so a ball can be fit in loaded(true), loading(false), armDown(false) { // Print an I'M ALIVE message before anything else. NOTHING ABOVE THIS LINE. dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, ROBOT_NAME); dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, __DATE__ " " __TIME__); dsLCD->UpdateLCD(); ds = DriverStation::GetInstance(); // Set the experation to 1.5 times the loop speed. robotDrive.SetExpiration(LOOP_PERIOD*1.5); leftDriveEncoder.SetReverseDirection(true); }
void OperatorControl(void) { // Loop counter to ensure that the program us running (debug helper // that can be removed when things get more stable) int sanity, bigSanity = 0; while (IsOperatorControl() && IsEnabled()) { dsLCD->Clear(); dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix"); dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Teleop Mode"); dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "bigSanity: %d", sanity); dsLCD->UpdateLCD(); sanity++; if (0 == sanity % 20) { bigSanity++; } Wait(1.0); // wait for a motor update time } }
virtual void RobotInit() { CommandBase::init(); DriverStationLCD *dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); dsLCD->UpdateLCD(); //Wait(0.1); //_autonomousCommand = new AutonomousCommand(); _autonomousCommand = NULL; _teleopCommand = NULL; _moveToAndDropBridgeCmd = new MoveToAndDropBridgeCmd(); _noOpCmd = new NoOpCmd(); _driveInSquareCmd = new DriveInSquareCmd(); _kinectDriveCmd = new KinectDriveCmd(CommandBase::oi->kinectLeft, CommandBase::oi->kinectRight, CommandBase::oi->kinectBridgeBtn); _testRobotCmd = new TestRobotCmd(CommandBase::oi->driveTrigger); _autoFireCmd = new AutonomousFireCmd(); //_driveWithJoysticksCmd = new DriveWithJoysticksCmd(); _autoChooser = new SendableChooser(); // _autoChooser->AddDefault("Autonomous Fire", _autoFireCmd); _autoChooser->AddDefault("Move To And Drop Bridge", _moveToAndDropBridgeCmd); // _autoChooser->AddObject("Move To And Drop Bridge", _moveToAndDropBridgeCmd); _autoChooser->AddObject("Autonomous Fire", _autoFireCmd); _autoChooser->AddObject("Drive with Kinect", _kinectDriveCmd); _autoChooser->AddObject("Drive In Square", _driveInSquareCmd); SmartDashboard::GetInstance()->PutData("AutoMenu", _autoChooser); _teleopChooser = new SendableChooser(); _teleopChooser->AddDefault("Drive With Joysticks", _noOpCmd); _teleopChooser->AddObject("Test Robot", _testRobotCmd); SmartDashboard::GetInstance()->PutData("TeleopMenu", _teleopChooser); // SmartDashboard::GetInstance()->PutData("SchedulerData", Scheduler::GetInstance()); // SmartDashboard::GetInstance()->PutData("DriveTrainSubsystem", CommandBase::driveTrainSubsystem); // SmartDashboard::GetInstance()->PutData("AzimuthSubsystem", CommandBase::azimuthSubsystem); }
/** * Drive left & right motors for 2 seconds then stop */ void Autonomous(void) { Saftey->SetEnabled(false); //myRobot->SetSafetyEnabled(false); //myRobot->Drive(0.5, 0.0); // drive forwards half speed dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); //dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Hello World" ); //dsLCD->UpdateLCD(); Wait(0.5); ds = DriverStation::GetInstance(); switch(ds->GetLocation()) { case 1: //Execute Autonomous code #1 dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 1"); break; case 2: dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 2"); //Execute Autonomous code #2 break; case 3: dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 3"); //Execute Autonomous code #3 break; } dsLCD->UpdateLCD(); Saftey->SetEnabled(false); Wait(0.5); // for 2 seconds delete Jaguar1; delete Jaguar2; delete Saftey; delete dsLCD; delete ds; }
void Autonomous(void) { GetWatchdog().SetEnabled(true); bool isHybrid = false; Kinect* kinect = Kinect::GetInstance(); isHybrid = (kinect->GetNumberOfPlayers() > 0); if (!isHybrid) { _driveControl.initializeAutonomous(); shooterControl.InitializeAutonomous(); _poleVaultControl.initialize(); } else { _driveControl.initializeHybrid(); shooterControl.InitializeHybrid(); _poleVaultControl.initialize(); } while (IsEnabled() && IsAutonomous()) { GetWatchdog().Feed(); dsLCD->Clear(); if (!isHybrid) { //Run Autonomous dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Autonomous Mode"); //if (_driveControl.RunAuto()) { shooterControl.RunAuto(); //} // if(_driveControl.RunAuto()){ // _poleVaultControl.deploy(); // } } else { //Run Hybrid dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hybrid Mode"); shooterControl.Run(); _driveControl.act(); _poleVaultControl.act(); } dsLCD->UpdateLCD(); Wait(WAIT_TIME); } GetWatchdog().SetEnabled(false); }
void OperatorControl() { if(!m_FromAutonomous){ init(); } m_FromAutonomous = false; driveTrain.SetSafetyEnabled(true); int printDelay = 0; int shootDelay = 0; //bool SavePreferencesToFlash = false; while (IsOperatorControl() && IsEnabled()) { /* bool SavePreferences = gamePad.GetRawButton(8); if (SavePreferences){ double elevatorAngleValue = SmartDashboard::GetNumber("Angle"); dashboardPreferences->PutDouble("Angle", elevatorAngleValue); SavePreferencesToFlash = true; } */ printDelay ++; float rJoyStick = limitSpeed(rightJoyStick.GetY()); float lJoyStick = limitSpeed(leftJoyStick.GetY()); bool button6 = gamePad.GetRawButton(6); //speedLimiter.SetMaxOutput(SmartDashboard::GetNumber("Slider 1")); driveTrain.TankDrive(lJoyStick, rJoyStick); //manual mode(no PID) for elevator //float dPadThumbstick = TestMode::GetThumbstickWithZero(&gamePad); //ballGrabber.DriveElevatorTestMode(dPadThumbstick); //Sets motor equal to the elevator sensor. //TODO Probably don't need but want to test because called inside operate grabber. ballGrabber.OperatePIDLoop(); if(printDelay == 100){ lcd->Clear(); if(m_display_page_1) { lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Teleop pg1"); lcd->PrintfLine(DriverStationLCD::kUser_Line2, "FR %4.0f, BA %4.0f", frontUltrasonic.GetAverageDistance(), backUltrasonic.GetAverageDistance()); shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd); SmartDashboard::PutNumber("UltrasonicF", 1); SmartDashboard::PutNumber("UltrasonicB", 1); SmartDashboard::PutNumber("ElvatorAngle", 2);//Change keyname to ElavatorAngle from (ElvatorAngle) //^^ if(button6){ m_display_page_1 = false; } } else{ lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Teleop pg2 %c", button6 ? '1':'0'); ballGrabber.DisplayDebugInfo(DriverStationLCD::kUser_Line2,lcd); //lcd->PrintfLine(DriverStationLCD::kUser_Line3, "G%f", ballGrabber.ballDetector.GetDistance()); //ballGrabber.UpDateWithState(DriverStationLCD::kUser_Line3,lcd); shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd); //lcd->PrintfLine(DriverStationLCD::kUser_Line4, "EV%6.2f", ballGrabber.elevatorAngleSensor.GetVoltage()); shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line4, lcd); //lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%5.3f %5.3f %5.3f", lJoyStick, rJoyStick, SmartDashboard::GetNumber("Slider 1")); lcd->PrintfLine(DriverStationLCD::kUser_Line5, "DEV=%6.3f", ballGrabber.m_desiredElevatorVoltage); lcd->PrintfLine(DriverStationLCD::kUser_Line6, "CEV=%5.2f", ballGrabber.elevatorAngleSensor.GetVoltage()); if(button6){ m_display_page_1 = true; } } lcd->UpdateLCD(); printDelay = 0; } //int rotation = elevation.Get(); //the above is commented because we are not using it yet bool shooterButton = gamePad.GetRawButton(7) || gamePad.GetRawButton(8);//TODO make constants bool automaticAimButton = gamePad.GetRawButton(1); //float distanceToWall = frontUltrasonic.GetAverageDistance(); //bool loadShooterButton = gamePad.GetRawButton(8); if (shooterButton && shootDelay == 0){ shootDelay++; } if(shootDelay>0){ shootDelay++; } bool ReadyToShoot = (shootDelay>PHOENIX2014_LOOP_COUNT_FOR_SHOOT_DELAY); shooter.OperateShooter(ReadyToShoot); if (ReadyToShoot){ shootDelay = 0; } bool okToGrab = (shootDelay == 0);//Normaly 0 unless delaying ballGrabber.OperateGrabber(shooterButton, okToGrab); //Trying to make some things happen automatically during teleoperated if(automaticAimButton){ ballGrabber.m_desiredElevatorVoltage = PHOENIX2014_TELEOP_ELEVATOR_ANGLE; } //((distanceToWall > (12.0*11.0)) && distanceToWall < (12.0*13.0)){ //lightBulb.Set(Relay::kOn); //} //else{ #ifdef WANTWEIRDPULSING if (printDelay < 30) { lightBulb.Set(Relay::kForward); } else if (printDelay >= 30 && printDelay < 65) { lightBulb.Set(Relay::kReverse); } else { lightBulb.Set(Relay::kOff); } #endif Wait(0.005);// wait for a motor update time } // end of while enabled driveTrain.StopMotor(); ballGrabber.StopPidLoop(); shooter.motorShutOff(); /* if(SavePreferencesToFlash){ dashboardPreferences->Save(); SavePreferencesToFlash = false; } */ } // end of OperatorControl()
/** * Drive left & right motors for 2 seconds then stop */ void Autonomous() { init(); lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Entered Autonomous"); driveTrain.SetSafetyEnabled(false); bool checkBox1 = SmartDashboard::GetBoolean("Checkbox 1"); m_FromAutonomous = true; //float rightDriveSpeed = -1.0; //float leftDriveSpeed = -1.0; //int rangeToWallClose = 60; //int rangeToWallFar = 120; //Initialize encoder. //int distanceToShoot = 133; //int shootDelay = 0; //ballGrabber.elevatorController.SetSetpoint(PHOENIX2014_INITIAL_AUTONOMOUS_ELEVATOR_ANGLE); //TODO Remove encoders from code?? driveDistanceRight.Reset(); driveDistanceLeft.Reset(); driveDistanceRight.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_RIGHT); driveDistanceLeft.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_LEFT); driveDistanceRight.Start(); driveDistanceLeft.Start(); //int printDelay = 0; float minDistance = 52.0; // Closer to the wall than this is too close float maxDistance = 12.0*11.0; // Once at least this close, within shooting range //float closeDistance = maxDistance + 24.0; float autoDriveSpeed = 0.55; //float autoDriveSlowSpeed = 0.38; int time = 0; int maxDriveLoop = 4*200; // 4 seconds @200 times/sec bool shootingBall = false; bool wantFirstShot = true; if(checkBox1 == false){ int printDelay = 0; //Ultrasonic Autonomous //bool isInRange = false; while(IsAutonomous() && IsEnabled()) { float currentDistance = frontUltrasonic.GetAverageDistance(); // Transition isInRange from false to true once if((minDistance < currentDistance) && (currentDistance < maxDistance)){ //isInRange = true; } time++; bool motorDriveTimeExceeded = time > maxDriveLoop; bool motorMinMet = time > m_MinDriveLoop; if(/*isInRange ||*/ motorMinMet){ driveTrain.TankDrive(0.0,0.0); if((ballGrabber.elevatorAngleSensor.GetVoltage() > PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE - 0.1) && (ballGrabber.elevatorAngleSensor.GetVoltage() < PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE + 0.1)){ //Enough to cover break release and start winding again. shootingBall = shooter.OperateShooter(wantFirstShot); } if(shootingBall == true){ wantFirstShot = false; } } else if(motorDriveTimeExceeded){ driveTrain.TankDrive(0.0,0.0); } else{ //if((currentDistance < closeDistance) && motorMinMet){ // autoDriveSpeed = autoDriveSlowSpeed; //} driveTrain.TankDrive(-0.97 * autoDriveSpeed, -1.0 * autoDriveSpeed);//the DriveTrain is BACKWARD } ballGrabber.RunElevatorAutonomous(PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE); printDelay = printDelay++; if(printDelay >= 200){ lcd->Clear(); lcd->PrintfLine(DriverStationLCD::kUser_Line1, "In Autonomous"); shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line2, lcd); shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd); lcd->PrintfLine(DriverStationLCD::kUser_Line4, "Ulra=%f", frontUltrasonic.GetAverageDistance()); //lcd->PrintfLine(DriverStationLCD::kUser_Line5, "CEV=%f", ballGrabber.elevatorAngleSensor.GetVoltage()); //lcd->PrintfLine(DriverStationLCD::kUser_Line6, "DEV=%f", ballGrabber.m_desiredElevatorVoltage); lcd->UpdateLCD(); printDelay = 0; } Wait(0.005); } lcd->Clear(); lcd->UpdateLCD(); lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Exiting Autonomous"); } else { //Timer Autonomous driveTrain.TankDrive(-0.5,-0.5); ballGrabber.DriveElevatorTestMode(-1.0); lcd->Clear(); lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Skip Auto"); lcd->PrintfLine(DriverStationLCD::kUser_Line2, "CheckBox Checked"); lcd->UpdateLCD(); Wait(2.0); bool shooting = true; driveTrain.TankDrive(0.0,0.0); int counter = 0; while(counter < 600){ shooter.OperateShooter(shooting); Wait(0.005); } } }
/****************************************************** * Drive left & right motors for 2 seconds then stop * ******************************************************/ void Autonomous(void) { //increase 2nd RPM compressor.Start(); // starts the compressor; bool BRIDGE = bridge.Get(); bool HIGH = high.Get(); bool MIDDLE = middle.Get(); bool TWOSEC = twosec.Get(); bool FIVESEC = fivesec.Get(); int highRPM = 1800; // 1st 1800 short about 5 ft int secondhighRPM = 1800; //1st 1400 (did not fire) int rpmForShooter; DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory char debugout [100]; // baddog.SetExpiration(30.0); // baddog.Feed(); dslcd->Clear(); sprintf(debugout,"Bridge=%u",BRIDGE); dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout); sprintf(debugout,"High=%u",HIGH); dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout); sprintf(debugout,"Middle=%u",MIDDLE); dslcd->Printf(DriverStationLCD::kUser_Line3,3,debugout); sprintf(debugout,"TwoSec=%u",TWOSEC); dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout); sprintf(debugout,"FiveSec=%u",FIVESEC); dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout); dslcd->UpdateLCD(); // update the Driver Station with the information in the code */ if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) { myRobot.Drive(0.0, 0.0); Wait(10.0); } if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //1700 RPM { /*myRobot.Drive(-0.5, 0.0); Wait(3.0); */ flashring.Set(Relay::kForward); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<7) { shooter.setTargetRPM(1700); //wait-0.01 Wait(0.005); } lynx.Set(-1.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<1) { shooter.setTargetRPM(1700); Wait(0.005); } turret.Set(-0.05); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<0.2) { shooter.setTargetRPM(1700); Wait(0.005); } turret.Set(0.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(1700); Wait(0.005); } // lynx.Set(0.0); shepard.Set(true); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(1700); Wait(0.005); } shepard.Set(false); shooter.setTargetRPM((int)0); flashring.Set(Relay::kOff); lynx.Set(0.0); } if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //main autonomous code-default { flashring.Set(Relay::kForward); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<7) { shooter.setTargetRPM((int)highRPM); //wait-0.01 Wait(0.005); } lynx.Set(-1.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<1) { shooter.setTargetRPM((int)highRPM); Wait(0.005); } turret.Set(-0.05); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<0.2) { shooter.setTargetRPM((int)secondhighRPM); Wait(0.005); } turret.Set(0.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM((int)secondhighRPM); Wait(0.005); } // lynx.Set(0.0); shepard.Set(true); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM((int)secondhighRPM); Wait(0.005); } shepard.Set(false); shooter.setTargetRPM((int)0); flashring.Set(Relay::kOff); lynx.Set(0.0); // baddog.Feed(); } if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0) { //Wait(2.0); //Robot aims //Robot shoots //myRobot.Drive(-0.5, 0.0); //Wait(3.0); flashring.Set(Relay::kForward); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<7) { shooter.setTargetRPM(1900); //wait-0.01 Wait(0.005); } lynx.Set(-1.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<1) { shooter.setTargetRPM(1900); Wait(0.005); } turret.Set(-0.05); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<0.2) { shooter.setTargetRPM(1800); Wait(0.005); } turret.Set(0.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(1800); Wait(0.005); } // lynx.Set(0.0); shepard.Set(true); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(1800); Wait(0.005); } shepard.Set(false); shooter.setTargetRPM((int)0); flashring.Set(Relay::kOff); lynx.Set(0.0); } if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1) { Wait(5.0); //Robot aims //Robot shoots //myRobot.Drive(-0.5, 0.0); myRobot.Drive(0.0,0.0); Wait(3.0); } if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0) { //Robot aims //Robot shoots myRobot.Drive(-0.5, 0.0); Wait(3.0); } if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0) { Wait(2.0); //Robot aims //Robot shoots myRobot.Drive(-0.5, 0.0); Wait(3.0); } if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5) { Wait(5.0); //Robot aims //Robot shoots myRobot.Drive(-0.5, 0.0); Wait(3.0); } if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //position robot-front of key-low RPMs { //Robot aims //Robot shoots //myRobot.Drive(-0.5, 0.0); //Wait(3.0); flashring.Set(Relay::kForward); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<7) { shooter.setTargetRPM(1600); //wait-0.01 Wait(0.005); } lynx.Set(-1.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<1) { shooter.setTargetRPM(1600); Wait(0.005); } turret.Set(-0.05); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<0.2) { shooter.setTargetRPM(1600); Wait(0.005); } turret.Set(0.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(1600); Wait(0.005); } // lynx.Set(0.0); shepard.Set(true); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(1600); Wait(0.005); } shepard.Set(false); shooter.setTargetRPM((int)0); flashring.Set(Relay::kOff); lynx.Set(0.0); } if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0) { Wait(2.0); //Robot aims //Robot shoots myRobot.Drive(-0.5, 0.0); Wait(3.0); } if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1) { Wait(5.0); //Robot aims //Robot shoots myRobot.Drive(-0.5, 0.0); Wait(3.0); } if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0) { //Robot aims //Robot shoots myRobot.Drive(-0.5, 0.0); Wait(3.0); } if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0) { Wait(2.0); //Robot aims //Robot shoots myRobot.Drive(-0.5, 0.0); Wait(3.0); } if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5) { Wait(5.0); //Robot aims //Robot shoots myRobot.Drive(-0.5, 0.0); Wait(3.0); } if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 1) { flashring.Set(Relay::kForward); ShooterTimer.Reset(); ShooterTimer.Start(); targeting.SetLMHTarget(BOGEY_H); while(ShooterTimer.Get()<2) { if (targeting.ProcessOneImage()) { targeting.ChooseBogey(); targeting.MoveTurret(); rpmForShooter = targeting.GetCalculatedRPM(); shooter.setTargetRPM(rpmForShooter); Wait(0.01); } } targeting.StopPID(); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get() < 7) { shooter.setTargetRPM(rpmForShooter); Wait(0.005); } lynx.Set(-1.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<1) { shooter.setTargetRPM(rpmForShooter); Wait(0.005); } turret.Set(-0.05); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<0.2) { shooter.setTargetRPM(rpmForShooter); Wait(0.005); } turret.Set(0.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(rpmForShooter); Wait(0.005); } // lynx.Set(0.0); shepard.Set(true); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(rpmForShooter); Wait(0.005); } shepard.Set(false); shooter.setTargetRPM((int)0); flashring.Set(Relay::kOff); lynx.Set(0.0); } if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0) { flashring.Set(Relay::kForward); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<7) { shooter.setTargetRPM(2000); //high RPM //wait-0.01 Wait(0.005); } lynx.Set(-1.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<1) { shooter.setTargetRPM(2000); //high RPM Wait(0.005); } turret.Set(-0.05); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<0.2) { shooter.setTargetRPM(1800); //low RPM Wait(0.005); } turret.Set(0.0); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(1800); //low RPM Wait(0.005); } // lynx.Set(0.0); shepard.Set(true); ShooterTimer.Reset(); ShooterTimer.Start(); while(ShooterTimer.Get()<2.0) { shooter.setTargetRPM(1800); //low RPM Wait(0.005); } shepard.Set(false); shooter.setTargetRPM((int)0); flashring.Set(Relay::kOff); lynx.Set(0.0); } //baddog.Feed(); myRobot.SetSafetyEnabled(false); }
void OperatorControl(void) { float counter; timer.Start(); float percent; deadband = .05; float pi = 3.141592653589793238462643; float bpotval, fpotval; float min = 600, max; float fchgval = .5; float bchgval = .5; while (IsOperatorControl()) { // comp.checkCompressor(); ShootModeSet(); Shoot(true); fpotval = fpot.GetValue(); bpotval = bpot.GetValue(); counter = timer.Get(); if (controller.GetRawButton(3)) { frot.SetSpeed(0); brot.SetSpeed(0); flmot.SetSpeed(0); frmot.SetSpeed(0); blmot.SetSpeed(0); brmot.SetSpeed(0); } else if (controller.GetRawButton(BTN_L1) == false) { if (controller.GetRawButton(7)) { if (fpotval < 860) frot.SetSpeed(-0.5); if (bpotval < 725) brot.SetSpeed(-0.5); if (fpotval > 860 and bpotval > 725) { frot.Set(0); brot.Set(0); flmot.Set(0.5); frmot.Set(0.5); blmot.Set(0.5); brmot.Set(0.5); } } else if (controller.GetRawButton(8)) { if (fpotval < 860) frot.SetSpeed(-0.5); if (bpotval < 725) brot.SetSpeed(-0.5); if (fpotval > 860 and bpotval > 725) { frot.Set(0); brot.Set(0); flmot.Set(-0.5); frmot.Set(-0.5); blmot.Set(-0.5); brmot.Set(-0.5); } } else { if (controller.GetRawAxis(4) <= 0) percent = ((acos(controller.GetRawAxis(3)) / pi)); else if (controller.GetRawAxis(4) > 0) percent = ((acos(-controller.GetRawAxis(3)) / pi)); fpotval = percent * 550 + 330; percent = (fpotval - 330) / 550; bpotval = percent * 500 + 245; if (fpot.GetValue() < fpotval) fchgval = -.5; else if (fpot.GetValue() > fpotval) fchgval = .5; if (fpot.GetValue() < fpotval + 10 && fpot.GetValue() > fpotval - 10) fchgval = 0; if (bpot.GetValue() > bpotval) bchgval = -.5; else if (bpot.GetValue() < bpotval) bchgval = .5; if (bpot.GetValue() < bpotval + 20 && bpot.GetValue() > bpotval - 20) bchgval = 0; frot.Set(fchgval); brot.Set(bchgval); if (pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2) > deadband && controller.GetRawButton(BTN_R1)) { if (controller.GetRawAxis(4) > 0) { flmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2))); frmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2))); blmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2))); brmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2))); } else if (controller.GetRawAxis(4) < 0) { flmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2))); frmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2))); blmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2))); brmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2))); } } else { flmot.Set(0); frmot.Set(0); blmot.Set(0); brmot.Set(0); } contaxis4 = 0; } } else { if (contaxis4 == 0) contaxis4 = controller.GetRawAxis(4); if (controller.GetRawButton(BTN_R1)) { if (contaxis4 > 0) { flmot.Set(-0.25); frmot.Set(0.25); blmot.Set(-0.25); brmot.Set(0.25); } else if (contaxis4 < 0) { flmot.Set(0.25); frmot.Set(-0.25); blmot.Set(0.25); brmot.Set(-0.25); } } } if (float (fpot.GetValue()) < min) min = float (fpot.GetValue()); else if (float (fpot.GetValue()) > max) max = float (fpot.GetValue()); if (counter >= .1) { lcda->Clear(); lcda->Printf(DriverStationLCD::kUser_Line3, 1, "FPot :: %d", fpotval); lcda->Printf(DriverStationLCD::kUser_Line4, 1, "BPot :: %d", bpotval); lcda->UpdateLCD(); timer.Reset(); } } }
void OperatorControl() { timer.Start(); while (IsOperatorControl()) { x = Stick.GetRawAxis(1); y = -Stick.GetRawAxis(2); if (fabs(x) < 0.1) { x = 0; } if (fabs(y) < 0.1) { y = 0; } x2 = x*x; y2 = y*y; magnitude = pow((x2+y2),0.5); angle = atan2(y,x)*180/PI; magnitude = max(magnitude, -1); magnitude = min(magnitude, 1); if (angle < 0) { angle += 180; magnitude *= -1; } /*front = 730 - (angle * 2.7888); if ((front + 5) < float(fpot.GetValue())) { fturn.Set(0.5); } else if ((front - 5) > float(fpot.GetValue())) { fturn.Set(-0.5); } else { fturn.Set(0); }*/ back = 235 + (angle * 2.8611); if ((back + 10) < float(bpot.GetValue())) { bturn.Set(0.5); } else if ((back - 10) > float(bpot.GetValue())) { bturn.Set(-0.5); } else { bturn.Set(0); } if (timer.Get() > 0.1) { lcd->Clear(); lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Front = %5.4f", front); lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Back = %5.4f", back); lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Front Pot = %5.4f", float(fpot.GetValue())); lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Back Pot = %5.4f", float(bpot.GetValue())); lcd->Printf(DriverStationLCD::kUser_Line5, 1, "Angle = %5.4f", angle); lcd->Printf(DriverStationLCD::kUser_Line6, 1, "Magnitude = %5.4f", magnitude); lcd->UpdateLCD(); timer.Reset(); timer.Start(); } } }