void TeleopPeriodic() { //printf("I done it\n"); //put the values of both the solenoids on SmartDashboard SmartDashboard::PutBoolean("Solenoid One", singOne->GetState()); SmartDashboard::PutBoolean("Solenoid Two", singTwo->GetState()); // Piston One goes out when any of the top buttons are pressed if (GetTopButtons() == true) { singOne->OverrideEnable(); printf("Piston One Out\n"); } // if none of the top buttons are pressed then Piston One comes in else if(GetTopButtons() == false) { singOne->OverrideDisable(); } // Piston Two goes out if Piston One is out and the trigger is pulled if (stick->GetTrigger() == true && GetTopButtons()==true) { singTwo->OverrideEnable(); printf("Piston Two Out\n"); } //If the trigger is not pulled or none of the top buttons are pushed Piston Two comes in else if (stick->GetTrigger() == false || GetTopButtons()== false) { singTwo->OverrideDisable(); } }
/** * Read the state of the trigger on the joystick. * * Look up which button has been assigned to the trigger and read its state. * * @param port The USB port for this joystick. * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface. * @return The state of the trigger. */ bool GetTrigger(UINT32 port, JoystickHand hand) { Joystick *stick = getJoystick(port); if (stick == NULL) return 0; return stick->GetTrigger((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 OperatorControl(void) { GetWatchdog().SetEnabled(true); printf("Entered OperatorControl\n"); while (IsOperatorControl()) { GetWatchdog().Feed(); // use the trigger to start recording.. at the moment, // it just gets ignored if you call it more than once if (stick.GetTrigger()) recorder.StartRecording(); myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick) // always call the recording routine recorder.Record(); } }
void TeleopPeriodic(void) { // increment the number of teleop periodic loops completed GetWatchdog().Feed(); m_telePeriodicLoops++; /* * No longer needed since periodic loops are now synchronized with incoming packets. if (m_ds->GetPacketNumber() != m_priorPacketNumber) { */ /* * Code placed in here will be called only when a new packet of information * has been received by the Driver Station. Any code which needs new information * from the DS should go in here */ m_dsPacketsReceivedInCurrentSecond++; // increment DS packets received // put Driver Station-dependent code here // Demonstrate the use of the Joystick buttons // DemonstrateJoystickButtons(m_rightStick, m_rightStickButtonState, "Right Stick", &m_solenoids[1]); // DemonstrateJoystickButtons(m_leftStick, m_leftStickButtonState, "Left Stick ", &m_solenoids[5]); /*** // determine if tank or arcade mode, based upon position of "Z" wheel on kit joystick if (m_rightStick->GetZ() <= 0) { // Logitech Attack3 has z-polarity reversed; up is negative // use arcade drive m_robotDrive->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick) if (m_driveMode != ARCADE_DRIVE) { // if newly entered arcade drive, print out a message printf("Arcade Drive\n"); m_driveMode = ARCADE_DRIVE; } } else { // use tank drive m_robotDrive->TankDrive(m_leftStick, m_rightStick); // drive with tank style if (m_driveMode != TANK_DRIVE) { // if newly entered tank drive, print out a message printf("Tank Drive\n"); m_driveMode = TANK_DRIVE; } } ***/ /* Grab z-wheel value and transform from [1, -1] to [0,4600] * 4600 rpm is a guess */ float rawZ, transformedZ; rawZ = m_leftStick->GetZ(); // transformedZ = (1.0 - rawZ)/(-2.0); transformedZ = -2300.0 * rawZ + 2300.0; /* Driver station display output. */ char msg[256]; static DriverStationLCD *dsLCD = DriverStationLCD::GetInstance(); sprintf(msg, "Launcher Speed = %f RPM", transformedZ); dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, msg); sprintf(msg, "Loader Limit = %u", m_loaderLimit->Get()); dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, msg); sprintf(msg, "Loader Trigger = %u", m_leftStick->GetTrigger()); dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, msg); // line number (enum), starting col, format string, args for format string ... dsLCD->UpdateLCD(); // use arcade drive //m_driveGain = (1.0 - m_rightStick->GetZ())/(-2.0); m_robot->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick) // m_robotFrontDrive->Drive(-m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn // m_robotRearDrive->Drive(m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn // -Add an elevation control, add a motor controller to port 8, and add a joystick button pair to control up/down if (m_rightStick->GetRawButton(m_elevatorUpButton)){ m_elevatorMotor->Set(m_elevatorUpSpeed); } else if (m_rightStick->GetRawButton(m_elevatorDownButton)){ m_elevatorMotor->Set(m_elevatorDownSpeed); } else { m_elevatorMotor->Set(0.0); } // trigger button activates feeding and loading mechanisms if ((m_leftStick->GetTop() || m_feeding) && (!m_loading) && (m_feedCounter <= m_feedCount)){ m_feeding = true; m_feedCounter++; m_feedMotor->Set(m_feedSpeed); } else { m_feeding = false; m_feedCounter = 0; m_feedMotor->Set(0.); } m_loaderLimit->Get(); //just testing digital input port 1 // top(2) button uses the Z wheel to control the speed of the loading mechanisms if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (m_loadCounter <= m_loadCount)){//comment out this line // if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (bool) m_loaderLimit->Get()){ //uncomment this line m_loading = true; m_loadCounter++; // comment out this line m_loadMotor->Set(m_loadSpeed); // load breech } else if ((m_loading && (!m_feeding)) && (m_loadCounter > m_loadCount) && (m_loadCounter <= ((m_loadCount+m_loadPauseCount)))) { // comment out the pause logic m_loading = true;// comment out the pause logic m_loadCounter++;// comment out the pause logic, m_loadMotor->Set(0.); // pause loader // comment out the pause logic } // else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loadCounter<= 2*m_loadCount-13)) { // comment out this line else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loaderLimit->Get()!=0)) { //Retracting loader arm (feeder), have not yet hit the limit switch. // else if ((m_loading &! m_feeding) && (bool) m_loaderResetLimit->Get()) { // uncomment this line m_loading = true; m_loadCounter++; // comment out the pause logic m_loadMotor->Set(-m_loadSpeed);; // reset loader } else { //Finished retracting loader arm (feeder), so stop it. m_loading = false; m_loadCounter = 0; m_loadMotor->Set(0.);; // stop loader } // Set launcher motor command m_launchMotor->Set(-(1.0 - m_leftStick->GetZ())/(-2.0)); // transform from [-1.0, +1.0] to [0.0, +1.0] /* } // if (m_ds->GetPacketNumber()... */ // use buttons 8 and 9 to trim load motor position in maintenance mode /*if (m_leftStick->GetRawButton(8) && (!m_feeding) && (!m_loading)){ m_loadMotor->Set(0.12); } else if (m_leftStick->GetRawButton(9) && (!m_feeding) && (!m_loading)){ m_loadMotor->Set(-0.1); }*/ //Use buttons 10 and 11 to trim feed motor position in maintenance mode if (m_leftStick->GetRawButton(11) && (!m_feeding) && (!m_loading)){ m_feedMotor->Set(0.15); } else if (m_leftStick->GetRawButton(10) && (!m_feeding) && (!m_loading)){ m_feedMotor->Set(-0.1); } } // TeleopPeriodic(void)
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(); } }