void SquareInputs(void) { if(stick.GetY() < 0) { if(DoubleSolenoid::kReverse == shifter.Get()) { myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * -4.0), stick.GetX()); } else if(DoubleSolenoid::kForward == shifter.Get()) { myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * -1.0), stick.GetX()); } } else if(stick.GetY() > 0) { if(DoubleSolenoid::kReverse == shifter.Get()) { myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * 4.0), stick.GetX()); } else if(DoubleSolenoid::kForward == shifter.Get()) { myRobot.ArcadeDrive((stick.GetY() * stick.GetY() * 1.0), stick.GetX()); } } }
/** * 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 }
void OperatorControl(void) { NetTest(); return; myRobot.SetSafetyEnabled(true); digEncoder.Start(); const double ppsTOrpm = 60.0/250.0; //Convert from Pos per Second to Rotations per Minute by multiplication // (See the second number on the back of the encoder to replace 250 for different encoders) const float VoltsToIn = 41.0; // Convert from volts to cm by multiplication (volts from ultrasonic). // This value worked for distances between 1' and 10'. while (IsOperatorControl()) { if (stick.GetRawButton(4)) { myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1); } else if (stick.GetRawButton(5)) { myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1); } else { myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0); } myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0); SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm)); SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch); SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage()); Wait(0.1); } digEncoder.Stop(); }
void TeleopPeriodic(void) { myarm->prepareSignal(); // Call the drive routine to drive the robot. if(rightStick->GetRawButton(1)|| leftStick->GetRawButton(1)) drive->MecanumDrive_Cartesian(rightStick->GetX()/2,leftStick->GetY()/2,leftStick->GetX()/2,0.00); else drive->MecanumDrive_Cartesian(rightStick->GetX(),leftStick->GetY(),leftStick->GetX(),0.00); GetStateForArm(); //Wait(.1); if(modeArm==DDCArm::kManualOveride) { myarm->OperateArm(0,0,0,modeArm); //Shoulder movement if(leftStick->GetRawButton(3)) myarm->MoveShoulder(1); else if(leftStick->GetRawButton(2)) myarm->MoveShoulder(-1); else myarm->MoveShoulder(0); //Elbow Movement if(rightStick->GetRawButton(3)) myarm->MoveElbow(1); else if (rightStick->GetRawButton(2)) myarm->MoveElbow(-1); else myarm->MoveElbow(0); //Wrist Movement if(rightStick->GetRawButton(4)) myarm->MoveWrist(-1); else if(rightStick->GetRawButton(5)) myarm->MoveWrist(1); else myarm->MoveWrist(0); } else myarm->OperateArm(0.0,0.0,peg,modeArm); if(leftStick->GetRawButton(4)) myarm->MoveClaw(-1); else if(leftStick->GetRawButton(5)) myarm->MoveClaw(1); else myarm->MoveClaw(0); if(rightStick->GetRawButton(10)) { printf("S: %f \n E: %f \n W: %f \n \n",myarm->GetShoulderVoltage(),myarm->GetElbowVoltage(), myarm->GetWristVoltage()); } deploy->OperateDeployment(fire,pull); // Send Data to the Driver Station for Monitoring (w/in . //sendIOPortData(); //Wait(.1); }
void RobotDemo::arcade_tank_code() { if (drive_stick_prim ->GetRawButton(arcade_button)) { arcadedrive = true; } else { if (drive_stick_prim ->GetRawButton(tank_button)) { arcadedrive = false; } } if (drive_stick_prim->GetRawButton(2) || drive_stick_sec->GetRawButton(2)) { slow_control = true; } else { slow_control = false; } if (arcadedrive) { if (slow_control) { drive->ArcadeDrive(Adjustment_Speed * (drive_stick_prim->GetY()), Adjustment_Speed * (drive_stick_prim->GetX()), true); } else { drive->ArcadeDrive((drive_stick_prim->GetY()), (drive_stick_prim->GetX()), true); } } else { if (slow_control) { drive->TankDrive(Adjustment_Speed * (drive_stick_prim->GetY()), Adjustment_Speed * (drive_stick_sec->GetY()), true); } else { drive->TankDrive((drive_stick_prim->GetY()), (drive_stick_sec->GetY()), true); } } }
void TeleopPeriodic() { // Comment the next line out to disable movement drive->doDrive(stick->GetX(), -stick->GetY()); intake->periodic(); shooter->periodic(); camSystem->periodic(); }
/** * Runs the motors with Mecanum drive. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); while (IsOperatorControl() && IsEnabled()) { bool collisionDetected = false; double curr_world_linear_accel_x = ahrs->GetWorldLinearAccelX(); double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x; last_world_linear_accel_x = curr_world_linear_accel_x; double curr_world_linear_accel_y = ahrs->GetWorldLinearAccelY(); double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y; last_world_linear_accel_y = curr_world_linear_accel_y; if ( ( fabs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) || ( fabs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) { collisionDetected = true; } SmartDashboard::PutBoolean( "CollisionDetected", collisionDetected); try { /* Use the joystick X axis for lateral movement, */ /* Y axis for forward movement, and Z axis for rotation. */ /* Use navX MXP yaw angle to define Field-centric transform */ robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ(),ahrs->GetAngle()); } catch (std::exception ex ) { std::string err_string = "Error communicating with Drive System: "; err_string += ex.what(); DriverStation::ReportError(err_string.c_str()); } Wait(0.005); // wait 5ms to avoid hogging CPU cycles } }
/** * Get the X value of the joystick. * This depends on the mapping of the joystick connected to the current port. * * @param port The USB port for this joystick. */ float GetX(UINT32 port, JoystickHand hand) { Joystick *stick = getJoystick(port); if (stick == NULL) return 0; return stick->GetX((Joystick::JoystickHand) hand); }
/** * @brief Sets a cached joystick value. * @param joy_id Which joystick to set the cached value for. * @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons. */ void Proxy::SetJoystick(int joy_id, Joystick & stick) { wpi_assert(joy_id < NUMBER_OF_JOYSTICKS+1 && joy_id >= 0); char tmp[32]; sprintf(tmp, "Joy%d", joy_id); string name = tmp; if(!disableAxes[joy_id-1]) { set(name + 'X', stick.GetX()); set(name + 'Y', stick.GetY()); set(name + 'Z', stick.GetZ()); set(name + 'R', stick.GetTwist()); set(name + 'T', stick.GetThrottle()); for(int AxisId=1; AxisId<=6; AxisId++) { sprintf(tmp, "%sA%d", name.c_str(), AxisId); set(tmp, stick.GetRawAxis(AxisId)); } } else { if(!stick.GetRawButton(disableAxes[joy_id-1])) { set(name + 'X', stick.GetX()); set(name + 'Y', stick.GetY()); set(name + 'Z', stick.GetZ()); set(name + 'R', stick.GetTwist()); set(name + 'T', stick.GetThrottle()); for(int AxisId=1; AxisId<=6; AxisId++) { sprintf(tmp, "%sA%d", name.c_str(), AxisId); set(tmp, stick.GetRawAxis(AxisId)); } } } if(!disableButtons[joy_id-1]) { for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) { sprintf(tmp, "%sB%d", name.c_str(), i); set(tmp,stick.GetRawButton(i)); } set(name + "BT", stick.GetTrigger()); } else { if(!stick.GetRawButton(disableButtons[joy_id-1])) { for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) { sprintf(tmp, "%sB%d", name.c_str(), i); set(tmp,stick.GetRawButton(i)); } set(name + "BT", stick.GetTrigger()); } } }
void TeleopPeriodic() { SmartDashboard::PutNumber("joystickX",stick.GetX()); SmartDashboard::PutNumber("joystickY",stick.GetY()); //SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1)); //SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage()); SmartDashboard::PutBoolean("infra",infra.Get()); SmartDashboard::PutNumber("accelX",accel.GetX()); SmartDashboard::PutNumber("accelY",accel.GetY()); SmartDashboard::PutNumber("accelZ",accel.GetZ()); servo.Set( trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick ); SmartDashboard::PutNumber("servo", servo.Get()); jag1.Set(stick.GetY()); jag2.Set(stick.GetY()); //tal1.Set(stick.GetY()); SmartDashboard::PutNumber("jag1", jag1.Get()); SmartDashboard::PutNumber("jag2", jag2.Get()); /*SmartDashboard::PutNumber("encpos", enc.Get()); SmartDashboard::PutNumber("encspd", enc.GetRate());*/ if (stick.GetRawButton(1) && !actuatePressed) { pistonVal=!pistonVal; piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse); actuatePressed = true; } else if (!stick.GetRawButton(1)) actuatePressed = false; SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward); }
/** * Drive based upon joystick inputs, and automatically control * motors if the robot begins tipping. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); while (IsOperatorControl() && IsEnabled()) { double xAxisRate = stick.GetX(); double yAxisRate = stick.GetY(); double pitchAngleDegrees = ahrs->GetPitch(); double rollAngleDegrees = ahrs->GetRoll(); if ( !autoBalanceXMode && (fabs(pitchAngleDegrees) >= fabs(kOffBalanceThresholdDegrees))) { autoBalanceXMode = true; } else if ( autoBalanceXMode && (fabs(pitchAngleDegrees) <= fabs(kOnBalanceThresholdDegrees))) { autoBalanceXMode = false; } if ( !autoBalanceYMode && (fabs(pitchAngleDegrees) >= fabs(kOffBalanceThresholdDegrees))) { autoBalanceYMode = true; } else if ( autoBalanceYMode && (fabs(pitchAngleDegrees) <= fabs(kOnBalanceThresholdDegrees))) { autoBalanceYMode = false; } // Control drive system automatically, // driving in reverse direction of pitch/roll angle, // with a magnitude based upon the angle if ( autoBalanceXMode ) { double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0); xAxisRate = sin(pitchAngleRadians) * -1; } if ( autoBalanceYMode ) { double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0); yAxisRate = sin(rollAngleRadians) * -1; } try { // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ()); } catch (std::exception ex ) { std::string err_string = "Drive system error: "; err_string += ex.what(); DriverStation::ReportError(err_string.c_str()); } Wait(0.005); // wait 5ms to avoid hogging CPU cycles } }
// the loop routine runs over and over again forever: void loop() { if(enabled) { S1overflow = 0.0; S2overflow = 0.0; Stick.Update(); driveSpeed = Stick.GetX(); rotSpeed = Stick.GetY(); S1 = driveSpeed + rotSpeed; S2 = driveSpeed + (-rotSpeed); if(S1 > 1) { //calculate S1 overflow S1overflow = S1 - 1.00; S1 = 1.00; } else{ if(S1 < -1.00) { S1overflow = S1 + 1.00; S1 = -1.00; } } if(S2 > 1) { //calculate S2 overflow S2overflow = S2 - 1.00; S2 = 1.00; } else{ if(S2 < -1.00) { S2overflow = S2 + 1.00; S2 = -1.00; } } S1 = S1 + (-S2overflow); S2 = S2 + (-S1overflow); if(reverseS1) { S1 = (-S1); } if(reverseS2) { S2 = (-S2); } talon1s = (S1 + 1.00) * 90; talon2s = (S2 + 1.00) * 90; //Talon1.write(talon1s); // Talon2.write(talon2s); } }
/** * Runs the motors with Mecanum drive. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); 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. robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ()); Wait(0.005); // wait 5ms to avoid hogging CPU cycles } }
/** * @brief Sets a cached joystick value. * @param joy_id Which joystick to set the cached value for. * @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons. */ void Proxy166::SetJoystick(int joy_id, Joystick & stick) { wpi_assert(joy_id < NUMBER_OF_JOYSTICKS && joy_id >= 0); //semTake(JoystickLocks[joy_id], WAIT_FOREVER); Joysticks[joy_id].X = stick.GetX(); Joysticks[joy_id].Y = stick.GetY(); Joysticks[joy_id].Z = stick.GetZ(); Joysticks[joy_id].throttle = stick.GetThrottle(); for(unsigned i=0;i<NUMBER_OF_JOY_BUTTONS;i++) { Joysticks[joy_id].button[i] = stick.GetRawButton(i); } //semGive(JoystickLocks[joy_id]); }
void OperatorControl() { myRobot.SetSafetyEnabled(true); while (IsOperatorControl() && IsEnabled()) { //MECANUM DRIVE float yValue = stick->GetY(); float xValue = stick->GetX(); float rotation = stick->GetTwist(); myRobot.MecanumDrive_Cartesian(xValue, yValue, rotation, 0.0); Wait(0.005); } }
/** * Runs the motors with Mecanum drive. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); while (IsOperatorControl() && IsEnabled()) { bool reset_yaw_button_pressed = stick.GetRawButton(1); if ( reset_yaw_button_pressed ) { ahrs->ZeroYaw(); } bool rotateToAngle = false; if ( stick.GetRawButton(2)) { turnController->SetSetpoint(0.0f); rotateToAngle = true; } else if ( stick.GetRawButton(3)) { turnController->SetSetpoint(90.0f); rotateToAngle = true; } else if ( stick.GetRawButton(4)) { turnController->SetSetpoint(179.9f); rotateToAngle = true; } else if ( stick.GetRawButton(5)) { turnController->SetSetpoint(-90.0f); rotateToAngle = true; } double currentRotationRate; if ( rotateToAngle ) { turnController->Enable(); currentRotationRate = rotateToAngleRate; } else { turnController->Disable(); currentRotationRate = stick.GetTwist(); } try { /* Use the joystick X axis for lateral movement, */ /* Y axis for forward movement, and the current */ /* calculated rotation rate (or joystick Z axis), */ /* depending upon whether "rotate to angle" is active. */ robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), currentRotationRate ,ahrs->GetAngle()); } catch (std::exception ex ) { std::string err_string = "Error communicating with Drive System: "; err_string += ex.what(); DriverStation::ReportError(err_string.c_str()); } Wait(0.005); // wait 5ms to avoid hogging CPU cycles } }
void DriveJoystickCommand::Execute() { Joystick* pXboxController = Robot::instance->pOperatorInterface->pXboxController; if(!WPILibException::isWPIObjectOK(pXboxController)){ printf("Xbox controller not present!\n"); return; } double y = pXboxController->GetY(); // there was a 0.7 turnMax variable in the 2015 bot y = clampJoystickValue(y, -TURN_MAX, TURN_MAX); double x = pXboxController->GetX(); // roadkill safety x = clampJoystickValue(x, -LINEAR_MAX, LINEAR_MAX); SmartDashboard::PutNumber("DriveJoystick_x", x); SmartDashboard::PutNumber("DriveJoystick_y", y); Subsystems::pDriveSubsystem->DriveJoystick(y, x); }
/** * Runs the motors with Mecanum drive. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); while (IsOperatorControl() && IsEnabled()) { bool motionDetected = ahrs->IsMoving(); SmartDashboard::PutBoolean("MotionDetected", motionDetected); try { /* Use the joystick X axis for lateral movement, */ /* Y axis for forward movement, and Z axis for rotation. */ /* Use navX MXP yaw angle to define Field-centric transform */ robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ(),ahrs->GetAngle()); } catch (std::exception ex ) { std::string err_string = "Error communicating with Drive System: "; err_string += ex.what(); DriverStation::ReportError(err_string.c_str()); } Wait(0.005); // wait 5ms to avoid hogging CPU cycles } }
void UpdateStatusDisplays(void) { // Joystick values SmartDashboard::PutNumber("stickX", stick.GetX()); SmartDashboard::PutNumber("stickY", stick.GetY()); SmartDashboard::PutBoolean("shift", stick2.GetState(BUTTON_SHIFT) ? kStateClosed : kStateOpen); // Shooter/Indexer values SmartDashboard::PutBoolean("indexSwitch", indexSwitch.GetState() ? kStateClosed : kStateOpen); SmartDashboard::PutNumber("shooterMotor", shooterMotor.Get()); SmartDashboard::PutNumber("indexerMotor", indexerMotor.Get()); // Misc Motor Values SmartDashboard::PutNumber("collectorMotor", collectorMotor.Get()); SmartDashboard::PutNumber("armMotor", armMotor.Get()); // Arm position via potentiometer voltage (2.5 volts is center position) dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "Voltage: %3.1f", potentiometer.GetVoltage()); SmartDashboard::PutNumber("Potentiometer", potentiometer.GetVoltage()); // Claw lock states SmartDashboard::PutBoolean("Green Claw State", greenClawLockSwitch.GetState()); SmartDashboard::PutBoolean("Yellow Claw State", yellowClawLockSwitch.GetState()); dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "Green : %s", greenClawLockSwitch.GetState() ? "Locked" : "Unlocked"); dsLCD->PrintfLine(DriverStationLCD::kUser_Line5, "Yellow: %s", yellowClawLockSwitch.GetState() ? "Locked" : "Unlocked"); // Pneumatic shifter count SmartDashboard::PutNumber("Shift Count", m_shiftCount); // State viariables dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "CMR: %s SMR: %s JTR: %s", m_collectorMotorRunning ? "T" : "F", m_shooterMotorRunning ? "T" : "F", m_jogTimerRunning ? "T" : "F"); }
void OperatorControl(void) { // Teleoperated Code. /*double WaitDash = 0.0; double FireDash = 0.0; double intPause = 0.0; SmartDashboard::PutNumber("P", 3.0); SmartDashboard::PutNumber("W", 0.0); SmartDashboard::PutNumber("A", 0.0); WaitDash = SmartDashboard::GetNumber("P"); FireDash = SmartDashboard::GetNumber("W"); intPause = SmartDashboard::GetNumber("A"); SmartDashboard::PutNumber("P", WaitDash); SmartDashboard::PutNumber("W", FireDash); SmartDashboard::PutNumber("A", WaitDash); */ // Enable and start the compressor. //compressor->Enabled(); compressor->Start(); // Enable drive motor safety timeout. myRobot.SetSafetyEnabled(true); // Enable watchdog and initial feed. GetWatchdog().SetEnabled(true); GetWatchdog().SetExpiration(1); GetWatchdog().Feed(); // Set robot in low gear by default. Not active. //s[0]->Set(false); GetWatchdog().Feed(); //bool blnShoot = false; bool blnLowHang = false; bool blnShift = false; GetWatchdog().Feed(); bool blnShooterSpd = false; bool blnReverse = false; float fltShoot; float fltSpeed = 1; int intFail = 0; timerLowHang.Reset(); timerShift.Reset(); timerFire.Reset(); timerShooter.Reset(); timerDriveCtrl.Reset(); timerCamera.Reset(); timerReverse.Reset(); timerLowHang.Start(); timerShift.Start(); timerFire.Start(); timerShooter.Start(); timerDriveCtrl.Start(); timerCamera.Start(); timerReverse.Start(); GetWatchdog().Feed(); //sd->sendIOPortData(); // Local variables. //float fltStick1X, fltStick1Y; while (IsOperatorControl()) { if(timerReverse.Get() > 0.5) { if(stick1->GetRawButton(8) && blnReverse == false ) { blnReverse = true; GetWatchdog().Feed(); timerReverse.Reset(); timerReverse.Start(); } else if(stick1->GetRawButton(9) && blnReverse == true) { blnReverse = false; GetWatchdog().Feed(); timerReverse.Reset(); timerReverse.Start(); } } if(blnReverse == false) { fltStick1Y = stick1->GetY(); fltStick1X = stick1->GetX(); SmartDashboard::PutBoolean("Reverse",false); GetWatchdog().Feed(); } else if(blnReverse == true) { fltStick1Y = ((stick1->GetY())*(-1)); fltStick1X = ((stick1->GetX())*(1)); SmartDashboard::PutBoolean("Reverse",true); GetWatchdog().Feed(); } myRobot.ArcadeDrive(fltStick1Y,fltStick1X); //myRobot.ArcadeDrive(stick1); GetWatchdog().Feed(); // Feed hungary demonic Watchdog. SmartDashboard::PutBoolean("Touching Tower?",LimitSwitch->Get()); SmartDashboard::PutNumber("Throttle (%)",stick1->GetY()*(-100)); SmartDashboard::PutNumber("Steering (%)",stick1->GetX()*(100)); GetWatchdog().Feed(); //End Stick1 arcade drive code. GetWatchdog().Feed(); fltShoot = (((-(stick2->GetRawAxis(3)))+1)/2); GetWatchdog().Feed(); SmartDashboard::PutNumber("Shooter Power (%)", fltShoot); SmartDashboard::PutNumber("Shooter Set Speed (%)", (fltSpeed*100)); //float fltPressureSwitch = m_pressureSwitch; //float fltRelay = m_relay; //SmartDashboard::PutNumber("Demo",3); GetWatchdog().Feed(); if(timerShift.Get() > 0.2) { if(stick1->GetRawButton(7) || stick1->GetTrigger()) { if(blnShift == false) { GetWatchdog().Feed(); s[0]->Set(false); s[1]->Set(true); SmartDashboard::PutString("Gear","Low"); blnShift = true; timerShift.Stop(); timerShift.Reset(); timerShift.Start(); GetWatchdog().Feed(); } else if (blnShift == true) { GetWatchdog().Feed(); s[0]->Set(true); s[1]->Set(false); SmartDashboard::PutString("Gear","High"); blnShift = false; timerShift.Stop(); timerShift.Reset(); timerShift.Start(); GetWatchdog().Feed(); } } } if(stick1->GetRawButton(2) && blnLowHang == false && timerLowHang.Get() > 0.5) { blnLowTime = true; blnLowHang = true; timerLowHang.Stop(); timerLowHang.Reset(); timerLowHang.Start(); GetWatchdog().Feed(); } else if(stick1->GetRawButton(2) && blnLowHang == true && timerLowHang.Get() > 0.5) { blnLowTime = false; blnLowHang = false; timerLowHang.Stop(); timerLowHang.Reset(); timerLowHang.Start(); GetWatchdog().Feed(); } if(blnLowTime == true) { s[3]->Set(true); GetWatchdog().Feed(); } else if (blnLowTime == false) { s[3]->Set(false); GetWatchdog().Feed(); } if(stick1->GetRawButton(3)) { mtdCameraCode(); GetWatchdog().Feed(); } if(stick2->GetTrigger() && intFail == 0 && timerFire.Get() > 0.8) { s[2]->Set(true); SmartDashboard::PutString("Shooter Piston","In"); intFail = 1; GetWatchdog().Feed(); timerFire.Stop(); timerFire.Reset(); timerFire.Start(); GetWatchdog().Feed(); } else if(stick2->GetTrigger() && intFail == 1 && timerFire.Get() > 0.8) { s[2]->Set(false); intFail = 0; SmartDashboard::PutString("Shooter Piston","Out"); GetWatchdog().Feed(); timerFire.Stop(); timerFire.Reset(); timerFire.Start(); GetWatchdog().Feed(); } if(stick2->GetRawButton(2) && blnShooterSpd == false && timerShooter.Get() > 0.5) { GetWatchdog().Feed(); myShooter1.Set(-fltSpeed); myShooter2.Set(-fltSpeed); SmartDashboard::PutString("Shooter","On"); SmartDashboard::PutNumber("Shooter Speed (%)",(fltSpeed)*(100)); blnShooterSpd = true; GetWatchdog().Feed(); timerShooter.Stop(); timerShooter.Reset(); timerShooter.Start(); GetWatchdog().Feed(); } else if(stick2->GetRawButton(2) && blnShooterSpd == true && timerShooter.Get() > 0.5) { GetWatchdog().Feed(); myShooter1.Set(0); myShooter2.Set(0); SmartDashboard::PutString("Shooter","Off"); SmartDashboard::PutNumber("Shooter Speed (%)",0); blnShooterSpd = false; GetWatchdog().Feed(); //Wait(0.2); timerShooter.Stop(); timerShooter.Reset(); timerShooter.Start(); GetWatchdog().Feed(); } if(stick2->GetRawButton(10)) { fltSpeed = 0.6; GetWatchdog().Feed(); } if(stick2->GetRawButton(9)) { fltSpeed = 0.7; GetWatchdog().Feed(); } if(stick2->GetRawButton(8)) { fltSpeed = 0.8; GetWatchdog().Feed(); } if(stick2->GetRawButton(7)) { fltSpeed = 0.9; GetWatchdog().Feed(); } if(stick2->GetRawButton(6)) { fltSpeed = 1; GetWatchdog().Feed(); } if(stick2->GetRawButton(11)) { fltSpeed = fltShoot; GetWatchdog().Feed(); } GetWatchdog().Feed(); } }
/** * 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 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()); }
/** * 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(); } }
void OperatorControl (void) { GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though. DriverStationLCD *dsLCD = DriverStationLCD::GetInstance(); dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " "); dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode"); SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector); SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector); SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector); while (IsOperatorControl() && IsEnabled()) { GetWatchdog().Feed(); // Feed the Watchdog. kinectMode = (bool) kinectModeSelector->GetSelected(); demoMode = (bool) demoModeSelector->GetSelected(); speedModeMult = static_cast<double*>(speedModeSelector->GetSelected()); if (kinectMode) { dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " "); dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode"); if (!demoMode) { if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) { motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult); motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult); } else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) { motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult); motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult); } else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) { motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult); motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult); } else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) { motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult); motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult); } else { motorDriveLeft.Set(0); motorDriveRight.Set(0); } } if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) { motorShooter.Set(SHOOTER_MOTOR_POWER); motorFeed.Set(FEED_MOTOR_POWER); motorPickup.Set(PICKUP_MOTOR_POWER); } else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) { motorShooter.Set(SHOOTER_MOTOR_POWER * -1); motorFeed.Set(FEED_MOTOR_POWER * -1); motorPickup.Set(PICKUP_MOTOR_POWER * -1); } else { motorShooter.Set(0); motorFeed.Set(0); motorPickup.Set(0); } if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) { motorTurret.Set(TURRET_POWER); } else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) { motorTurret.Set(TURRET_POWER * -1); } else { motorTurret.Set(0); } } else { if (joystickDriveLeft.GetThrottle() == 0) { dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " "); dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode"); motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult)); motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult)); if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) { motorShooter.Set(SHOOTER_MOTOR_POWER); motorFeed.Set(FEED_MOTOR_POWER); motorPickup.Set(PICKUP_MOTOR_POWER); } else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) { motorShooter.Set(SHOOTER_MOTOR_POWER * -1); motorFeed.Set(FEED_MOTOR_POWER * -1); motorPickup.Set(PICKUP_MOTOR_POWER * -1); } else { motorShooter.Set(0); motorFeed.Set(0); motorPickup.Set(0); } if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) { motorTurret.Set(TURRET_POWER); } else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) { motorTurret.Set(TURRET_POWER * -1); } else { motorTurret.Set(0); } } else { dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, " "); dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode"); motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult)); motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult)); if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) { motorShooter.Set(SHOOTER_MOTOR_POWER); motorFeed.Set(FEED_MOTOR_POWER); motorPickup.Set(PICKUP_MOTOR_POWER); } else if (joystickManipulator.GetRawButton(MANIPULATOR_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) { motorShooter.Set(SHOOTER_MOTOR_POWER * -1); motorFeed.Set(FEED_MOTOR_POWER * -1); motorPickup.Set(PICKUP_MOTOR_POWER * -1); } else { motorShooter.Set(0); motorFeed.Set(0); motorPickup.Set(0); } motorTurret.Set(joystickManipulator.GetX() * -1 * TURRET_POWER); } } dsLCD->UpdateLCD(); } GetWatchdog().SetEnabled(false); // Teleop is done, so let's turn off Watchdog. }
/* * 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 SwerveDrive::Update(Joystick &stick, Joystick &stick2, float gyroValue) { float zInput = stick.GetZ(); // Rotation Clockwise float xInput = stick.GetX(); // Strafe float yInput = stick.GetY(); // Forward float temp = yInput * cos(gyroValue * M_PI / 180) + xInput * sin(gyroValue * M_PI / 180); // This block of commands SHOULD make this thing field oriented xInput = -yInput * sin(gyroValue * M_PI / 180) + xInput * cos(gyroValue * M_PI / 180); yInput = temp; int *controlType; controlType = (int *)a_ControlTypeChooser.GetSelected(); if (controlType == NULL) { std::cout << "error reading control type" << std::endl; return; } float A = xInput - zInput * (a_RobotLength / a_ChassisRadius); float B = xInput + zInput * (a_RobotLength / a_ChassisRadius); float C = yInput - zInput * (a_RobotWidth / a_ChassisRadius); float D = yInput + zInput * (a_RobotWidth / a_ChassisRadius); float max = -9 * pow(10,4); double frSpeed = 0.0; double frAngle = 0.0; double flSpeed = 0.0; double flAngle = 0.0; double blSpeed = 0.0; double blAngle = 0.0; double brSpeed = 0.0; double brAngle = 0.0; switch (*controlType) { case CONTROL_TYPE_SWERVE: frSpeed = sqrt(pow(B,2) + pow(C,2)); flSpeed = sqrt(pow(B,2) + pow(D,2)); blSpeed = sqrt(pow(A,2) + pow(D,2)); brSpeed = sqrt(pow(A,2) + pow(C,2)); max = frSpeed; if(flSpeed > max) { max = flSpeed; } if(blSpeed > max) { max = blSpeed; } if(brSpeed > max) { max = brSpeed; } if(max > 1) { // This is done so that if a speed greater than 1 is calculated, all are reduced proportionally frSpeed /= max; flSpeed /= max; blSpeed /= max; brSpeed /= max; } // atan2 outputs values in a manner similar to what is shown on the below diagram //////////////////// // 0 // // // // // // // // // // //-90///////////90// // // // // // // // // // // -180 or 180 // //////////////////// frAngle = (atan2(C,B) * 180.0 / M_PI); flAngle = (atan2(D,B) * 180.0 / M_PI); blAngle = (atan2(D,A) * 180.0 / M_PI); brAngle = (atan2(C,A) * 180.0 / M_PI); // If on the left side, add 360 to normalize values to the below diagram //////////////////// // 360 or 0 // // // // // // // // // // //270///////////90// // // // // // // // // // // 180 // //////////////////// if(frAngle < 0) { frAngle += 360; } if(flAngle < 0) { flAngle += 360; } if(brAngle < 0) { brAngle += 360; } if(blAngle < 0) { blAngle += 360; } break; case CONTROL_TYPE_CRAB: float setAngle = atan2(yInput,xInput) * 180.0 / M_PI; // find the angle the stick is pointed to, use that for all wheels frAngle = setAngle; flAngle = setAngle; blAngle = setAngle; brAngle = setAngle; float setSpeed = sqrt(pow(xInput,2) + pow(yInput,2)); // find the r of the joystick vector, if you think about it in polar coordinates frSpeed = setSpeed; flSpeed = setSpeed; blSpeed = setSpeed; brSpeed = setSpeed; break; } a_FrontRight.Set(frAngle, frSpeed); a_FrontLeft.Set(flAngle, flSpeed); a_BackLeft.Set(blAngle, blSpeed); a_BackRight.Set(brAngle, brSpeed); }
/** * unchanged from SimpleDemo: * * Runs the motors under driver control with either tank or arcade * steering selected by a jumper in DS Digin 0. * * added for vision: * * Adjusts the servo gimbal based on the color tracked. Driving the * robot or operating an arm based on color input from gimbal-mounted * camera is currently left as an exercise for the teams. */ void OperatorControl(void) { char funcName[] = "OperatorControl"; DPRINTF(LOG_DEBUG, "OperatorControl"); //GetWatchdog().Feed(); TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT); /* for controlling loop execution time */ float loopTime = 0.05; double currentTime = GetTime(); double lastTime = currentTime; double savedImageTimestamp = 0.0; bool foundColor = false; bool staleImage = false; while (IsOperatorControl()) { setServoPositions(rightStick->GetX(), rightStick->GetY()); /* calculate gimbal position based on color found */ if (FindColor (IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, &par, &cReport)) { foundColor = true; if (par.imageTimestamp == savedImageTimestamp) { // This image has been processed already, // so don't do anything for this loop staleImage = true; } else { staleImage = false; savedImageTimestamp = par.imageTimestamp; // compute final H & V destination horizontalDestination = par.center_mass_x_normalized; verticalDestination = par.center_mass_y_normalized; } } else { foundColor = false; } PrintReport(&cReport); if (!staleImage) { if (foundColor) { /* Move the servo a bit each loop toward the * destination. Alternative ways to task servos are * to move immediately vs. incrementally toward the * final destination. Incremental method reduces the * need for calibration of the servo movement while * moving toward the target. */ ShowActivity ("** %s found: Servo: x: %f y: %f", td.name, horizontalDestination, verticalDestination); } else { ShowActivity("** %s not found", td.name); } } dashboardData.UpdateAndSend(); // sleep to keep loop at constant rate // elapsed time can vary significantly due to debug printout currentTime = GetTime(); lastTime = currentTime; if (loopTime > ElapsedTime(lastTime)) { Wait(loopTime - ElapsedTime(lastTime)); } } while (IsOperatorControl()) { // determine if tank or arcade mode; default with no jumper is // for tank drive if (ds->GetDigitalIn(ARCADE_MODE) == 0) { // drive with tank style myRobot->TankDrive(leftStick, rightStick); } else { // drive with arcade style (use right stick) myRobot->ArcadeDrive(rightStick); } } } // end operator control
void OperatorControl(void) { bool button1Bool; bool button2Bool; bool button3Bool; bool button4Bool; bool button5Bool; bool button6Bool; bool button7Bool; bool button8Bool; bool button9Bool; bool button10Bool; bool button11Bool; bool button12Bool; float LeftaxisYValue; float LeftaxisXValue; float RightaxisXValue; float RightaxisYValue; float TriggerValue; myRobot.SetSafetyEnabled(true); while (IsOperatorControl()) { button1Bool = button1.Get(); SmartDashboard::PutNumber("button1",button1Bool); button2Bool = button2.Get(); SmartDashboard::PutNumber("button2",button2Bool); button3Bool = button3.Get(); SmartDashboard::PutNumber("button3",button3Bool); button4Bool = button4.Get(); SmartDashboard::PutNumber("button4",button4Bool); button5Bool = button5.Get(); SmartDashboard::PutNumber("button5",button5Bool); button6Bool = button6.Get(); SmartDashboard::PutNumber("button6",button6Bool); button7Bool = button7.Get(); SmartDashboard::PutNumber("button7",button7Bool); button8Bool = button8.Get(); SmartDashboard::PutNumber("button8",button8Bool); button9Bool = button9.Get(); SmartDashboard::PutNumber("button9",button9Bool); button10Bool = button10.Get(); SmartDashboard::PutNumber("button10",button10Bool); button11Bool = button11.Get(); SmartDashboard::PutNumber("button11",button11Bool); button12Bool = button12.Get(); SmartDashboard::PutNumber("button12",button12Bool); LeftaxisXValue = stick.GetX(); SmartDashboard::PutNumber("Joystick1 X Axis",LeftaxisXValue); LeftaxisYValue = 0 - stick.GetY(); SmartDashboard::PutNumber("Joystick1 Y Axis",LeftaxisYValue); RightaxisXValue = stick.GetTwist(); SmartDashboard::PutNumber("Joystick2 X Axis", RightaxisXValue); RightaxisYValue = 0 - stick.GetRawAxis(5); SmartDashboard::PutNumber("Joystick2 Y Axis", RightaxisYValue); TriggerValue = stick.GetThrottle(); SmartDashboard::PutNumber("Trigger value", TriggerValue); Wait(0.005); // wait for a motor update time } }
void TeleopPeriodic() { if(tick==10) if (ds->IsSysBrownedOut()) { ds->ReportError("[ERROR] BROWNOUT DETECTED!!"); } if(tick == 15) if (!ds->IsNewControlData()) { ds->ReportError( "[ERROR] NO DATA FROM DRIVER STATION IN THIS TICK!"); } if(tick==20) if (!ds->IsDSAttached()) { ds->ReportError("[ERROR] DRIVER STATION NOT DETECTED!"); } if (stick.GetRawButton(10)) zeroSanics(); if (stick.GetRawButton(8)) { leftIRZero = 0; rightIRZero = 0; } tick++; if (liftStick.GetRawButton(2)) { double canScale = liftStick.GetRawAxis(2); canScale += 1; canScale = 2 - canScale; canScale /= 2; canGrabber.SetSpeed(canScale); } else if (liftStick.GetRawButton(3)) { double canScale = liftStick.GetRawAxis(2); canScale += 1; canScale = 2 - canScale; canScale /= 2; canGrabber.SetSpeed(-canScale); } else canGrabber.SetSpeed(0); double speed; //Calculate scalar to use for POV/Adjusted drive double scale = stick.GetRawAxis(3); scale += 1; scale = 2 - scale; scale /= 2; //Use pov/hat switch for movement if enabled if (stick.GetRawButton(1) && stick.GetRawButton(2)) { AutomaticLineup(); } else if (stick.GetRawButton(1)) { double leftVolts = leftIR.GetAverageVoltage() - leftIRZero; double rightVolts = rightIR.GetAverageVoltage() - leftIRZero; if (rightVolts + VOLTAGE_TOLERANCE > leftVolts && rightVolts - VOLTAGE_TOLERANCE < leftVolts) { robotDrive.MecanumDrive_Cartesian(0, 0, 0); } else if (rightVolts > leftVolts) robotDrive.MecanumDrive_Cartesian(0, 0, 0.2); else if (leftVolts > rightVolts) robotDrive.MecanumDrive_Cartesian(0, 0, -0.2); } else if (stick.GetRawButton(6)) { //Rotate robotDrive.MecanumDrive_Polar(0, 0, scale); } else if (stick.GetRawButton(5)) { //Rotate robotDrive.MecanumDrive_Polar(0, 0, -scale); } else if (stick.GetPOV(0) != -1) { //If POV moved, move polar (getPOV returns an angle in degrees) robotDrive.MecanumDrive_Polar(scale, -stick.GetPOV(0), 0); } else if (stick.GetRawButton(2)) { //Drive with scalar robotDrive.MecanumDrive_Cartesian(-stick.GetRawAxis(0) * scale, stick.GetRawAxis(1) * scale, stick.GetRawAxis(2) * scale); } else { //Drive normally robotDrive.MecanumDrive_Cartesian(-stick.GetX(), stick.GetY(), stick.GetZ()); } speed = -liftStick.GetY(); //bool canGoUp = maxUp.Get(); bool canGoUp = true; //bool canGoDown = maxDown.Get(); bool canGoDown = true; //If at a limit switch and moving in that direction, stop if (speed > 0 && !canGoUp) speed = 0; if (speed < 0 && !canGoDown) speed = 0; chainLift.SetSpeed(speed); if (tick >50) { if (SmartDashboard::GetBoolean("Smart Dashboard Enabled")) { //Smart Dash outputs //SmartDashboard::PutNumber("X Acceleration: ", accel.GetX()); //SmartDashboard::PutNumber("Y Acceleration: ", accel.GetY()); //SmartDashboard::PutNumber("Z Acceleration: ", accel.GetZ()); SmartDashboard::PutBoolean("Switch 1: (up)", maxUp.Get()); SmartDashboard::PutBoolean("Switch 2: (down)", maxDown.Get()); SmartDashboard::PutBoolean("Switch 3: (mid)", midPoint.Get()); SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get()); SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get()); //SmartDashboard::PutBoolean("RobotDrive Alive?", // robotDrive.IsAlive()); //SmartDashboard::PutBoolean("ChainLift Alive?", // robotDrive.IsAlive()); SmartDashboard::PutNumber("Left Sensor", leftIR.GetAverageVoltage()); SmartDashboard::PutNumber("Right Sensor", rightIR.GetAverageVoltage()); SmartDashboard::PutNumber("Left w zero", leftIR.GetAverageVoltage() - leftIRZero); SmartDashboard::PutNumber("Rigt w zero", rightIR.GetAverageVoltage() - rightIRZero); SmartDashboard::PutNumber("PDP 14 Current", pdp.GetCurrent(14)); SmartDashboard::PutNumber("PDP 15 Current", pdp.GetCurrent(15)); } tick = 0; } }
/** * 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() { /*ALTERNATE OPTION: use constants at the top of the code to pass in buttons associated * with various actions. * *FOR ALTERNATE LAYOUTS: *have one set of layouts commented out? *take input from Driver Station I/O tab to determine which set is used? * */ //driver joystick: Extreme 3D bool compressor = stickDrive->GetRawButton(10); //drive-related bool stopDrive = stickDrive->GetRawButton(1); bool slowSpeed = stickDrive->GetRawButton(2); //arm controls--solenoids involved bool extendAll_driver = stickDrive->GetRawButton(9); bool retractAll_driver = stickDrive->GetRawButton(11); //spinner controls bool leftDrop_driver = stickDrive->GetRawButton(3); bool leftPick_driver = stickDrive->GetRawButton(5); bool rightDrop_driver = stickDrive->GetRawButton(4); bool rightPick_driver = stickDrive->GetRawButton(6); //other joystick: Attack 3 //arm controls--solenoids involved bool extendAll_other = stickOther->GetRawButton(3); bool retractAll_other = stickOther->GetRawButton(2); bool leftArm_other = (stickOther->GetRawButton(6) || stickOther->GetRawButton(10)); bool rightArm_other = (stickOther->GetRawButton(7) || stickOther->GetRawButton(11)); //spinner controls bool leftDrop_other = (stickOther->GetRawButton(4) && !stickOther->GetRawButton(1)); bool leftPick_other = (stickOther->GetRawButton(4) && stickOther->GetRawButton(1)); bool rightDrop_other = (stickOther->GetRawButton(5) && !stickOther->GetRawButton(1)); bool rightPick_other = (stickOther->GetRawButton(5) && stickOther->GetRawButton(1)); SmartDashboard::PutBoolean("Left-Spinner", spinnerStatusLeft); SmartDashboard::PutBoolean("Right-Spinner", spinnerStatusLeft); SmartDashboard::PutBoolean("S1-Status", s1Status); SmartDashboard::PutBoolean("S2-Status", s2Status); SmartDashboard::PutBoolean("S3-Status", s3Status); SmartDashboard::PutNumber("Compressor-Pressure", this->compressor->GetPressureSwitchValue()); if(compressor) { toggleCompressor(); } ++periodicCounter; float x = stickDrive->GetX(); float y = stickDrive->GetY(); float z = (stickDrive->GetRawAxis(3)) / 1.5; //actually twist--capped at 2/3 for speed control if(abs(x) < .125) x =0; if(abs(y) < .125) y =0; if(abs(z) < .125) z =0; if(!slowSpeed) { //myRobot.MecanumDrive_Cartesian(x, y, z, 0.0); MecanumDrive(x, y, z, 0.0); } else if(slowSpeed) { if (x < 0) x = x/3; else x = x/3; if (y < 0) y = y/3; else y = y/3; if (z < 0) z = z/3; else z = z/3; //myRobot.MecanumDrive_Cartesian(x, y, z, 0.0); MecanumDrive(x, y, z, 0.0); } //stop robot if (stopDrive){ stop(); return; } //extend all arms if(extendAll_driver){ extend(s1,s1Status); //spinner arms go out first extend(s3,s3Status); Wait(.2); extend(s2,s2Status); return; } //retract all arms if(retractAll_driver){ retract(s2,s2Status); //catcher arms come in first Wait(.2); retract(s3,s3Status); retract(s1,s1Status); return; } //toggle left set of spinners if(leftDrop_driver){ toggleLeftDrop(SPINNER_SPEED); return; } if(leftPick_driver){ toggleLeftPick(SPINNER_SPEED); return; } //toggle right set of spinners if(rightDrop_driver){ toggleRightDrop(SPINNER_SPEED); return; } if(rightPick_driver){ toggleRightPick(SPINNER_SPEED); return; } //attack3 joystick code below //extend all arms if(extendAll_other){ extend(s1,s1Status); //spinner arms go out first extend(s3,s3Status); Wait(.2); extend(s2,s2Status); Wait(0.5); return; } //retract all arms if(retractAll_other){ retract(s2,s2Status); //catcher arms come in first Wait(.2); retract(s3,s3Status); retract(s1,s1Status); Wait(0.5); return; } if(leftDrop_other){ spinnerL1->Set(SPINNER_SPEED); //may need to spinnerL2->Set(SPINNER_SPEED); //reverse these return; } if(leftPick_other){ spinnerL1->Set(-SPINNER_SPEED); //may need to spinnerL2->Set(-SPINNER_SPEED); //reverse these return; } //toggle right set of spinners if(rightDrop_other){ spinnerR1->Set(SPINNER_SPEED); spinnerR2->Set(SPINNER_SPEED); return; } if(rightPick_other){ spinnerR1->Set(-SPINNER_SPEED); spinnerR2->Set(-SPINNER_SPEED); return; } if (!(leftDrop_driver || leftPick_driver || leftDrop_other || leftPick_other)) { stopSpinnersLeft(); } if (!(rightDrop_driver || rightPick_driver || rightDrop_other || rightPick_other)) { stopSpinnersRight(); } //toggle left spinner arm if(leftArm_other) { toggle(s1,s1Status); return; } //toggle right spinner arm if(rightArm_other) { toggle(s3,s3Status); return; } }