void TestRamLock() { if (fabs(m_driver->GetRawAxis(TRIGGERS)) < .2) m_ramServo->SetAngle(0); else if (fabs(m_driver->GetRawAxis(TRIGGERS)) > .2) m_ramServo->SetAngle(90); }
void updateShooter(){ SmartDashboard::PutBoolean("stateDisarmed", stateDisarmed); SmartDashboard::PutBoolean("stateArming1", stateArming1); SmartDashboard::PutBoolean("stateArming2", stateArming2); SmartDashboard::PutBoolean("stateArmed", stateArmed); SmartDashboard::PutBoolean("stateFiring1", stateFiring1); float winchLocked = SmartDashboard::GetNumber("winchLocked",-4500.00); // float winchUnlocked = SmartDashboard::GetNumber("winchUnlocked",-3000.00); float winchMidway = SmartDashboard::GetNumber("winchMidway",-0.00); // float winchServoTrigger = SmartDashboard::GetNumber("winchServoTrigger",-0.00); // float winchOverUnlocked = SmartDashboard::GetNumber("winchOverUnlocked",0.00); float servoLocked = SmartDashboard::GetNumber("servoLocked",5.00); float servoUnlocked = SmartDashboard::GetNumber("servoUnlocked",175.00); float motorStopped = 0.00; float motorWinding = SmartDashboard::GetNumber("motorWinding",-.70); float motorUnwinding = SmartDashboard::GetNumber("motorUnwinding",.50); // float motorFastUnwinding = SmartDashboard::GetNumber("motorFastUnwinding",.80); if (stateDisarmed == true){ // t_motor.Set(0); } if (stick2.GetRawButton(buttonA) == true and stateDisarmed == true){ myServo->SetAngle(servoLocked); stateDisarmed = false; stateArming1 = true; } if (stateArming1 == true){ t_motor.Set(motorWinding); myServo->SetAngle(servoLocked); if (t_motor.GetEncPosition() < winchLocked){ t_motor.Set(motorStopped); stateArming1 = false; stateArming2 = true; } } if (stateArming2 == true){ t_motor.Set(motorUnwinding); if (t_motor.GetEncPosition() >= winchMidway){ t_motor.Set(motorStopped); stateArming2 = false; stateArmed = true; } } if (stick.GetRawButton(1) == true and stateArmed == true){ stateArmed = false; stateFiring1 = true; } if (stateFiring1 == true){ t_motor.Set(0); myServo->SetAngle(servoUnlocked); stateFiring1 = false; stateDisarmed = true; } }
/** * Gets calibrated parameters from previously calibrated gyro, allocates a new * gyro with the given parameters for center and offset, and re-runs tests on * the new gyro. */ void TiltPanCameraTest::GyroCalibratedParameters() { uint32_t cCenter = m_gyro->GetCenter(); float cOffset = m_gyro->GetOffset(); delete m_gyro; m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel, cCenter, cOffset); m_gyro->SetSensitivity(kSensitivity); // Default gyro angle test // Accumulator needs a small amount of time to reset before being tested m_gyro->Reset(); Wait(.001); EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f); // Gyro angle test // Make sure that the gyro doesn't get jerked when the servo goes to zero. m_pan->SetAngle(0.0); Wait(0.5); m_gyro->Reset(); for (int i = 0; i < 600; i++) { m_pan->Set(i / 1000.0); Wait(0.001); } double gyroAngle = m_gyro->GetAngle(); EXPECT_NEAR(gyroAngle, kTestAngle, 10.0) << "Gyro measured " << gyroAngle << " degrees, servo should have turned " << kTestAngle << " degrees"; }
// WHen robot is enabled void init() { log->info("initializing"); log->print(); climber = NULL; leftDriveEncoder->Reset(); leftDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse); leftDriveEncoder->Start(); rightDriveEncoder->Reset(); rightDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse); rightDriveEncoder->Start(); //leftClimber->motorController->Disable(); leftClimber->encoder->Reset(); leftClimber->encoder->Start(); //rightClimber->motorController->Disable(); rightClimber->encoder->Reset(); rightClimber->encoder->Start(); bcdValue = bcd->value(); loadSwitchOldState = loaderSwitch->Get(); #if 0 bool leftDone = false; bool rightDone = false; // Only do this for some BCD values? while (!leftDone || !rightDone){ if (!leftDone) leftDone = leftClimber->UpdateState(-1.0, -30); if (!rightDone) rightDone = rightClimber->UpdateState(-1.0, -30); log->info("Wait: Ll Rl: %d %d", leftClimber->lowerLimitSwitch->Get(), rightClimber->lowerLimitSwitch->Get()); log->print(); } #endif climbState = NotInitialized; cameraElevateAngle = (cameraElevateMotor->GetMaxAngle()-cameraElevateMotor->GetMinAngle()) * 2/3; cameraPivotAngle = 0; cameraPivotMotor->SetAngle(cameraPivotAngle); cameraElevateMotor->SetAngle(cameraElevateAngle); loading = false; loaderDisengageDetected = false; //This is a rough guess of motor power it should be based on voltage shooterMotorVolts = 8.0; // volts as a fraction of 12V loadCount = 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 shootingModes(){ if (manShootMode == 0){ updateShooter(); } if (manShootMode == 1){ if (stick2.GetRawAxis(triggerR) > 0){ t_motor.Set(scaler(-1*(stick2.GetRawAxis(triggerR)))); } else if (stick2.GetRawAxis(triggerL) > 0){ t_motor.Set(scaler(stick2.GetRawAxis(triggerL))); } else if (stick2.GetRawAxis(triggerL) == 0 and stick2.GetRawAxis(triggerR) == 0){ t_motor.Set(0); } if (stick2.GetRawButton(buttonX) == true){ myServo->SetAngle(175); } if (stick2.GetRawButton(buttonX) == false){ myServo->SetAngle(5); } } }
/** * Test if the servo turns 90 degrees and the gyroscope measures this angle * Note servo on TestBench is not the same type of servo that servo class * was designed for so setAngle is significantly off. This has been calibrated * for the servo on the rig. */ void TiltPanCameraTest::GyroAngle() { // Make sure that the gyro doesn't get jerked when the servo goes to zero. m_pan->SetAngle(0.0); Wait(0.5); m_gyro->Reset(); for (int i = 0; i < 600; i++) { m_pan->Set(i / 1000.0); Wait(0.001); } double gyroAngle = m_gyro->GetAngle(); EXPECT_NEAR(gyroAngle, kTestAngle, 10.0) << "Gyro measured " << gyroAngle << " degrees, servo should have turned " << kTestAngle << " degrees"; }
bool servoSetPos(bool s){ if (s == true){ myServo->SetAngle(SmartDashboard::GetNumber("servoMin",5)); myServo2->SetAngle(SmartDashboard::GetNumber("servoMin",5)); myServo3->SetAngle(SmartDashboard::GetNumber("servoMin",5)); // myServo4->SetAngle(SmartDashboard::GetNumber("servoMin",5)); } if (s == false){ myServo->SetAngle(SmartDashboard::GetNumber("servoMax",165)); myServo2->SetAngle(SmartDashboard::GetNumber("servoMax",165)); myServo3->SetAngle(SmartDashboard::GetNumber("servoMax",165)); // myServo4->SetAngle(SmartDashboard::GetNumber("servoMax",165)); } return true; }
/* * OPERATOR CONTROL using a arcade style drive */ void OperatorControl(void) { GetWatchdog().SetEnabled(true); while (IsOperatorControl()) //This code will loop continuously as long it is operator control mode { GetWatchdog().Feed(); // Feed the watchdog shoulderPotentiometerReading = (5-(shoulderPotentiometerChannel->GetVoltage())); // reads the potentiometer at channel 1 /* COMMENT GRIPPER CODE BELOW if (shoulderPotentiometerReading == 0) { gripperMotor->SetAngle(0); //opens gripper } else if (shoulderPotentiometerReading > 3) { gripperMotor->SetAngle(55); //closes gripper } */ if (leftstick->GetRawButton(4) == true) { shoulderMotor->Drive(-kMaxShoulderMotorSpeed, 0); //moves shoulderMotor down shoulderDestinationVoltage = shoulderPotentiometerReading; } else if (leftstick->GetRawButton(5) == true) { shoulderMotor->Drive(kMaxShoulderMotorSpeed, 0); //moves shoulderMotor up shoulderDestinationVoltage = shoulderPotentiometerReading; } else { //LEFTSTICK PRESETS if (leftstick->GetRawButton(2) == true) { //shoulderDestinationVoltage = kPickUpPosition; } else if (leftstick->GetRawButton(6) == true) { shoulderDestinationVoltage = kTopRow; } else if (leftstick->GetRawButton(7) == true) { shoulderDestinationVoltage = kMiddleRow; } else if (leftstick->GetRawButton(8) == true) { shoulderDestinationVoltage = kBottomRow; } else if (leftstick->GetRawButton(9) == true) { shoulderDestinationVoltage = kBottomRowSecondColumn; } else if (leftstick->GetRawButton(10) == true) { shoulderDestinationVoltage = kMiddleRowSecondColumn; } else if (leftstick->GetRawButton(11) == true) { shoulderDestinationVoltage = kTopRowSecondColumn; } MoveShoulderTo(shoulderDestinationVoltage); } //---------------------------------------------------------------- //GRIPPER OPEN AND CLOSE if (leftstick->GetRawButton(1) == true) { gripperMotor->SetAngle(0); //opens gripper } else { gripperMotor->SetAngle(55); //closes gripper } //MINIBOT DEPLOYMENT if (leftstick->GetRawButton(3) == true) { minibotDeployMotor->SetAngle(60); //releases four-bar myRobot->Drive(0.0, 0); // stop robot since controls are going to be inverted momentarily - don't want to go from full forward to full reverse instantly } //RIGHTSTICK PRESETS if (rightstick->GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick myRobot->ArcadeDrive((rightstick->GetY()), (rightstick->GetX()), false); // inverted drive control } else { //front of the robot is moved forward by pushing forward on the joystick myRobot->ArcadeDrive(-(rightstick->GetY()), (rightstick->GetX()), false); // normal drive control } //MINIBOT CLOSING/CLIMBING POLE if (rightstick->GetRawButton(3) == true) { //minibot closes only after its four-bar is dropped minibotCloseMotor1->SetAngle(45); //closes minibot so it starts climbing pole minibotCloseMotor2->SetAngle(45); //closes minibot so it starts climbing pole } else if (rightstick->GetRawButton(4) == true) { //MANUAL ARM CONTROL armMotor->Drive(-kMaxArmMotorSpeed/2.0, 0); //turn armMotor counter clockwise } else if (rightstick->GetRawButton(5) == true) { //MANUAL ARM CONTROL armMotor->Drive(kMaxArmMotorSpeed/1.5, 0); //turn armMotor cw } } }
void TeleopPeriodic() { displayCount++; //tbs change button number if ((control->gamepadButton(9) && control->gamepadButton(10)) || // start climbState != NotInitialized) { // continue // Do we want a manual abort here? ClimbPeriodic(); return; } // drive drive->setLeft(control->left()); // control->setRightScale(.95); drive->setRight(control->right()); drive->setScale(control->throttle()); //drive->setLowShift(control->button(1)); // right trigger drive->setReversed(control->toggleButton(11)); // right JS button 11 //turn light on or off //lightRing->Set(control->gamepadToggleButton(4) ? Relay::kForward : Relay::kOff ); blowerMotor->Set(control->gamepadButton(6) ? 1.0 : 0.0 ); // For the loader, if we are rotating, wait for at least 100 counts before // checking the switch or adjusting motor power if (loading) { //if (loaderSwitch->Get()) { //loaderDisengageDetected = true; //} loadSwitchDelay++; // If already rotating, and the switch trips, power down the motor if (/*loaderDisengageDetected*/ loaderSwitch->Get() != loadSwitchOldState) { loaderMotor->Set(Relay::kOff); loading = false; loadSwitchOldState = loaderSwitch->Get(); } } else { // If not rotating and the gamepad button is set, start rotating if (control->gamepadButton(7)) { loadSwitchDelay = 0; loadCount++; loaderMotor->Set(Relay::kForward); loading = true; loaderDisengageDetected = false; } } if (control->gamepadToggleButton(4)){ if (control->gamepadRightVertical() > 0.05 || control->gamepadRightVertical() < -0.05) { shooterMotorVolts += control->gamepadRightVertical(); if (shooterMotorVolts < 6.0) shooterMotorVolts = 6.0; if (shooterMotorVolts > 12.0) shooterMotorVolts = 12.0; } } // For the shooter, spin it up or down based on the toggle // if (control->gamepadToggleButton(8)) { shooterMotor->Set(-shooterMotorVolts);// negative because motor is wired backwerds log->info("shooter on"); } else { shooterMotor->Set(0.0); log->info("shooter off"); } if (control->gamepadLeftVertical() > 0.05 || control->gamepadLeftVertical() < -0.05) { cameraElevateAngle += control->gamepadLeftVertical()*5; if (cameraElevateAngle < cameraElevateMotor->GetMinAngle()) cameraElevateAngle = cameraElevateMotor->GetMinAngle(); if (cameraElevateAngle > cameraElevateMotor->GetMaxAngle()) cameraElevateAngle = cameraElevateMotor->GetMaxAngle(); } if (control->gamepadLeftHorizontal() > 0.05 || control->gamepadLeftHorizontal() < -0.05) { cameraPivotAngle += control->gamepadLeftHorizontal()*5; if (cameraPivotAngle < cameraPivotMotor->GetMinAngle()) cameraPivotAngle = cameraPivotMotor->GetMinAngle(); if (cameraPivotAngle > cameraPivotMotor->GetMaxAngle()) cameraPivotAngle = cameraPivotMotor->GetMaxAngle(); } cameraPivotMotor->SetAngle(cameraPivotAngle); cameraElevateMotor->SetAngle(cameraElevateAngle); if (control->gamepadToggleButton(1)) { // If the climber lower limit switch is set, and the distance is >0, set it to 0 if (!leftClimber->lowerLimitSwitch->Get()) { leftClimber->encoder->Reset(); leftClimber->encoder->Start(); // Only allow forward motion if (control->gamepadRightVertical() > 0) { leftClimber->motor->Set(control->gamepadRightVertical()); } else { leftClimber->motor->Set(0.0); } } else { leftClimber->motor->Set(control->gamepadRightVertical()); } } if (control->gamepadToggleButton(3)) { // If the climber lower limit switch is set, and the distance is >0, set it to 0 if (!rightClimber->lowerLimitSwitch->Get()) { rightClimber->encoder->Reset(); rightClimber->encoder->Start(); // Only allow forward motion if (control->gamepadRightVertical() > 0) { rightClimber->motor->Set(control->gamepadRightVertical()); } else { rightClimber->motor->Set(0.0); } } else { rightClimber->motor->Set(control->gamepadRightVertical()); } } // assorted debug //log->info("Shift %s", control->toggleButton(8) // ? "low" : "high"); //log->info("ArmPot: %.0f", arm->encoderValue()); //log->info("pidf: %.2f", arm->pidFactor()); //log->info("piden: %s", arm->isPidEnabled() ? "true" : "false"); //double myTest = drive->rightPosition(); //log->info("renc: %f", myTest); //myTest = drive->leftPosition(); //log->info("lenc: %f", myTest); //log->info("gplh %f %f", control->gamepadLeftHorizontal(), cameraPivotAngle); //log->info("gplv %f %f", control->gamepadLeftVertical(), cameraElevateAngle); //log->info("gprh %f", control->gamepadRightHorizontal()); //log->info("gprv %f", control->gamepadRightVertical()); if (display()) { //log->info("lg %d ldd %d", loading, loaderDisengageDetected); //log->info("BCD: %d", bcd->value()); log->info("SHV: %f", shooterMotorVolts); //log->info("LdCDS: %d %d %d", loadCount, loaderDisengageDetected, loaderSwitch->Get()); log->info("ena: %c%c%c%c", " L"[control->gamepadToggleButton(1)], " R"[control->gamepadToggleButton(3)], " J"[control->gamepadToggleButton(2)], " S"[control->gamepadToggleButton(4)]); log->info("LRC %f %f", leftClimber->encoder->GetDistance(), rightClimber->encoder->GetDistance()); log->info("LRD %f %f", leftDriveEncoder->GetDistance(), rightDriveEncoder->GetDistance()); log->info("LRL %d %d %d %d %d %d %d", leftClimber->lowerLimitSwitch->Get(), leftClimber->lowerHookSwitch->Get(), leftClimber->upperHookSwitch->Get(), rightClimber->lowerLimitSwitch->Get(), rightClimber->lowerHookSwitch->Get(), rightClimber->upperHookSwitch->Get(), loaderSwitch->Get()); log->print(); } }
void TeleopPeriodic(void) { float x = gamepad->GetLeftX(); float y = gamepad->GetLeftY(); float rot = gamepad->GetRightX(); //small gamepad values are ignored if (x < 0.1f && x > -0.1f) { x = 0; } if (y < 0.1f && y > -0.1f) { y = 0; } if (rot < 0.1f && rot > -0.1f) { rot = 0; } drive->MecanumDrive_Cartesian(SPEED_LIMIT * x, SPEED_LIMIT * y, SPEED_LIMIT * rot); //shoot smoke if button is pressed if (gamepad2->GetNumberedButton(FIRE_SMOKE_BUTTON)){ //SHOOT SMOKE! //makingSmoke = !makingSmoke; smoke_cannon->Set(SMOKE_CANNON_SPEED); lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Shooting"); firing_smoke_timer->Start(); //measure how long we've fired smoke, so we know if it's ok to make more } else { smoke_cannon->Set(0.0f); lcd->PrintfLine(DriverStationLCD::kUser_Line5, "Not shooting"); firing_smoke_timer->Stop(); //stop the timer, since we're not firing smoke. //don't reset, cuz we need to how much smoke we've fired. } //Eye Code // float eye_pos = gamepad2->GetLeftX(); // // right_eye_x->Set((eye_pos * 60) + default_eye_position); // left_eye_x->Set((eye_pos * 60) + default_eye_position - LEFT_EYE_OFFSET); // // //button lock code // if(gamepad2->GetNumberedButtonPressed(EYE_LOCK_BUTTON)){ // default_eye_position = eye_pos; // } // //left eye control //If A isn't pressed the value should stay the same as before if (!gamepad2->GetNumberedButton(1)){ float left_joystick_x = gamepad2->GetLeftX(); float left_eye_x_axis = (1 - left_joystick_x)*60; left_eye_val = left_eye_x_axis + 50; float right_joystick_x = gamepad2->GetRawAxis(4);//right x axis float right_eye_x_axis = (1-right_joystick_x)*60; right_eye_val = right_eye_x_axis+20; } left_eye_x->SetAngle(left_eye_val); right_eye_x->SetAngle(right_eye_val); //float right_joystick_y = gamepad2->GetRawAxis(4); //float right_eye_y_axis = (right_joystick_y+1)*60; //right_eye_y->SetAngle(right_eye_y_axis); /* bool rbutton = gamepad2->GetNumberedButton(HEAD_UP_BUTTON); bool lbutton = gamepad2->Ge tNumberedButton(HEAD_DOWN_BUTTON); if (rbutton){ lcd->PrintfLine(DriverStationLCD::kUser_Line6, "rb pressed"); jaw_motor->Set(0.2f); }else if(lbutton){ lcd->PrintfLine(DriverStationLCD::kUser_Line6, "lb pressed"); jaw_motor->Set(-0.15f); }else{ lcd->PrintfLine(DriverStationLCD::kUser_Line6, "no buttons"); jaw_motor->Set(0.0f); } */ //REAL head & jaw code //move head down if(gamepad2->GetRightX()<=-0.5f && can_move_head_down() && can_move_jaw_down()){ head_motor->Set(-0.3f); jaw_motor->Set(0.3f); } //move head up else if(gamepad2->GetNumberedButton(HEAD_UP_BUTTON) && can_move_head_up()){ head_motor->Set(0.3f); jaw_motor->Set(-0.3f); } //move jaw down else if(gamepad2->GetRightX()>=0.5f && can_move_jaw_down()){ jaw_motor->Set(0.3f); } //move jaw up else if(gamepad2->GetNumberedButton(JAW_UP_BUTTON) && can_move_jaw_up()){ jaw_motor->Set(-0.3f); } //sets to zero if no buttons pressed else { jaw_motor->Set(0.0f); head_motor->Set(0.0f); } lcd->PrintfLine(DriverStationLCD::kUser_Line6, "b:%d t:%d c:%d", bottomjaw_limit->Get(), tophead_limit->Get(), crash_limit->Get()); //Smoke code if (gamepad2->GetNumberedButton(MAKE_SMOKE_BUTTON)){ //MAKE SMOKE!!!!!!!!!!! //only if we don't have too much excess smoke if (making_smoke_timer->Get() - firing_smoke_timer->Get() < MAX_EXCESS_SMOKE_TIME){ lcd->PrintfLine(DriverStationLCD::kUser_Line4, "smoke"); smoke_machine->Set(true); } else { lcd->PrintfLine(DriverStationLCD::kUser_Line4, "too much smoke"); smoke_machine->Set(false); } making_smoke_timer->Start(); //measure how long we've been making smoke, so we don't overflow the machine //doesn't do anything if we've already started the timer } else { lcd->PrintfLine(DriverStationLCD::kUser_Line4, "not smoke"); smoke_machine->Set(false); making_smoke_timer->Stop(); //stop the timer, since we're not making smoke //don't reset it, cuz we need to know how much smoke we've made } //if both timers are the same, we can set them both to zero to ensure we don't overflow them or something if (making_smoke_timer->Get() == firing_smoke_timer->Get()){ making_smoke_timer->Reset(); firing_smoke_timer->Reset(); } lcd->PrintfLine(DriverStationLCD::kUser_Line1, "x:%f", x); lcd->PrintfLine(DriverStationLCD::kUser_Line2, "y:%f", y); lcd->PrintfLine(DriverStationLCD::kUser_Line3, "r:%f", rot); lcd->UpdateLCD(); }
void OperatorControl() { while (IsOperatorControl() && IsEnabled()) { robotDrive.ArcadeDrive(scaler(stick.GetZ()),scaler(stick.GetY())); SmartDashboard::PutNumber("StickZ",stick.GetZ()); SmartDashboard::PutNumber("StickZscaled",scaler(stick.GetZ())); finger_Motor.Set(scaler(stick2.GetRawAxis(thumbpadL_Y))); // scalerValue = stick.GetRawAxis(3); arm_Motor.Set(scaler(stick2.GetRawAxis(thumbpadR_Y))); manualShooter(); shootingModes(); toggleIntake(); toggleIntakeMode(); setScalerValue(); // double volts = ourRangefinder->GetVoltage(); // SmartDashboard::PutNumber("Voltage",volts); //t_motor.Set(stick2.GetZ()); //t_motor.Set(stick.GetAxis(Joystick::kDefaultThrottleAxis)); // t_motor.Set(stick.GetAxis(Joystick::kThrottleAxis)); // finger_Motor.Set(stick2.GetAxis(Joystick::kThrottleAxis)); // arm_Motor.Set(stick2.GetY()); // Current Control mode Debug SmartDashboard::PutNumber("Motor30 Current",t_motor.GetOutputCurrent()); SmartDashboard::PutNumber("Position",t_motor.GetPosition()); // t_motor.Set(stick.GetAxis(Joystick::Slider)); // if (stick.GetRawButton(3) == true){ // t_motor.SetPosition(10000); // } // if (stick2.GetRawButton(7) == true and buttonpress == false){ // // } // else if (stick2.GetRawButton(3) == false and buttonpress == true){ // buttonpress = false;// drive with arcade style (use right stick) // // } SmartDashboard::PutBoolean("buttonpress state",buttonpress); if (stick2.GetRawButton(buttonBack) == true){ stateDisarmed = true; stateArming1 = false; stateArming2 = false; stateArmed = false; stateFiring1 = false; stateFiring2 = false; t_motor.SetPosition(0); } toggleIntake(); // if (stick2.GetRawButton(5) == true){ // intake_Spin_Motor.Set(1); // } // if (stick2.GetRawButton(5) == false){ // intake_Spin_Motor.Set(0); // } if (stick.GetRawButton(2) == true and buttonpress2 == false){ myServo->SetAngle(175); } if (stick.GetRawButton(2) == false and buttonpress2 == true){ myServo->SetAngle(5); } // servoSetPos(servoState); // myServo->Set(.5); Wait(0.005);// wait for a motor update time // motorSetPos(launcherState); } }
/** * Runs the motors with arcade steering. */ void OperatorControl(void) { myRobot->SetSafetyEnabled(true); while (IsOperatorControl()) { bool setLimit; double cimValue1= -scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1, also invert the cim, since we want it to rotate coutnerclockwise/clockwise double cimValue2= scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1 //For shooter /*if (stick.GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick myRobot.ArcadeDrive((stick.GetY()), (stick.GetX()), false); // inverted drive control } else { //front of the robot is moved forward by pushing forward on the joystick myRobot.ArcadeDrive(-(stick.GetY()), (stick.GetX()), false); // normal drive control } */ //For manual button speed control, this sets the speed if (stick->GetRawButton(4) == true) { cim1->Set(cimValue1); //use the value from the throttle to set cim speed cim2->Set(cimValue2);//Get speed from throttle, and then scale it setLimit = true; //Open Ball Stopper } else { cim1->Set(0.0); cim2->Set(0.0); setLimit = false; //Close Ball Stopper } //For precisebelt pickup if (stick->GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick if (setLimit == true) { JagBelt->ArcadeDrive(0.1, (stick->GetX()), false); // inverted drive control } else { JagBelt->ArcadeDrive((stick->GetY()), (stick->GetX()), false); // inverted drive control } } else { //front of the robot is moved forward by pushing forward on the joystick if (setLimit == true) { JagBelt->ArcadeDrive(-0.1, (stick->GetX()), false); // inverted drive control } else { JagBelt->ArcadeDrive(-(stick->GetY()), (stick->GetX()), false); // inverted drive control } } //For normal belt pickup /*if (stick->GetRawButton(6) == true) { JagBelt->Drive(1.0, 0); //opens gripper } else { JagBelt->Drive(0.0, 0); //closes gripper } */ //For drive if (rightstick->GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick if (rightstick->GetRawButton(10) == true) { precisionMode= true; } else { precisionMode= false; } myRobot->ArcadeDrive((rightstick->GetY()), (rightstick->GetX()), precisionMode); // inverted drive control } else { if (rightstick->GetRawButton(10) == true) { precisionMode= true; } else { precisionMode= false; } //front of the robot is moved forward by pushing forward on the joystick myRobot->ArcadeDrive(-(rightstick->GetY()), (rightstick->GetX()), precisionMode); // normal drive control } /*if (stick->GetRawButton(8) == true) { cim1->Set(-0.37); //run cim 1 at 50% speed counterclockwise?? cim2->Set(0.37); // run cim at 50% speed clockwise check = true; //indicate if motors are running } else if (check == true){ // if motors are running a Wait(2.0); belt->Set(1); // run the belt Wait(2.0); // one sec delay belt->Set(0.0); // turn belt off check = false; // put new check } else if (check == false){ //if false // 2 sec delay to wait for the first ball to shoot cim1->Set(0.0); //stop cims cim2->Set(0.0); } else { // Stop everything cim1->Set(0.0); //stop cims cim2->Set(0.0); belt->Set(0.0); } */ //Code for using window motor if (rightstick->GetRawButton(4)) { window->Set(Relay::kOn); window->Set(Relay::kForward); // tell window motor to go forward } else if (rightstick->GetRawButton(5) == true){ window->Set(Relay::kOn); window->Set(Relay::kReverse); //tell window motor to go backward } else { //Wait(1.0); window->Set(Relay::kOff); //turn it off, if the relays aint being used } //Code for Banebot Motor for stopping ballz if (stick->GetRawButton(2) == true) { // press button TWO to close pickStop->Set(-0.5); //closes ball stopper Wait(1); pickStop->Set(0.0); } else if (stick->GetRawButton(3) == true){ //press button three to open pickStop->Set(0.5); //opens ball stopper Wait(1.2); // too slow, so needs more time pickStop->Set(0.0); } else if (stick->GetRawButton(5)== true){ //press 5 to stop imediately, useful for adjusting angles... //Wait(1.0); pickStop->Set(0.0); } //Code for ... servoooo if (stick->GetRawButton(10) == true) { //press 10 on the left stick... bar->SetAngle(60); // set the angles to 60...clockwise? bar2->SetAngle(60); } else if (stick->GetRawButton(11) == true) // press 11 on the left stick { bar->SetAngle(-60); //set the angles to -60...counterclockwise? bar2->SetAngle(-60); } // Initialize functions... // RelayServo(); //PreciseBelt(); //UltrasonicRange(); // Accelerometer(); //dash->GetPacketNumber(); // send data back to dashboard dash->GetPacketNumber(); //not sure why this is here 0_0 //int limitValue= limitSwitch->Get() ? 1 : 0; // retrieve 1 and 0 only.../ look for 0 and 1 float servoAngle = bar->GetAngle(); //dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Limit State: %d", limitValue); //send data back to driver station... // dsLCD->Printf(DriverStationLCD::kUser_Line2, 2, "Servo Angle: %f", servoAngle); //send data back to driver station... float gyroVal = gyro -> GetVoltage();//Gets voltage from gyro float ultraVal = rangeFinder -> GetVoltage(); //Get voltage from ultrasonic sensor float tempVal = Temperature -> GetVoltage();//Gets temperature //Do the math to convert data received from the ultrasonic volts->miliVolts->milivolts per inch->inches float Vm = (ultraVal*1000); float Ri = (Vm/9.765625); // Print data back to dashboard dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Ultrasonic Range: %f",Ri); dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Gyro: %f", gyroVal); dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Temperature: %f", tempVal); dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Cim1 Speed: %f%%", (cimValue1*100)); //display speed that the mototrs are reunning at different percentages... dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Cim2 Speed: %f%%", (cimValue2*100)); //Update dashboard dsLCD->UpdateLCD(); } }