//Drive base control void DriveBaseControl(void){ //Local declarations float driveThreshold = 0.005; //Get the y-axis of the joystick float yAxis1 = 1 * leftStick.GetY(); float yAxis2 = 1 * rightStick.GetY(); //std::cout << "yAxisVal: " << yAxisVal << std::endl; //Convert input to [-1.0, 1.0] range //Don't need to - already in the correct range //Drive the drive motors when any input is within +-driveThreshold of 0.0 //NOTE - currently this doesn't scale up the input from 0.0 after the deadband region -- it just uses the raw value. if(yAxis1 >= driveThreshold || yAxis2 >= driveThreshold || yAxis1 <= -driveThreshold || yAxis2 <= -driveThreshold ) { robotDrive.TankDrive(-yAxis1,-yAxis2); // drive Forwards } else { robotDrive.TankDrive(0.0, 0.0); // stop robot } }
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(); }
/** * 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 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()); } } }
void TeleopPeriodic() { rightDrive = rightStick->GetY(); leftDrive = leftStick->GetY(); rightDrive = .6*rightDrive; leftDrive = .6*leftDrive; robotDrive->TankDrive(rightDrive, leftDrive); ax = accel-> GetX(); ay = accel-> GetY(); az = accel-> GetZ(); SmartDashboard::PutData("Auto Modes", chooser); SmartDashboard::PutNumber("ax",ax); SmartDashboard::PutNumber("ay",ay); SmartDashboard::PutNumber("az",az); bool triggerRight = rightStick->GetRawButton(1); bool triggerLeft = leftStick->GetRawButton(1); SmartDashboard::PutBoolean("trigger", triggerRight); SmartDashboard::PutBoolean("trigger", triggerLeft); if(triggerRight || triggerLeft){ pickup->Set(.3); } else{ pickup->Set(0); } }
/** * Runs the motors with arcade steering. */ void OperatorControl(void) { left1.Set(leftStick.GetY()); left2.Set(leftStick.GetY()); right1.Set(rightStick.GetY()); right2.Set(rightStick.GetY()); }
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::dumb_drive_code() { #if DUMB_DRIVE_CODE left_drive_motor_A->Set(-drive_stick_sec ->GetY()); left_drive_motor_B->Set(-drive_stick_sec->GetY()); right_drive_motor_A->Set(drive_stick_prim->GetY()); right_drive_motor_B->Set(drive_stick_prim->GetY()); #endif }
/** * Function to control and actuate all us er inputs related to the drive base * * Left Joystick - the Y axis will control the left drive base motors forward or back. * * Right Joystick - the Y axis will control the right drive base motors forward or back. */ void ManualDriveBaseActuation() { //get joystick inputs with GetY and filter float lefty = DeadZone(leftDriveStick.GetY(), DRIVE_MM_DEADZONE_VAL); float righty = DeadZone(rightDriveStick.GetY(), DRIVE_MM_DEADZONE_VAL); //float arm = DeadZone(Operator_Control_Stick.GetY()); //driving left side inverted motor1.Set(-1 * lefty); motor3.Set(-1 * lefty); motor5.Set(-1 * lefty); //driving right Side motor2.Set(righty); motor4.Set(righty); motor6.Set(righty); }
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 arcade steering. */ void OperatorControl(void) { GetWatchdog().SetEnabled(true); compressor->Start(); GetWatchdog().SetExpiration(0.5); bool valve_state = false; while (IsOperatorControl()) { motor->Set(stick->GetY()); if (stick->GetRawButton(1) && !valve_state) { valve->Set(true); valve_state = true; } if (!stick->GetRawButton(1) && valve_state) { valve->Set(false); valve_state = false; } // Update driver station //dds->sendIOPortData(valve); GetWatchdog().Feed(); } }
/** * 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 } }
void TeleopPeriodic() { static int targetSpeed = 0; targetSpeed+= stick->GetY()*-3; static int fakerange = 0; if (stick->GetRawButton(2)) { targetSpeed=0; fakerange = 250; } if (stick->GetRawButton(3)) { targetSpeed += 150; } if (stick->GetRawButton(4)) { targetSpeed -= 150; } if(stick->GetRawButton(5)) { fakerange+=3; } if(stick->GetRawButton(6)) { fakerange-=3; } launch->Obey(); launch->SetTargetSpeed(targetSpeed); float currAngle= angle->PIDGet(); float range = fakerange*cos(currAngle*3.1415/180); std::cout<<"range estimate = "<<range<<std::endl; launch->Aim(range/100.0); int current =rightWheel->GetEncVel(); std::cout<<targetSpeed<<" "<<current<<std::endl; }
/** * Get the Y 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 GetY(UINT32 port, JoystickHand hand) { Joystick *stick = getJoystick(port); if (stick == NULL) return 0; return stick->GetY((Joystick::JoystickHand) hand); }
void OperatorControl(void) { OperatorControlInit(); compressor.Start(); testActuator.Start(); while (IsOperatorControl()) { ProgramIsAlive(); //No need to do waits because ProgramIsAlive function does a wait. //Wait(0.005); bool isButtonPressed = stick.GetRawButton(3); SmartDashboard::PutNumber("Actuator Button Status",isButtonPressed); if (isButtonPressed) { testActuator.Go(); } float leftYaxis = stick.GetY(); float rightYaxis = stick.GetRawAxis(5); //RawAxis(5); TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1 SmartDashboard::PutNumber("Left Axis",leftYaxis); SmartDashboard::PutNumber("Right Axis",rightYaxis); } }
/** * Runs the motors under driver control with either tank or arcade steering selected * by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. */ void OperatorControl(void) { Victor armMotor(5); // create arm motor instance while (IsOperatorControl()) { GetWatchdog().Feed(); // determine if tank or arcade mode; default with no jumper is for tank drive if (ds->GetDigitalIn(ARCADE_MODE) == 0) { myRobot->TankDrive(leftStick, rightStick); // drive with tank style } else{ myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick) } // Control the movement of the arm using the joystick // Use the "Y" value of the arm joystick to control the movement of the arm float armStickDirection = armStick->GetY(); // if at a limit and telling the arm to move past the limit, don't drive the motor if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) { armStickDirection = 0; } else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) { armStickDirection = 0; } // Set the motor value armMotor.Set(armStickDirection); } }
void TeleopPeriodic(void) { Scheduler::GetInstance()->Run(); myDrive_all->TankDrive(drivestick_left->GetY(), drivestick_right->GetY()); ABCheck = Xbox_Button_A->Get(); // Gets the value of the A button on the xbox BBCheck = Xbox_Button_B->Get(); // Gets the value of the B button on the xbox if (ABCheck == 1) // If A button is pressed { FirstSolenoid->Set(DoubleSolenoid::kForward); // Make piston extend } if (BBCheck == 1) // If B button is pressed { FirstSolenoid->Set(DoubleSolenoid::kReverse); // Make piston retract } if ((ABCheck == 0) && (BBCheck == 0)) // If A and B buttons are not pressed { FirstSolenoid->Set(DoubleSolenoid::kOff); // Make piston stay where it is } XBCheck = Xbox_Button_X->Get(); // Gets value of the X button on the xbox YBCheck = Xbox_Button_Y->Get(); // Get value of the Y button on the xbox if (XBCheck == 1)//If X button is pressed { if (limitSwitch->Get() == 0) // If limitswitch is pressed { intake_Motor->Set(0); // Stop test motor } if (limitSwitch->Get() == 1) // If limitswitch is not pressed { intake_Motor->Set(1); // Test motor goes forward at 0.5 speed } } if (YBCheck == 1) // If Y button is pressed { intake_Motor->Set(-1); // Test motor goes backward at -0.5 speed } if ((XBCheck == 0) && (YBCheck == 0)) // If X and Y buttons are not pressed { intake_Motor->Set(0); // Test motor stops } }
/** * @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()); } } }
/** * Runs the motor from the output of a Joystick. */ void OperatorControl() { while (IsOperatorControl() && IsEnabled()) { // Set the motor controller's output. // This takes a number from -1 (100% speed in reverse) to +1 (100% speed forwards). m_motor.Set(m_stick.GetY()); Wait(kUpdatePeriod); // Wait 5ms for the next update. } }
// Called repeatedly when this Command is scheduled to run void DriveBot::Execute() { Joystick* stick = Robot::oi->getGamePad(); if(Robot::mover->speedSwitch->Get()){ Robot::mover->rightMotor0->Set(deadband(stick->GetThrottle()*-0.75,.01)); Robot::mover->rightMotor1->Set(deadband(stick->GetThrottle()*-0.75,.01)); Robot::mover->leftMotor2->Set(deadband(stick->GetY()*0.75,.01)); Robot::mover->leftMotor3->Set(deadband(stick->GetY()*0.75,.01)); }else{ Robot::mover->rightMotor0->Set(stick->GetThrottle()*-0.25); Robot::mover->rightMotor1->Set(stick->GetThrottle()*-0.25); Robot::mover->leftMotor2->Set(stick->GetY()*0.25); Robot::mover->leftMotor3->Set(stick->GetY()*0.25); } }
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 } }
void HandleDriverInputsAutomatic(void) { //myRobot.ArcadeDrive(stick); if(DoubleSolenoid::kReverse == shifter.Get()) { if(stick.GetY() < -0.25) { shifter.Set(DoubleSolenoid::kForward); } } // If the robot is in low gear and is over 0.2 input, // then switch into high gear. else if(stick.GetY() > -0.2) { shifter.Set(DoubleSolenoid::kReverse); } SquareInputs(); }
// 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 } }
void TeleopPeriodic(void) { //Drive code leftFrontDrive->Set(-1 * leftStick->GetY()); rightFrontDrive->Set(rightStick->GetY()); leftRearDrive->Set(-1 * leftStick->GetY()); rightRearDrive->Set(rightStick->GetY()); //airCompressor->Start(); // Simple Button Drive Forward if(rightStick->GetRawButton(3)) { leftFrontDrive->Set(-1); rightFrontDrive->Set(1); leftRearDrive->Set(-1); rightRearDrive->Set(1); } // Simple Button Drive Backwards if(rightStick->GetRawButton(2)) { leftFrontDrive->Set(1); rightFrontDrive->Set(-1); leftRearDrive->Set(1); rightRearDrive->Set(-1); } // Code to print to the user messages box in the Driver Station LCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hello World"); LCD->Printf(DriverStationLCD::kUser_Line2,1,"Y Left: %f", leftStick->GetY()); LCD->Printf(DriverStationLCD::kUser_Line3,1,"Y Right: %f", rightStick->GetY()); LCD->UpdateLCD(); Wait(0.2); }
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); } }
/** * @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]); }
/** Function to retrieve the user inputs * Inputs (For all user input modes): * * Potentiometers * * Limit switches * * the requested state for the robot (from the joystick buttons) * For manual mode only: * * The intake and ejection wheel speeds - based on the operator joystick throttle * * The arm position - based on the operator joystick Y axis */ void GetUserInputs(RobotStates_t &requestedState) { float throttleRaw = 0.0; //Read potentiometer goes from -5 to 28 potentiometerValueRight = armPotentiometer1.GetValue(); //potentiometerValueLeft = armPotentiometer2.GetValue(); ballCaughtLimitSwitch1 = limitSwitch1.Get(); //TODO 0 is no ball 1 is ball //Read Joystick state buttons requestedState = ReadJoystickButtons(); throttleRaw = DeadZone(Operator_Control_Stick.GetTwist(), EJWHEEL_MM_DEADZONE_VAL); manualModeThrottlePosition = (throttleRaw * -1); //invert throttle value to match the joystick manualModeArmPosition = DeadZone(Operator_Control_Stick.GetY(), ARM_MM_DEADZONE_VAL); }
/** * 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 } }