void TeleopPeriodic() { if(m_driver->GetRawButton(BUTTON_LB)) { // PYRAMID m_PIDController->SetSetpoint(PLATE_PYRAMID_THREE_POINT); m_PIDController->Enable(); } else if(m_driver->GetRawButton(BUTTON_RB)) { // FEEDER m_PIDController->SetSetpoint(PLATE_FEEDER_THREE_POINT); m_PIDController->Enable(); } else if(m_driver->GetRawAxis(TRIGGERS) > 0.5) { m_PIDController->SetSetpoint(PLATE_TEN_POINT_CLIMB); m_PIDController->Enable(); } else { // MANUAL CONTROL m_PIDController->Disable(); m_plate1->Set(-deadband(m_driver->GetRawAxis(LEFT_Y))); m_plate2->Set(-deadband(m_driver->GetRawAxis(LEFT_Y))); } // ----- PRINT ----- SmartDashboard::PutNumber("Plate Position: ", m_plateSensor->GetVoltage()); SmartDashboard::PutNumber("PID GET: ", m_plateSensor->PIDGet()); } // TeleopPeriodic()
void TeleopPeriodic() { myRobot.ArcadeDrive(-gPad.GetRawAxis(GP_RSTICK_YAXIS)*ROBOT_START_SPEED, -gPad.GetRawAxis(GP_RSTICK_XAXIS)*ROBOT_START_SPEED ); // drive with arcade style (use right stick) if(gPad.GetRawButton(GP_Y_BUTTON) == true && yButton == false) { if(driveSpeed + 0.25 < 1 && driveSpeed - 0.25 > 0) { yButton = true; driveSpeed = driveSpeed + 0.25; } else { yButton = false; } } if(gPad.GetRawButton(GP_A_BUTTON) == true && aButton == false) { if(driveSpeed + 0.25 < 1 && driveSpeed - 0.25 > 0) { aButton = true; driveSpeed = driveSpeed + 0.25; } else { aButton = false; } } }
void ArcadeDriveWithJoystick::Execute() { Joystick* stick = CommandBase::oi->GetJoystick(); float moveValue = stick->GetRawAxis(1); float rotateValue = stick->GetRawAxis(4); CommandBase::drivetrain->Drive(moveValue, rotateValue); CommandBase::drivetrain->Log(); }
/** * Runs the motors with arcade steering. */ void OperatorControl(void) { while (IsOperatorControl()) { left.Set(cont.GetRawAxis(2)); right.Set(cont.GetRawAxis(4)); } }
//void StartAutomaticCapture(std::shared_ptr<USBCamera>cam0); void TeleopPeriodic() { // ui.GetData(&wui); // m_tank.Drive(wui.LeftSpeed, wui.RightSpeed); // // m_shooter.Rotate(wui.RotateSpeed*3); //70 degrees per second at full value // m_shooter.Lift(wui.LiftSpeed*1.193); //4 seconds for 180 degree revolution // if(wui.SpinUp) { // m_shooter.Spinup(1); // } // if(wui.Shoot) { // m_shooter.Shoot(); // } // if(wui.Pickup) { // m_shooter.Pickup(); // } // // m_suspension.SetFrontLeft(wui.DropFL); // m_suspension.SetBackLeft(wui.DropBL); // m_suspension.SetFrontRight(wui.DropFR); // m_suspension.SetBackRight(wui.DropBR); // m_leddar.GetDetections(); // m_shooter.Update(); //float RTrigger = m_lStick->GetRawAxis(3); //float LTrigger = m_lStick->GetRawAxis(2); //if (m_PWMTalonLeftFrontTop == .5) //if (abs(RTrigger) < 0.2) //RTrigger = 0; //if (abs(LTrigger) < 0.2) //LTrigger = 0; float leftSpeed = m_lStick->GetRawAxis(1); float rightSpeed = m_lStick->GetRawAxis(5); if (abs(leftSpeed) < 0.2) leftSpeed = 0; if (abs(rightSpeed) < 0.2) rightSpeed = 0; //float LTrigger = m_lStick->GetRawAxis(3); //float RTrigger = m_lStick->GetRawAxis(2); SmartDashboard::PutNumber("Left Stick", leftSpeed); SmartDashboard::PutNumber("Right Stick", rightSpeed); //SmartDashboard::PutNumber("L Trigger", LTrigger); //SmartDashboard::PutNumber("R Trigger", RTrigger); SmartDashboard::PutNumber("Left Encoder", leftEncoder->Get()); SmartDashboard::PutNumber("Right Encoder", rightEncoder->Get()); drive->TankDrive(leftSpeed, rightSpeed, true); //drive->TankDrive(RTrigger, LTrigger, true); LEFTDRIVE1->Set(leftSpeed); LEFTDRIVE2->Set(leftSpeed); RIGHTDRIVE1->Set(rightSpeed); RIGHTDRIVE2->Set(rightSpeed); //m_PWMTalonLeftFrontTop->Set(RTrigger); //m_PWMTalonRightFrontTop->Set(RTrigger); //m_PWMTalonRightRearTop->Set(LTrigger); //m_PWMTalonLeftRearTop->Set(LTrigger); }
void TeleopContinuous(void) { printf("Running in teleop continuous...\n"); GetWatchdog().Feed(); //Drive the robot drivetrain->ArcadeDrive(driverJoystick->GetRawAxis(2) * -1,driverJoystick->GetRawAxis(4) * -1); //Run the kicker kicker->Act(); }
void OperatorControl() { tank.StartDrive(); kick.StartKicker(); while (IsOperatorControl()) { comp.checkCompressor(); tank.Drive(PrimaryController.GetRawAxis(LEFT_JOYSTICK), PrimaryController.GetRawAxis(RIGHT_JOYSTICK), (int)PrimaryController.GetRawButton(BUTTON_HIGH_DRIVE_SHIFT), (int)PrimaryController.GetRawButton(BUTTON_LOW_DRIVE_SHIFT)); kick.StateMachine(PrimaryController.GetRawButton(BUTTON_KICK)); collect.runCollector(PrimaryController.GetRawButton(BUTTON_ROLLER_ON)); } }
void OperatorControl(void) { while(!IsDisabled()) { GetWatchdog().Feed(); float speed = stick.GetRawAxis(2); float strafe = -1*stick.GetRawAxis(1); float turn = -1*stick.GetRawAxis(3); Dlf->Set(speed + turn + strafe); Dlb->Set(speed + turn - strafe); Drf->Set(-speed + turn + strafe); Drb->Set(-speed + turn - strafe); Wait(.05); } }
void Intake::move (const Joystick& joystick) { float left = joystick.GetRawAxis (OI::kIntakeTake); float right = joystick.GetRawAxis (OI::kIntakeGive); if (left > right) move (left); else if (right > left) move (right * -1); else move (0); }
virtual void TeleopPeriodic() { rightDrive->SetSpeed(-(Driver->GetRawAxis(2))); leftDrive->SetSpeed((Driver->GetRawAxis(5))); shooterFWD->SetSpeed(-(Operator->GetRawAxis(2))); shooterRear->SetSpeed(-(Operator->GetRawAxis(2))); //shoioter angle if(Operator->GetRawButton(5)) { cout<<"Relay 1 forward"<<endl; shooterAngle->Set(Relay::kForward); } if(Operator->GetRawButton(6)) { cout<<"Relay 1 Reverse"<<endl; shooterAngle->Set(Relay::kReverse); } //Fire button if(Operator->GetRawButton(1)) { cout<<"Relay 1 forward"<<endl; shooterFire->Set(Relay::kForward); } if(Operator->GetRawButton(2)) { cout<<"Relay 1 Reverse"<<endl; shooterFire->Set(Relay::kReverse); } if(CompressorSwitch->Get() == 0){ CompressorRelay->Set(Relay::kForward); }else{ CompressorRelay->Set(Relay::kOff); } //if(canPDP == 0){ // cout << "NULL" << endl; //}else{ //canPDP->GetVoltage(Voltage) ; //cout << "0" << endl; //} }
/* gets stick values into the x y and z values, these values are not returned by this but rather stored into the respective variables, the values are filtered*/ void RawControl::getStickValues(float &x, float &y, float &z) { x=(fabs(stick->GetRawAxis(1))<.3 ? 0 : stick->GetRawAxis(1)); y=(fabs(stick->GetRawAxis(2))<.3 ? 0 : stick->GetRawAxis(2)); z=(fabs(stick->GetRawAxis(3))<.3 ? 0 : stick->GetRawAxis(3)); x=x*x*x*x*x*x*x; y=y*y*y*y*y*y*y; z*=.9; z=z*z*z; //y*=.9; //stick->GetRawButton(2) ? z=0 : z=z; //theres a lot more filtering of axes after this....but with comments on top of comments i dont remember what actually worked. Any other //manipulation of joystick input for x-y-z axes goes here. }
void Print () { if (PrintTime.Get() > PRINT_TIME) { lcd->Clear(); lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Left Speed = %5.4f", PrimaryController.GetRawAxis(LEFT_JOYSTICK)); lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Right Speed = %5.4f", PrimaryController.GetRawAxis(RIGHT_JOYSTICK)); lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Charge State = %d", (int)Shooter.chargestate); //lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Collector speed= %d", Collector.CollectorSpeed()); lcd->UpdateLCD(); PrintTime.Reset(); PrintTime.Start(); } }
/** * Runs the motors with arcade steering. */ void OperatorControl() { myRobot.SetSafetyEnabled(true); while (IsOperatorControl() && IsEnabled()) { leftIntake.Set(0.8); rightIntake.Set(0.8); leftLift.Set(operatorStick.GetRawAxis(1)); rightLift.Set(operatorStick.GetRawAxis(1)); myRobot.TankDrive(driverStick.GetRawAxis(5), driverStick.GetRawAxis(1), true); // drive with arcade style (use right stick) Wait(0.005); // wait for a motor update time } }
void OperatorControl(void) { PrintTime.Start(); DriveTrain.StartDriveTrain(); Shooter.StartShooterTeleop(); while (IsOperatorControl()) { //Shooter.Safe = Collector.SafeToShoot(); DriveTrain.RunDriveTrain(PrimaryController.GetRawAxis(LEFT_JOYSTICK), PrimaryController.GetRawAxis(RIGHT_JOYSTICK), (int)PrimaryController.GetRawButton(BUTTON_HIGH_DRIVE_SHIFT), (int)PrimaryController.GetRawButton(BUTTON_LOW_DRIVE_SHIFT)); //Collector.RunCollector(PrimaryController.GetRawButton(BUTTON_COLLECTOR_DEPLOY), PrimaryController.GetRawButton(BUTTON_COLLECTOR_RETRACT), PrimaryController.GetRawButton(BUTTON_COLLECT_OUT), PrimaryController.GetRawButton(BUTTON_COLLECT_IN), PrimaryController.GetRawButton(BUTTON_COLLECT_STOP)); //Shooter.RunShooter(SecondaryController.GetRawButton(BUTTON_PRIME_SHOOTER), SecondaryController.GetRawButton(BUTTON_GOAL_SHOT), SecondaryController.GetRawButton(BUTTON_TRUSS_SHOT)); Shooter.RunPracticeShooter(SecondaryController.GetRawAxis(LEFT_JOYSTICK),SecondaryController.GetRawButton(BUTTON_PRIME_SHOOTER), SecondaryController.GetRawButton(BUTTON_GOAL_SHOT)); Print(); } }
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()); }
/** * Get the value of the axis. * * @param port The USB port for this joystick. * @param axis The axis to read [1-6]. * @return The value of the axis. */ float GetRawAxis(UINT32 port, UINT32 axis) { Joystick *stick = getJoystick(port); if (stick == NULL) return 0; return stick->GetRawAxis(axis); }
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); } }
void TeleopPeriodic() { while(IsOperatorControl() && IsEnabled()) { // FrontL->Set(.5*Driver->GetRawAxis(0)+.5*Driver->GetRawAxis(2)); //FrontR->Set(.5*Driver->GetRawAxis(0)-.5*Driver->GetRawAxis(2)); RearL->Set(.5*Driver->GetRawAxis(0)+.5*Driver->GetRawAxis(2)); RearR->Set(.5*Driver->GetRawAxis(0)-.5*Driver->GetRawAxis(2)); /**Manipulator->Set(.5*Operator->GetRawAxis(1));*/ /** Takes axes value of driver's and operator's joysticks to recieve values that the motors will be set to. * Pushing left stick forward and backwards on driver controller causes all motors to move forward and backwards, respectively. * Pushing right stick left on driver controller causes right motors to speed up making the robot turn left * Pushing right stick right on driver controller causes left motors to speed up making the robot turn right * Pushing left stick forward and backwards causes the arm to mover up and down, respectively. */ } }
/** * @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(void) { // increment the number of teleop periodic loops completed m_telePeriodicLoops++; GetWatchdog().Feed(); // if(autoPilot == true) // { // Auto Align Disable Button // if(operatorGamepad->GetButton(Joystick::kTopButton) == 2) // { // Goal_Align_PID->Disable(); // Stop outputs // Goal_Align_PID->Enable(); // Start PIDContoller up again // Goal_Align_PID->SetSetpoint(0.0); // autoPilot = false; // } // } // else // { // Calculate jaguar output based on exponent we pass from SmartDashboard double leftOutput, rightOutput; leftOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(2)); rightOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(5)); m_robotDrive->SetLeftRightMotorOutputs(leftOutput, rightOutput); if(operatorGamepad->GetRawButton(1) && !buttonWasDown) { printf("LEFT ENCODER: %f\n", Front_L->GetPosition()); printf("RIGHT ENCODER: %f\n", Front_R->GetPosition()); } buttonWasDown = operatorGamepad->GetRawButton(1); // Auto Align Button // if(operatorGamepad->GetButton(Joystick::kTopButton) == 1) // { // // Turn Auto Align on if we see a goal and we know the azimuth // if(SmartDashboard::GetBoolean(FOUND_KEY) == true) // { // Goal_Align_PID->SetSetpoint(SmartDashboard::GetNumber(AZIMUTH_KEY)); // autoPilot = true; // } // } // } }
void TeleopPeriodic() { while(1) { fps->SetLeftRightMotorOutputs(0.5*-drivercontroller->GetRawAxis(1) + drivercontroller->GetRawAxis(2), 0.5*-drivercontroller->GetRawAxis(1) - drivercontroller->GetRawAxis(2)); if(operatorcontroller->GetRawAxis(1) > 0) { armminipluator->Set(0.6*operatorcontroller->GetRawAxis(1)); } else if(operatorcontroller->GetRawAxis(1) < 0) { armminipluator->Set(0.3*operatorcontroller->GetRawAxis(1)); } if(operatorcontroller->GetRawButton(5)) { shro->Set(1); } else if(operatorcontroller->GetRawAxis(6)) { shro->Set(-1); } else { shro->Set(0); } } }
void BackgroundGather::Execute() { Joystick *stick = Robot::oi->getDriveStick(); if(fabs(stick->GetRawAxis(Joystick::kThrottleAxis))>0.5) { double speed = SmartDashboard::GetNumber("GatherSpeed"); if (Robot::gatherer->motor) { Robot::gatherer->motor->Set(speed); } } }
void Capturer::update(Joystick &controller) { float speed = controller.GetRawAxis(3); if (speed < 0) { speed = -speed; } std::cout << speed << '\n'; m_motor.Set(speed); }
void Drive::arcadeDrive(Joystick stick){ double power = 0; //This is the power variable based on the left y axis double turn = 0; //This is the turn variable based on right x axis if((stick.GetRawAxis(2) < -.1 )||(stick.GetRawAxis(2) > .1)){ //Deadzone for the left y axis power = .45*stick.GetRawAxis(2); printf("The Thrust is " + power); } else { power = 0; } if((stick.GetRawAxis(4) < -.1 )||(stick.GetRawAxis(4) > .1)){ //Deadzone for the right x axis turn = .45*stick.GetRawAxis(4); printf("The Turn is " + turn); } else { turn = 0; } if (Drive.slowSpeed){ printf("Speed is slowed"); power = power*.45/.75; turn = turn*.45/.75; } rDrive = (power - turn); lDrive = (power + turn); }
void OperatorControl(void) { char count=0; double target = 0, speed = 0; while(!IsDisabled()) { double tmpStick = -1*stick.GetRawAxis(2); if(tmpStick < .2 && tmpStick > -.2) tmpStick=0; target += tmpStick*1.5; int location = enc.GetRaw(); if(stick.GetRawButton(5)) { up.Set(true); down.Set(false); }else if(stick.GetRawButton(7) && location > 0) { down.Set(true); up.Set(false); }else if(stick.GetRawButton(8)) { down.Set(true); up.Set(false); }else if(stick.GetRawButton(9)){ speed = pid(target, location); if(speed > 1) { up.Set(true); down.Set(false); }else if(speed < -1) { up.Set(false); down.Set(true); }else{ up.Set(false); down.Set(false); } }else if(stick.GetRawButton(10)) { enc.Reset(); }else{ up.Set(false); down.Set(false); } if(stick.GetRawButton(1)) target = 2; if(stick.GetRawButton(4)) target = 400; if(stick.GetRawButton(3)) target = 200; if(stick.GetRawButton(2)) target = 70; Wait(.02); while(count++%30==0) cerr << location << '\t' << target << '\t' << speed << endl; } }
void TeleopPeriodic(void) { bool flag = true; //flag object initial declaration to ensure passing Piston toggle works properly float leftStick = gamePad->GetRawAxis(4); float rightStick = gamePad->GetRawAxis(2); bool rightBumper = gamePad->GetRawButton(6); bool leftBumper = gamePad->GetRawButton(5); bool buttonA = gamePad->GetRawButton(2); if(fabs(leftStick) >= 0.05 || fabs(rightStick >= 0.05)) { m_robotDrive->TankDrive(leftStick, rightStick); } else if(rightBumper || leftBumper) { if(rightBumper && !leftBumper) { ShiftHigh(); } else if(leftBumper && !rightBumper) { ShiftLow(); } } else if(buttonA && flag) { flag = false; passingPiston->Set(true); } else { if(!buttonA) { flag = false; MotorControlLeft(0.0); MotorControlRight(0.0); } else { MotorControlLeft(0.0); MotorControlRight(0.0); } } }
void OperatorControl(void) { //OperatorControlInit(); testTask.Start(); while (IsOperatorControl()) { //ProgramIsAlive(); //No need to do waits because ProgramIsAlive function does a wait. //Wait(0.005); bool isButtonPressed = stick.GetRawButton(3); if (isButtonPressed) { int taskCounter = testTask.ReturnCounter(); SmartDashboard::PutNumber("Task Counter",taskCounter); } 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); } }
void shootingModes(){ if (manShootMode == 0){ updateShooter(); } if (manShootMode == 1){ if (stick2.GetRawAxis(triggerR) > 0){ t_motor.Set(scaler(-1*(stick2.GetRawAxis(triggerR)))); } else if (stick2.GetRawAxis(triggerL) > 0){ t_motor.Set(scaler(stick2.GetRawAxis(triggerL))); } else if (stick2.GetRawAxis(triggerL) == 0 and stick2.GetRawAxis(triggerR) == 0){ t_motor.Set(0); } if (stick2.GetRawButton(buttonX) == true){ myServo->SetAngle(175); } if (stick2.GetRawButton(buttonX) == false){ myServo->SetAngle(5); } } }
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 DoctaEight::OperatorControl(void) { REDRUM; LTop.SetSafetyEnabled(0); LTop.EnableControl(); LBot.SetSafetyEnabled(0); LBot.EnableControl(); while (IsOperatorControl()) { REDRUM; output(); if (pilot.GetRawButton(6)) { arm.Set (.3); } else if (pilot.GetRawButton(5)) { arm.Set (-.3); } else arm.Set (0); //Set Shooter state and reset RPMoffset if necessary if (copilot.GetRawButton(1)) //BUTTON A { REDRUM; if(ShooterState != 1) { REDRUM; ShooterState = 1; RPMoffset = 0; } } else if (copilot.GetRawButton(2)) //BUTTON B { REDRUM; if(ShooterState != 2) { REDRUM; ShooterState = 2; RPMoffset = 0; } } else if (copilot.GetRawButton(3)) //BUTTON X { REDRUM; if(ShooterState != 3) { REDRUM; ShooterState = 3; RPMoffset = 0; } } else if (copilot.GetRawButton(4)) //BUTTON Y { REDRUM; if(ShooterState != 4) { REDRUM; ShooterState = 4; RPMoffset = 0; } } //increment or decrement RPMoffset if(copilot.GetRawButton(5)) //BUTTON LB { REDRUM; FlagB5 = true; } else if(copilot.GetRawButton(6)) //BUTTON RB { REDRUM; FlagB6 = true; } if(FlagB5 == true && copilot.GetRawButton(5) == false) { REDRUM; RPMoffset -= 50; FlagB5 = false; } else if(FlagB6 && !copilot.GetRawButton(6)) { REDRUM; RPMoffset += 50; FlagB6 = false; } if (pilot.GetRawButton(1) && !cycle) { cycle = 1; negate*=-1; } else { cycle=0; } //prepare shooter/launcher ouput speed if(ShooterState == 1) { REDRUM; //BUTTON A LTopSpeed = MAX/12; LBotSpeed = RPMMid + RPMoffset; } else if(ShooterState == 2) { //BUTTON B REDRUM; LTopSpeed = MAX/12; LBotSpeed = RPMLow + RPMoffset; } else if(ShooterState == 3) { //BUTTON X REDRUM; LTopSpeed = MAX/12; LBotSpeed = RPMHigh + RPMoffset; } else if(ShooterState == 4) { //BUTTON Y REDRUM; LTopSpeed = 0; LBotSpeed = 0; } //set shooter launcher speed to CANJags LTop.Set(LTopSpeed); LBot.Set(-LBotSpeed); //move simple platform arm leftyrighty(-pilot.GetY(), -pilot.GetRawAxis(4)); intake.Set(-copilot.GetY()); } //stops encoders }