/** * Get the twist value of the current joystick. * This depends on the mapping of the joystick connected to the current port. * * @param port The USB port for this joystick. */ float GetTwist(UINT32 port) { Joystick *stick = getJoystick(port); if (stick == NULL) return 0; return stick->GetTwist(); }
/** * @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 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); } }
/** 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 } }
void driveTrainValues(void) { //Testing the encoder... //Get the encoder value //encoder_value = right_encoder->Get(); //Print encoder value //printf("right encoder value: %f ", encoder_value); //Shifting gears... //Low gear if (gamepad->GetRawButton(5)) { printf("The left button is being pressed.\n"); drivetrain_pressure->Set(false); drivetrain_vent->Set(true); } //Six is high gear if (gamepad->GetRawButton(6)) { printf("The right button is being pressed.\n"); drivetrain_pressure->Set(true); drivetrain_vent->Set(false); } //assign right joystick value to 'right' right=(gamepad->GetTwist()); //find the change in joystick values rightchange=fabs(oldright-right); // if the change in joystick values is less than 0.2 then use the old right value vs the new one //Stops driver from accelerating the robot too quickly if(rightchange<=threshold) { useright=right; } //else... if the new right is greater than the old one use the old value plus the threshold (faster) //if the new right is less than the old right use the old value minue the threshold (slower) //so that really large movements of the joystick don't translate immediately into a really big acceleration else { if(oldright<right) { useright=oldright+threshold; } if(oldright>right) { useright=oldright-threshold; } } //Same as right, but on the left side left=(gamepad->GetY()); leftchange=fabs(oldleft-left); if(leftchange<=threshold) { useleft=left; } else { if(oldleft<left) { useleft=oldleft+threshold; } if(oldleft>left) { useleft=oldleft-threshold; } } //printf("hello world"); //make useright and useleft (the values sent to the robot) the old values for the next loop oldright=useright; oldleft=useleft; }
/**************************************** * Runs the motors with arcade steering.* ****************************************/ void OperatorControl(void) { //TODO put in servo for lower camera--look in WPI to set // Watchdog baddog; // baddog.Feed(); myRobot.SetSafetyEnabled(true); //SL Earth.Start(); // turns on Earth // SmartDashboard *smarty = SmartDashboard::GetInstance(); //DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory) //char debugout [100]; compressor.Start(); gyro.Reset(); // resets gyro angle int rpmForShooter; while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop" { // baddog.Feed(); //myRobot.SetSafetyEnabled(true); //myRobot.SetExpiration(0.1); float leftYaxis = driver.GetY(); float rightYaxis = driver.GetTwist();//RawAxis(5); myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1 float random = gamecomponent.GetY(); float lazysusan = gamecomponent.GetZ(); //bool elevator = Frodo.Get(); float angle = gyro.GetAngle(); bool balance = Smeagol.Get(); SmartDashboard::PutNumber("Gyro Value",angle); int NumFail = -1; //bool light = Pippin.Get(); //SL float speed = Earth.GetRate(); //float number = shooter.Get(); //bool highspeed = button1.Get() //bool mediumspeed = button2.Get(); //bool slowspeed = button3.Get(); bool finder = autotarget.Get(); //bool targetandspin = autodistanceandspin.Get(); SmartDashboard::PutString("Targeting Activation",""); //dslcd->Clear(); //sprintf(debugout,"Number=%f",angle); //dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout); //SL sprintf(debugout,"Number=%f",speed); //SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout); //sprintf(debugout,"Number=%f",number); //dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout); //sprintf(debugout,"Finder=%u",finder); //dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout); //dslcd->UpdateLCD(); // update the Driver Station with the information in the code // sprintf(debugout,"Number=%u",maxi); // dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout) bool basketballpusher = julesverne.Get(); bool bridgetipper = joystickbutton.Get(); if (bridgetipper) // if joystick button 7 is pressed (is true) { solenoid.Set(true); // then the first solenoid is on } else { //Wait(0.5); // and then the first solenoid waits for 0.5 seconds solenoid.Set(false); //and then the first solenoid turns off } if (basketballpusher) // if joystick button 6 is pressed (is true) { shepard.Set(true); // then shepard is on the run //Wait(0.5); // and shepard waits for 0.5 seconds } else { shepard.Set(false); // and then shepard turns off } //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2 //} //cheetah.Set(0.3*lazysusan); // smarty->PutDouble("pre-elevator",lynx.Get()); lynx.Set(random); // smarty->PutDouble("elevator",lynx.Get()); // smarty->PutDouble("joystick elevator",random); if (balance) // this is the start of the balancing code { angle = gyro.GetAngle(); myRobot.Drive(-0.03*angle, 0.0); Wait(0.005); } /*if (light) //button 5 turns light on oand off on game controller flashring.Set(Relay::kForward); else flashring.Set(Relay::kOff); */ if (finder) { flashring.Set(Relay::kForward); if (button_H.Get()==true) { targeting.SetLMHTarget(BOGEY_H); SmartDashboard::PutString("Targeting","High Button Pressed"); } if (button_M.Get()==true) { targeting.SetLMHTarget(BOGEY_M); SmartDashboard::PutString("Targeting","Medium Button Pressed"); } if (button_L.Get()==true) { targeting.SetLMHTarget(BOGEY_L); SmartDashboard::PutString("Targeting","Low Button Pressed"); } if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true) { if (targeting.ProcessOneImage()) { NumFail = 0; SmartDashboard::PutString("Targeting Activation","YES"); targeting.ChooseBogey(); targeting.MoveTurret(); #ifdef USE_HARDWIRED_RPM shooter.setTargetRPM(HARDWIRED_RPM); #else rpmForShooter = targeting.GetCalculatedRPM(); shooter.setTargetRPM(rpmForShooter); #endif targeting.InteractivePIDSetup(); } else { NumFail++; if (NumFail > 10) targeting.StopPID(); } SmartDashboard::PutNumber("Numfail", NumFail); shooter.setTargetRPM(rpmForShooter); } else { SmartDashboard::PutString("Targeting Activation","NO"); shooter.setTargetRPM(0); targeting.StopPID(); } } else { flashring.Set(Relay::kOff); targeting.StopPID(); turret.Set(lazysusan); // the lazy susan would turn right & left based on how far the person moves the right joystick#2 side to side //targeting.StopPID(); //if (elevator) //shooter would move at full speed if button is pressed //TODO Change RPM values //TODO Disable calculation of RPM values SmartDashboard::PutNumber("CurrentRPM",shooter.GetCurrentRPM()); if (button_H.Get() == true) shooter.setTargetRPM((int)2100); //From front of free throw line, should hit the backboard and go in //used to be 2700 RPMs else if (button_M.Get() == true) shooter.setTargetRPM((int)1900); //From front of free throw line, should go in the net--can shoot the next ball on the overshoot? //Used to be 2250 RPMs else if (button_L.Get() == true) shooter.setTargetRPM((int)1350); //From fender, should hit the backboard //Used to be 2000 RPMs //shooter.Set(0.5); else shooter.setTargetRPM(0); // else if (mediumspeed) //shooter.setTargetRPM((int)0); //else if (slowspeed) //shooter.setTargetRPM((int)0); /*if (targetandspin) //code for autotargeting and speed will go here { shooter.setTargetRPM((int)1800); } else {*/ //} myRobot.TankDrive(leftYaxis,rightYaxis); } //Wait(0.005); } }
void 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 OperatorControl(void) { // feed watchdog --- TheVessel.SetSafetyEnabled(true); GetWatchdog().Kill(); if (!game){ AlmostZeroPi = manEnc->GetDistance() -1;//Zeros manipulator encoder-- only here for tele op practice w/out auto cmp->Start(); } cd->MecanumDrive(0, 0, 0); minibot->Set(DoubleSolenoid::kForward); while(IsOperatorControl() && IsEnabled()) { GetWatchdog().Kill(); if(IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kReverse); else if(!IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kForward); cd->MecanumDrive(controller);//passes joystick controller's input to cd mechdrv if(stick->GetRawButton(1)) { } else if(stick->GetRawButton(2)) { } else if(stick->GetRawButton(3)) { reset = true;// ENABLES NO RESTRICTION MANIPULATOR MOVEMENT } else if(stick->GetRawButton(4)) { cout << "Manipulator Encoder Value: " << manEnc->Get() << "/n";//prints manip encoder to console if open; Wait(.1f);//pause so not above a billion times } else if(stick->GetRawButton(6)) { open->Set(true);//self explanatory, eote pnuematics closed->Set(false); } else if(stick->GetRawButton(8)) { closed->Set(true); open->Set(false); } ShoulderJag->Set(stick->GetY());//shoulder moves with stick->GetY() if (stick->GetTwist() < 0 || manEnc->GetDistance() < AlmostZeroPi || reset)//man extending, enc not at 0, or reset true { manJag->Set(stick->GetTwist()*.3);//shoulder above but for wrist, also, only 3/10ths power } } cmp->Stop();//stops compressor while (manEnc->GetDistance() < AlmostZeroPi)//brings manipulator back to 0 so not need reset { manJag->Set(.1); } }