/** * Get the throttle 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 GetThrottle(UINT32 port) { Joystick *stick = getJoystick(port); if (stick == NULL) return 0; return stick->GetThrottle(); }
/** * @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()); } } }
// 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); } }
/** * @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 DriveControl() { //Drives la wheels of the robot flightX = flightStick->GetRawAxis(0); //Pull joystick side motion for later use flightY = flightStick->GetRawAxis(1); //Pull joystick forward motion for later use (forward is -, backwards is +) flightZ = flightStick->GetRawAxis(4); //Pull joystick twist motion for later use flightThrottle = ((((shootStick->GetThrottle() - 1)*-1)/2) * .8 + .2); //Pull throttle to modify drive variables //Throttle value is between .2 and 1.0 if (fabs(flightX) < deadZone) { //Deaden x flightX = 0; } if (fabs(flightY) < deadZone) { //Deaden y flightY = 0; } if (fabs(flightZ) < deadZone) { //Deaden z flightZ = 0; } if (flightStick->GetRawButton(strafeButtonChannel)){ //Set drive to strafe mode driveMode = 0; } if (flightStick->GetRawButton(arcadeButtonChannel)){ //Set drive to arcade mode driveMode = 1; } if (flightStick->GetRawButton(fieldButtonChannel)){ //Set drive to field-centric mode driveMode = 2; } if (shootStick->GetRawButton(gyroResetChannel)){ //Reset gyro with the trigger yawGyro->Reset(); } flightX = flightX * flightThrottle; flightY = flightY * flightThrottle; flightZ = flightZ * flightThrottle; if(driveMode == 1){ robotDrive->MecanumDrive_Cartesian(flightZ, flightY, flightX, 0); SmartDashboard::PutString("DriveMode", "Arcade"); } else if(driveMode == 2){ robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, yawGyro->GetAngle()); SmartDashboard::PutString("DriveMode", "Field"); } else{ robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, 0); SmartDashboard::PutString("DriveMode", "Strafe"); } SmartDashboard::PutNumber("GyroAngle", yawGyro->GetAngle()); }
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. }
void TeleopPeriodic() { //camera->GetImage(frame); //imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f); //CameraServer::GetInstance()->SetImage(frame); printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle()); drive->ArcadeDrive(driveStick); drive->SetMaxOutput((1-driveStick->GetThrottle())/2); //printf("%f\n", (1-stick->GetThrottle())/2); //leftMotor->Set(0.1); //rightMotor->Set(0.1); if (shootStick->GetRawAxis(3) > 0.5) { launch1->Set(1.0); launch2->Set(1.0); } else if (shootStick->GetRawAxis(2) > 0.5) { printf("Power Counter: %i\n", powerCounter); if (powerCounter < POWER_MAX) { powerCounter++; launch1->Set(-0.8); launch2->Set(-0.8); } else { launch1->Set(-0.6); launch2->Set(-0.6); } } else { launch1->Set(0.0); launch2->Set(0.0); powerCounter = 0.0; } //use this button to spin only one winch, to lift up. if (shootStick->GetRawButton(7)) { otherWinch->Set(0.5); } else if (shootStick->GetRawButton(8)) { otherWinch->Set(-0.5); } else { otherWinch->Set(0.0); } if (shootStick->GetRawButton(5)) { winch->Set(-0.7); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { // otherWinch->Set(-0.5); } } else if (shootStick->GetRawButton(6)) { winch->Set(0.7); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { // otherWinch->Set(0.5); } } else { winch->Set(0.0); if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) { //, otherWinch->Set(0.0); } } if (shootStick->GetRawButton(1)) { launchPiston->Set(1); } else { launchPiston->Set(0); } if (shootStick->GetRawButton(3)) { Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer); } if (shootStick->GetRawButton(3) && debounce == false) { debounce = true; if (defenseUp) { defensePiston->Set(DoubleSolenoid::Value::kReverse); defenseUp = false; } else { defenseUp =true; defensePiston->Set(DoubleSolenoid::Value::kForward); } } else if (!shootStick->GetRawButton(3)){ debounce = false; } }
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() override { float leftPower, rightPower; // Get the values for the main drive train joystick controllers leftPower = -leftjoystick->GetY(); rightPower = -rightjoystick->GetY(); float multiplier; // TURBO mode if (rightjoystick->GetRawButton(1)) { multiplier = 1; } else { multiplier = 0.5; } // wtf is a setpoint - it's an angle to turn to if (leftjoystick->GetRawButton(6)) { turnController->Reset(); turnController->SetSetpoint(0); turnController->Enable(); ahrs->ZeroYaw(); //ahrs->Reset(); } // Press button to auto calculate angle to rotate bot to nearest ball // if(leftjoystick->GetRawButton(99)) // { // ahrs->ZeroYaw(); // turnController->Reset(); // turnController->SetSetpoint(mqServer.GetDouble("angle")); // turnController->Enable(); // aimState = 1; // } switch(aimState) { default: case 0: // No camera assisted turning //Drive straight with one controller, else: drive with two controllers if (leftjoystick->GetRawButton(1)) { drive->TankDrive(leftPower * multiplier, leftPower * multiplier, false); } else if (leftjoystick->GetRawButton(2)) { drive->TankDrive(leftPower * multiplier + rotateRate, leftPower * multiplier + -rotateRate, false); } else { drive->TankDrive(leftPower * multiplier, rightPower * multiplier, false); } break; case 1: // Camera assisted turning, deny input from controllers drive->TankDrive(rotateRate, -rotateRate, false); if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) { aimState = 0; // Finished turning, auto assist off turnController->Disable(); turnController->Reset(); } break; } // That little flap at the bottom of the joystick float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2); // Depending on the button, our intake will eat or shoot the ball if (controlstick->GetRawButton(2)) { intake->Set(-scaleIntake); shooter->Set(scaleIntake); } else if (controlstick->GetRawButton(1)) { intake->Set(scaleIntake); shooter->Set(-scaleIntake); } else { intake->Set(0); shooter->Set(0); } // Control the motor that lifts and descends the intake bar float intake_lever_power = 0; if (controlstick->GetRawButton(6)) { manual = true; intake_lever_power = .3; // intakeLever->Set(.30); // close } else if (controlstick->GetRawButton(4)) { manual = true; intake_lever_power = -.4; // intakeLever->Set(-.40); // open } else if (controlstick->GetRawButton(3)){ manual = true; intake_lever_power = -scaleIntake; // intakeLever->Set(-scaleIntake); } else if (controlstick->GetRawButton(5)) { manual = true; intake_lever_power = scaleIntake; // intakeLever->Set(scaleIntake); } else { if (manual) { manual = false; lastLiftPos = intakeLever->GetEncPosition(); intakeLever->SetControlMode(CANTalon::ControlMode::kPosition); intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder); intakeLever->SetPID(1, 0.001, 0.0); intakeLever->EnableControl(); } intake_hold = true; intakeLever->Set(lastLiftPos); } if (manual) { intake_hold = false; intakeLever->SetControlMode(CANTalon::ControlMode::kPercentVbus); intakeLever->Set(intake_lever_power); } if (controlstick->GetRawButton(11)) { lift->Set(true); liftdown->Set(false); } else if (controlstick->GetRawButton(12)){ lift->Set(false); liftdown->Set(true); } else if (controlstick->GetRawButton(7)) { liftdown->Set(false); } if (controlstick->GetRawButton(9)) { winch->Set(scaleIntake); } else if (controlstick->GetRawButton(10)) { winch->Set(-scaleIntake); } else { winch->Set(0); } if (controlstick->GetPOV() == 0 && !bounce ) { constantLift -= 0.05; bounce = true; } else if (controlstick->GetPOV() == 180 && !bounce) { constantLift += 0.05; bounce = true; } else if (controlstick->GetPOV() == 270 && !bounce) { constantLift = 0; bounce = true; } else { bounce = false; } UpdateDashboard(); }