void driveBackward() //Creates a function to drive backward (sets it) { leftFront.Set(-0.75); leftBack.Set(-0.75); rightFront.Set(-0.75); rightBack.Set(-0.75); }
void stopDriving() { leftFront.Set(0); leftBack.Set(0); rightFront.Set(0); rightBack.Set(0); }
inline void grabberPositionTaskFunc(uint32_t joystickPtr, uint32_t grabTalonPtr, uint32_t grabInnerLimitPtr, uint32_t pdpPtr, uint32_t backOutPtr, uint32_t grabPowerPtr, uint32_t isGrabbingPtr...) {//uint is a pointer and not an integer Joystick *joystick = (Joystick *) joystickPtr;//initializes objects from pointers Talon *grabTalon = (Talon *) grabTalonPtr; Switch *grabInnerLimit = (Switch *) grabInnerLimitPtr; PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr; bool *isGrabbing = (bool *) isGrabbingPtr; bool *backOut = (bool *) backOutPtr; double *grabPower = (double *) grabPowerPtr; Timer timer; timer.Start(); *isGrabbing = true;//tells robot.cpp that thread is running while (grabInnerLimit->Get() && timer.Get() < Constants::grabDelay) {//starts to spin motor to pass startup current grabTalon->Set(1);//move in } while (pdp->GetCurrent(Constants::grabPdpChannel) < *grabPower && grabInnerLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it hasn't reached the current cutoff, hit a limit switch, or been cancelled grabTalon->Set(1); SmartDashboard::PutNumber("Current",pdp->GetCurrent(Constants::grabPdpChannel));//displays current on SmartDashboard } if (*backOut) { grabTalon->Set(0);//stop moving timer.Reset(); while (timer.Get() < Constants::liftBackoutTime && joystick->GetRawButton(Constants::pickupCancelButton) == false) { grabTalon->Set(-.75); } } grabTalon->Set(0);//stop moving timer.Stop(); *isGrabbing = false;//tells that thread is over }
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); } }
void driveForward() //Drives forward { leftFront.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive leftBack.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive rightFront.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be negative rightBack.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be negative }
/**** Yo yo yo the right side motors are inverted, so a positive left motor (probably) moves forward and a positive right motor (probably) moves backward ****/ void stopDriving() // Stops drive motors { leftFront.Set(0); leftBack.Set(0); rightFront.Set(0); rightBack.Set(0); }
void TestBGrabber() { //ROLLERS if (m_operator->GetRawAxis(TRIGGERS) > 0.4) { m_roller->Set(0.5); } else if (m_operator->GetRawAxis(TRIGGERS) < -0.4) m_roller->Set(0.5); else { m_roller->Set(0.0); } //BALL CATCH (#Sweg) if (m_operator->GetRawButton(BUTTON_A)) { m_catch->Set(true); } else if (m_operator->GetRawButton(BUTTON_B)) { m_catch->Set(false); } //bArm OPEN / CLOSE if (m_operator->GetRawButton(BUTTON_X)) { m_bArm->Set(true); } else if (m_operator->GetRawButton(BUTTON_Y)) { m_bArm->Set(false); } }
void driveForward() //Creates a function to drive forward (sets it) { leftFront.Set(0.75); leftBack.Set(0.75); rightFront.Set(0.75); rightBack.Set(0.75); }
void TestRamMotion() { if (m_driver->GetRawButton(BUTTON_LB)) m_ramMotor->Set(.8); else if (m_driver->GetRawButton(BUTTON_RB)) m_ramMotor->Set(-.2); else m_ramMotor->Set(0); }
void RobotDemo::spinnerRight(float speed) { float sec = 0.0; float currentMotorPower = 0.0; if(speed<0.05 && speed>-0.05){ stopSpinnersRight(); } else if(speed>0){ while(currentMotorPower<speed){ spinnerR1->Set(currentMotorPower); spinnerR2->Set(currentMotorPower); sec += 0.1; currentMotorPower = ramp(sec); } spinnerR1->Set(speed); //may need to spinnerR2->Set(speed); //reverse these } else{ while(currentMotorPower<abs(speed)){ spinnerR1->Set(-currentMotorPower); spinnerR2->Set(-currentMotorPower); sec += 0.3; currentMotorPower = ramp(sec); } spinnerR1->Set(speed); //may need to spinnerR2->Set(speed); //reverse these } }
void Reset() { m_talonCounter->Reset(); m_victorCounter->Reset(); m_jaguarCounter->Reset(); m_talon->Set(0.0f); m_victor->Set(0.0f); m_jaguar->Set(0.0f); }
/** * Runs the motors with arcade steering. */ void OperatorControl() { myRobot.SetSafetyEnabled(true); while (IsOperatorControl() && IsEnabled()) { myRobot.ArcadeDrive(joystick); // drive with arcade style (use right stick) if(joystick.GetRawButton(1)) { crochets.Set(-0.6); } else if(joystick.GetRawButton(2)) { crochets.Set(0.6); } Wait(0.005); // wait for a motor update time } }
/** * 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. } }
void turnLeftMore() //Creates a function to turn the robot a larger set amount left and then stops movement. { leftFront.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousFastDriveSpeedMultiplier))); // Probably needs to be negative leftBack.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousFastDriveSpeedMultiplier))); // Probably needs to be negative rightFront.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousFastDriveSpeedMultiplier))); // Probably needs to be negative rightBack.Set(justIncaseMyPolarityDunGoofed*(-1 + (autonomousFastDriveSpeedMultiplier))); // Probably needs to be negative Wait(1); leftFront.Set(0); leftBack.Set(0); rightFront.Set(0); rightBack.Set(0); }
void turnRight() //Creates a function to turn the robot a set amount right and then stops movement { leftFront.Set(0.5); leftBack.Set(0.5); rightFront.Set(-0.5); rightBack.Set(-0.5); Wait(1.5); leftFront.Set(0); leftBack.Set(0); rightFront.Set(0); rightBack.Set(0); }
void turnRight() //Creates a function to turn the robot a set amount right and then stops movement { leftFront.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive leftBack.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive rightFront.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive rightBack.Set(justIncaseMyPolarityDunGoofed*(1 - (autonomousSlowDriveSpeedMultiplier))); // Probably needs to be positive Wait(0.75); leftFront.Set(0); leftBack.Set(0); rightFront.Set(0); rightBack.Set(0); }
void TeleopPeriodic() { double throttle; bool BuddyBoxEnabled = BuddyBoxEnableChooser->GetSelected(); bool SlaveInControl = MasterInterLink->GetCh5(); SmartDashboard::PutBoolean( "Slave In Control", BuddyBoxEnabled ? SlaveInControl : false ); bool SlaveControlsSpeed = SlaveSpeedControlChooser->GetSelected(); if( BuddyBoxEnabled && SlaveInControl ) ActiveInterLink = SlaveInterLink; else ActiveInterLink = MasterInterLink; if( SlaveInControl && SlaveControlsSpeed ) throttle = SlaveInterLink->getCh6(); else throttle = MasterInterLink->getCh6(); double aile = ActiveInterLink->getAile(); double elev = ActiveInterLink->getElev(); double rudd = ActiveInterLink->getRudd(); SmartDashboard::PutNumber( "Rudder", rudd ); SmartDashboard::PutNumber( "Throttle", throttle ); throScale = throttle + 1; double driveAngle = atan2( -aile*aileScale, elev*elevScale ); SmartDashboard::PutNumber( "Drive Angle", driveAngle ); double driveSpeed = sqrt( aile*aile + elev*elev ); SmartDashboard::PutNumber( "Drive Speed", driveSpeed ); frontLeft->Set( (float)( throScale * frontLeftSpeed * ( driveSpeed * sin( frontLeftAngle-driveAngle ) + ruddScale * rudd ) ) ); backLeft->Set( (float)( throScale * backLeftSpeed * ( driveSpeed * sin( backLeftAngle-driveAngle ) + ruddScale * rudd ) ) ); frontRight->Set( (float)( throScale * frontRightSpeed * ( driveSpeed * sin( frontRightAngle-driveAngle ) + ruddScale * rudd ) ) ); backRight->Set( (float)( throScale * backRightSpeed * ( driveSpeed * sin( backRightAngle-driveAngle ) + ruddScale * rudd ) ) ); }
void loadCatapult() { if (buttonOne.Get()==1 && buttonTwo.Get()==1 && dogSolenoid.Get()==DoubleSolenoid::kReverse) { dogSolenoid.Set(DoubleSolenoid::kForward); Wait(0.5); ratchetSolenoid.Set(DoubleSolenoid::kForward); Wait(0.5); catapultMotor.Set(1); } }
inline void lifterBrakeTaskFunc(uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t isBrakingPtr ...) {//uint is a pointer and not an integer Talon *liftTalon = (Talon *) liftTalonPtr; Encoder *liftEncoder = (Encoder *) liftEncoderPtr; Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr; Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr; bool *isBraking = (bool *) isBrakingPtr; PIDController pid(Constants::liftBrakeP, Constants::liftBrakeI, Constants::liftBrakeD, liftEncoder, liftTalon); Timer timer; //TODO enable and test out brake if (!Constants::liftBrakeIsEnabled) { return; } timer.Start(); while (timer.Get() < Constants::liftBrakeUpTime) { liftTalon->Set(Constants::liftBrakeUpPower); } liftTalon->Set(0); pid.Enable(); pid.SetSetpoint(liftEncoder->Get()); while (*isBraking) { if ((pid.Get() > 0 && liftLowerLimit->Get()) || (pid.Get() < 0 && liftUpperLimit->Get())){ liftTalon->Set(0); } else { liftTalon->Set(pid.Get()); } SmartDashboard::PutBoolean("Braking", true); } pid.Disable(); SmartDashboard::PutBoolean("Braking", false); liftTalon->Set(0); }
void autonomousCatapultRelease() { if ((leftLimitSwitch.Get()== 0 || rightLimitSwitch.Get()== 0) && winchMotor.Get() == 0) { stopDriving(); // Stops the drive so the robot doesn't flip on itself or something winchMotor.Set(0); // Redundant line for extra safety that can be removed after testing (The winch should already be off) dogSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the dog gear Wait(0.2); // Giving the pistons time to disengage properly ratchetSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the ratchet Wait(5); // Waits 5 seconds after shooting before starting to load the catapult } }
void turnRightMore() //Creates a function to turn the robot a larger set amount right and then stops movement. { leftFront.Set(1); leftBack.Set(1); rightFront.Set(-1); rightFront.Set(-1); Wait(1.5); leftFront.Set(0); leftBack.Set(0); rightFront.Set(0); rightBack.Set(0); }
inline void lifterPositionTaskFunc(uint32_t joystickPtr, uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t pdpPtr, uint32_t heightPtr, uint32_t isLiftingPtr ...) {//uint is a pointer and not an integer double *height = (double *) heightPtr;//initializes double Joystick *joystick = (Joystick *) joystickPtr; Talon *liftTalon = (Talon *) liftTalonPtr; Encoder *liftEncoder = (Encoder *) liftEncoderPtr; Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr; Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr; PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr; bool *isLifting = (bool *) isLiftingPtr; *isLifting = true;//tells robot.cpp that thread is running if (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height) {//checks to see if encoder is higher than it's supposed to be if (liftLowerLimit->Get() == false) {//starts to spin motor to pass startup current liftTalon->Set(1);//move down Wait(Constants::liftDelay); } while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftLowerLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too high and hasn't hit a limit switch or been cancelled SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder in SmartDashboard liftTalon->Set(.7);//move down } } else { if (liftUpperLimit->Get() == false) {//starts to spin motor to pass startup current liftTalon->Set(-1);//move up Wait(Constants::liftDelay); } while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftUpperLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too low and hasn't hit a limit switch or been cancelled SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder on SmartDashboard liftTalon->Set(-1);//move up } } liftTalon->Set(0);//stop *isLifting = false;//tells robot.cpp that thread is finished }
inline void setDriveMotors() { //cannot turn belts if the limit switch is pressed if(handlerState == BallHandlerState::up_off) { drive.Set(Relay::kOff); spinnermotor.Set(0); handlerposition.Set(Relay::kOff); } else if(handlerState == BallHandlerState::down_in_grab) { drive.Set(Relay::kReverse); handlerposition.Set(Relay::kOff); if(ballhandlerstick.GetRawButton(ARM_OUT_SPIN_BUTTON)) { spinnermotor.Set(-0.75f); } else { spinnermotor.Set(0.75f); } } else if(handlerState == BallHandlerState::down_out_shoot) { drive.Set(Relay::kForward); spinnermotor.Set(-0.75f); handlerposition.Set(Relay::kOff); } else if(handlerState == BallHandlerState::goingdown_off) { drive.Set(Relay::kReverse); spinnermotor.Set(0); handlerposition.Set(Relay::kReverse); } else if(handlerState == BallHandlerState::goingup_off) { drive.Set(Relay::kOff); spinnermotor.Set(0); handlerposition.Set(Relay::kForward); } else { spinnermotor.Set(0); drive.Set(Relay::kOff); handlerposition.Set(Relay::kOff); } }
inline void grabberChuteTaskFunc(uint32_t joystickPtr, uint32_t grabTalonPtr, uint32_t grabOuterLimitPtr, uint32_t grabInnerLimitPtr, uint32_t pdpPtr, uint32_t isGrabbingPtr...) {//uint is a pointer and not an integer SmartDashboard::PutBoolean("Breakpoint 0", true); Wait(.5); Joystick *joystick = (Joystick *) joystickPtr;//initializes objects from pointers Talon *grabTalon = (Talon *) grabTalonPtr; Switch *grabInnerLimit = (Switch *) grabInnerLimitPtr; Switch *grabOuterLimit = (Switch *) grabOuterLimitPtr; PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr; bool *isGrabbing = (bool *) isGrabbingPtr; Timer timer; timer.Start(); SmartDashboard::PutBoolean("Breakpoint 1", true); Wait(.5); *isGrabbing = true;//tells robot.cpp that thread is running SmartDashboard::PutBoolean("Breakpoint 2", true); Wait(.5); while (grabOuterLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//starts to spin motor to pass startup current //grabTalon->Set(1);//move in } SmartDashboard::PutBoolean("Breakpoint 3", true); Wait(.5); timer.Reset(); while (timer.Get() < Constants::grabChuteTime && grabInnerLimit->Get() && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it hasn't reached the current cutoff, hit a limit switch, or been cancelled //grabTalon->Set(1); SmartDashboard::PutNumber("Current",pdp->GetCurrent(Constants::grabPdpChannel));//displays current on SmartDashboard Wait(.5); } SmartDashboard::PutBoolean("Breakpoint 4", true); Wait(.5); grabTalon->Set(0);//stop moving timer.Stop(); *isGrabbing = false;//tells that thread is over }
/** * 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); }
/* Function MoveArmToSetPoint * moves the arm to a position on the arc * */ bool MoveArmToSetPoint(float setPoint, int16_t currentPosition){ //local variables //float setPoint = 0; //TODO - may need to scale the rotation speed based on how close we are to the setpoint //convert from degrees to potentiometer value //setPoint = ArmAngleLookUp1D(setPointDegrees); //determining how to drive motors if(currentPosition > (setPoint + kArmPositionTolerance)){ leftArmAngleMotor.Set(-1 * ARM_HOME_ROTATE_DIR * kArmRotateSpeed); rightArmAngleMotor.Set(ARM_HOME_ROTATE_DIR * kArmRotateSpeed); return(false); }else if(currentPosition < (setPoint - kArmPositionTolerance)){ leftArmAngleMotor.Set(ARM_HOME_ROTATE_DIR * kArmRotateSpeed); rightArmAngleMotor.Set(-1 * ARM_HOME_ROTATE_DIR * kArmRotateSpeed); return(false); }else{ leftArmAngleMotor.Set(kMotorStopped); rightArmAngleMotor.Set(kMotorStopped); return(true); } }
void Test() // DONT TOUCH THIS AREA. I KEEL YOU. { DriverStationLCD *screen = DriverStationLCD::GetInstance(); int counter = 0; bool solenoidTest=0; while (IsTest()) { if(logitech.GetRawButton(9)) //press rightBack { solenoidTest=1; compressor.Start(); } if(logitech.GetRawButton(10)) //press Start { solenoidTest=0; compressor.Stop(); } if(solenoidTest) { if(logitech.GetRawButton(1)) //press X { rightArmSolenoid.Set(DoubleSolenoid::kForward); leftArmSolenoid.Set(DoubleSolenoid::kForward); } else if(logitech.GetRawButton(2)) //press A { rightArmSolenoid.Set(DoubleSolenoid::kReverse); leftArmSolenoid.Set(DoubleSolenoid::kReverse); } else { leftArmSolenoid.Set(DoubleSolenoid::kOff); rightArmSolenoid.Set(DoubleSolenoid::kOff); } if(logitech.GetRawButton(3)) //PRess URTrigger { retrievalMotor.Set(logitech.GetRawAxis(2)); } else { retrievalMotor.Set(0); } if(logitech.GetRawButton(4)) { winchMotor.Set(logitech.GetRawAxis(2)); } else { winchMotor.Set(0); } if(logitech.GetRawButton(5)) { ratchetSolenoid.Set(DoubleSolenoid::kForward); } else if(logitech.GetRawButton(7)) { ratchetSolenoid.Set(DoubleSolenoid::kReverse); } else { ratchetSolenoid.Set(DoubleSolenoid::kOff); } if(logitech.GetRawButton(6)) { dogSolenoid.Set(DoubleSolenoid::kForward); } else if(logitech.GetRawButton(8)) { dogSolenoid.Set(DoubleSolenoid::kReverse); } else { dogSolenoid.Set(DoubleSolenoid::kOff); } } else { if(logitech.GetRawButton(1)) //Press X { rightFront.Set(logitech.GetRawAxis(2)); //Press left joystick } else { rightFront.Set(0); } if(logitech.GetRawButton(2)) //Press A { rightBack.Set(logitech.GetRawAxis(2)); } else { rightBack.Set(0); } if(logitech.GetRawButton(3)) //Press B { leftFront.Set(logitech.GetRawAxis(2)); } else { leftFront.Set(0); } if(logitech.GetRawButton(4)) //Press Y { leftBack.Set(logitech.GetRawAxis(2)); } else { leftBack.Set(0); } if(logitech.GetRawButton(5)) //Press ULTrigger { retrievalMotor.Set(logitech.GetRawAxis(2)); } else { retrievalMotor.Set(0); } if(logitech.GetRawButton(6)) //PRess URTrigger { winchMotor.Set(logitech.GetRawAxis(2)); } else { winchMotor.Set(0); } if(logitech.GetRawButton(7)) //Press LLTrigger { ringLight.Set(Relay::kForward); } else { ringLight.Set(Relay::kOff); } if(logitech.GetRawButton(8)) //Press LRTrigger { compressor.Start(); } else { compressor.Stop(); } } /****** MANUAL LOAD FUNCTION END *****/ screen -> PrintfLine(DriverStationLCD::kUser_Line1,"LeftJoystick: %f", logitech.GetRawAxis(2)); screen -> PrintfLine(DriverStationLCD::kUser_Line2,"RF:%f RB:%f LF:%f LB:%f", rightFront.Get(), rightBack.Get(), leftFront.Get(), leftBack.Get()); // Print WinchMotor State screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Solenoid Testing:%d", solenoidTest); screen -> PrintfLine(DriverStationLCD::kUser_Line4,"rightArmSolenoid:%d", rightArmSolenoid.Get()); screen -> PrintfLine(DriverStationLCD::kUser_Line5,"time:%d", counter); counter ++; Wait(0.005); // Waits to run the loop every 0.005 seconds so the cRIO doesn't explode screen->UpdateLCD(); } }
void OperatorControl() { compressor.Start(); // The class can run independently. This compressor will start and stop on its own to keep pressure under 120 PSI DriverStationLCD *screen = DriverStationLCD::GetInstance(); // Enables Driver Station Printing bool autoLoad = false; //autoLoad function retrievalMotor.Set(1); //Starts with retrieval wheels on while (IsOperatorControl()) { /****** AUTO LOAD TOGGLE START *****/ if(logitech.GetRawButton(10)) { if(autoLoad) { autoLoad = false; Wait(0.2); } else if(!autoLoad) { autoLoad = true; Wait(0.2); } } /****** AUTO LOAD TOGGLE END *****/ /** RETRIEVAL WHEELS: RED button (B) turns the arm wheels off, GREEN button (A) turns arm wheels on, BLUE button (X) turns the arm wheels in reverse**/ if(logitech.GetRawButton(1)) { retrievalMotor.Set(-1); } if(logitech.GetRawButton(3)) { retrievalMotor.Set(1); } if(logitech.GetRawButton(3)) { retrievalMotor.Set(0); } /** OPTIONS ON DEM WHEELZ END **/ /****** TANK DRIVE START ******/ if (fabs(logitech.GetRawAxis(2)) > driveStickBuffer) // If the left Y Axis on the Joystick is moved beyond the driveStickBuffer { if (logitech.GetRawButton(11)) { leftFront.Set(-1); leftBack.Set(-1); } else { leftFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction leftBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction } } else { leftFront.Set(0); // Turns Left Motor 1 off leftBack.Set(0); // Turns Left Motor 2 off } if (fabs(logitech.GetRawAxis(4)) > driveStickBuffer) // If the Y Axis on the Joystick is moved beyond the driveStickBuffer { if (logitech.GetRawButton(12)) { rightFront.Set(1); rightBack.Set(1); } else { rightFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier rightBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier } } else { rightFront.Set(0); // Turns Right Motor 1 off rightBack.Set(0); // Turns Right Motor 2 off } /****** TANK DRIVE END ******/ /****** BALL RETRIEVAL START ******/ /*CHECK THE POLARITY OF YOUR SOLENOIDS*/ if (logitech.GetRawButton(8)) //Press Lower Right Trigger to go down and STOP ball motor { leftArmSolenoid.Set(DoubleSolenoid::kReverse); // Pushes the pneumatic piston forward to lower the retrieval arm rightArmSolenoid.Set(DoubleSolenoid::kReverse); // Pushes the pneumatic piston forward to lower the retrieval arm if(retrievalMotor.Get()==1) { retrievalMotor.Set(0); // Toggles the motor that spins the wheel to bring in the ball } else { retrievalMotor.Set(1); } } else if (logitech.GetRawButton(6)) //Press Upper Trigger to go up and STOP ball motor { leftArmSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm rightArmSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm if(retrievalMotor.Get()==1) { retrievalMotor.Set(0); // Toggles the motor that spins the wheel to bring in the ball } else { retrievalMotor.Set(1); } } else { leftArmSolenoid.Set(DoubleSolenoid::kOff); // Does nothing to the retreival arm piston // rightArmSolenoid.Set(DoubleSolenoid::kOff); ////////////////////////////////////////////// } /****** BALL RETRIEVAL END ******/ /******* AUTOLOAD CATAPULT START ******/ // Limit Buttons not pressed, both dogSolenoid and ratchetSolenoids are disengaged if (autoLoad) { dogSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm winchMotor.Set(0.1); Wait(0.3); winchMotor.Set(0); ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to lower the retrieval arm Wait(0.1); while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1) { winchMotor.Set(1); /**ALLOW FOR WHILE LOOP TANK DRIVING**/ if (fabs(logitech.GetRawAxis(2)) > driveStickBuffer) // If the left Y Axis on the Joystick is moved beyond the driveStickBuffer { leftFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction leftBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction } else { leftFront.Set(0); // Turns Left Motor 1 off leftBack.Set(0); // Turns Left Motor 2 off } if (fabs(logitech.GetRawAxis(4)) > driveStickBuffer) // If the Y Axis on the Joystick is moved beyond the driveStickBuffer { rightFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier rightBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier } else { rightFront.Set(0); // Turns Motor 2 off rightBack.Set(0); // Turns Motor 2 off } /**END ALLOW FOR WHILE LOOP TANK DRIVING **/ if(logitech.GetRawButton(9)) //kills the winch { winchMotor.Set(0); } else { winchMotor.Set(1); } } winchMotor.Set(0); // Stops the Winch Motor since one or more buttons are pressed } /****** AUTOLOAD CATAPULT END ******/ /****** MANUAL LOAD CATAPULT START ******/ if (!autoLoad) { if (logitech.GetRawButton(5)) //pressing Upper Left Trigger loads { dogSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm winchMotor.Set(0.1); Wait(0.3); ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to lower the retrieval arm while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1) { winchMotor.Set(1); /**ALLOW FOR WHILE LOOP TANK DRIVING**/ if (fabs(logitech.GetRawAxis(2)) > driveStickBuffer) // If the left Y Axis on the Joystick is moved beyond the driveStickBuffer { leftFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction leftBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(2)*(-1)); // The left motors are facing in an opposite direction } else { leftFront.Set(0); // Turns Left Motor 1 off leftBack.Set(0); // Turns Left Motor 2 off } if (fabs(logitech.GetRawAxis(4)) > driveStickBuffer) // If the Y Axis on the Joystick is moved beyond the driveStickBuffer { rightFront.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier rightBack.Set((driveSpeedMultiplier) * logitech.GetRawAxis(4)); // Sets motor speed to Joystick position times the driveSpeedMultiplier } else { rightFront.Set(0); // Turns Motor 2 off rightBack.Set(0); // Turns Motor 2 off } if(logitech.GetRawButton(9)) //kills the winch { winchMotor.Set(0); } else { winchMotor.Set(1); } } winchMotor.Set(0); //shuts the winch motor off } } /****** MANUAL LOAD CATAPULT END ******/ /****** CATAPULT RELEASE START ******/ // Limit Buttons pressed, winchMotor is inactive, and lower left trigger on the joystick is pressed if ((leftLimitSwitch.Get()== 0 || rightLimitSwitch.Get()== 0) && winchMotor.Get() == 0 && logitech.GetRawButton(7)) { stopDriving(); // Stops the drive so the robot doesn't flip on itself or something winchMotor.Set(0); // Redundant line for extra safety that can be removed after testing (The winch should already be off) dogSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the dog gear Wait(0.05); // Giving the pistons time to disengage properly ratchetSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the ratchet if(autoLoad) { Wait(3); // Waits 3 seconds after shooting before starting to load the catapult } else { } } /****** CATAPULT RELEASE END ******/ screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Pressure:%f", compressor.GetPressureSwitchValue()); // Print Pneumatics Pressure screen -> PrintfLine(DriverStationLCD::kUser_Line2,"LimSwi:%d and %d", leftLimitSwitch.Get(), rightLimitSwitch.Get()); // Print Left& Buttons screen -> PrintfLine(DriverStationLCD::kUser_Line3,"WinchMotor:%f", winchMotor.Get()); // Print WinchMotor State screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Autoload:%s", autoLoad? "On" : "Off" ); // Print state of autoLoad //screen -> PrintfLine(DriverStationLCD::kUser_Line5,""); // Free Line screen -> PrintfLine(DriverStationLCD::kUser_Line6,"LDri:%f RDri:%f", leftFront.Get(), rightFront.Get()); // Print Tank Drive Motor State Wait(0.005); // Waits to run the loop every 0.005 seconds so the cRIO doesn't explode screen->UpdateLCD(); // Updates the Driverstation LCD in the loop } compressor.Stop(); // Allows the compressor to stop running if the mode is switched outside of OperatorControl }
/****** AUTO FUNCTIONS END *******/ void Autonomous() { int counter=0; int autonomousEngagement = 0; DriverStationLCD *screen = DriverStationLCD::GetInstance(); compressor.Start(); //starts compressor class rightArmSolenoid.Set(DoubleSolenoid::kReverse); //brings the arms down leftArmSolenoid.Set(DoubleSolenoid::kReverse); /*** ENSURES THE CATAPULT IS LOADED AND LOADS IF UNLOADED ***/ if (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1) { winchMotor.Set(0.1); // Gears need to be moving slowly to allow the dog gear to engage properly dogSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the dog gear Wait(0.2); // Giving the pistons time to engage properly winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to engage the ratchet Wait(0.2); // Giving the pistons time to engage properly } while (leftLimitSwitch.Get() == 1 && rightLimitSwitch.Get() == 1) // If Limit Switch Buttons are not pressed { winchMotor.Set(1); //Now starts the winch motor to load the catapult } // If the Catapult Left & Limit Switches are (0,0), (0,1), (1,0) { winchMotor.Set(0); // Stops the Winch Motor since one or more buttons are pressed if ((dogSolenoid.Get() == DoubleSolenoid::kReverse) && (ratchetSolenoid.Get() == DoubleSolenoid::kForward)) // If the Dog Gear is disengaged but the ratchet is engaged { winchMotor.Set(0.05); // Gears need to be moving slowly to allow the dog gear to engage properly. Might want to test this since the catapult's already loaded. dogSolenoid.Set(DoubleSolenoid::kForward); // Engages the dog gear so both dog gear and ratchet are engaged before shooting for safety Wait(0.1); // Giving the pistons time to engage properly winchMotor.Set(0); // Now that the dog gear is engaged, the gears do not have to move } else if ((dogSolenoid.Get() == DoubleSolenoid::kForward) && (ratchetSolenoid.Get() == DoubleSolenoid::kReverse)) // If the dog gear is engaged but the ratchet is disengaged { ratchetSolenoid.Set(DoubleSolenoid::kForward); // Engages the ratchet so that both dog gear and ratchet are engaged before shooting for safety Wait(0.1); // Giving the pistons time to engage properly } } /*** DONE LOADING THE CATAPULT ***/ float pLower = 5; // min height of rectangle for comparison float pUpper = 15; // max height of rectangle for comparison int criteriaCount = 1; // number of elements to include/exclude at a time int rejectMatches = 1; // when set to true, particles that do not meet the criteria are discarded int connectivity = 1; // declares connectivity value as 1; so corners are not ignored int filterFunction; // removes small blobs int borderSetting; // variable to store border settings, limit for rectangle int borderSize = 1; // border for the camera frame (if you don't put this, DriverStation gets mad at you) ParticleFilterCriteria2 particleCriteria; ParticleFilterOptions2 particleFilterOptions; int numParticles; particleCriteria.parameter = IMAQ_MT_BOUNDING_RECT_HEIGHT; //The Morphological measurement we use particleCriteria.lower = pLower; // The lower bound of the criteria range particleCriteria.upper = pUpper; // The upper bound of the criteria range particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this. particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit while ((IsAutonomous())) { if (logitech.GetRawButton(4)) { autonomousEngagement = 1; } if (autonomousEngagement == 0) // If real autonomous is not engaged start { if (logitech.GetRawButton(1)) { driveForward(); } if (logitech.GetRawButton(9)) { dogSolenoid.Set(DoubleSolenoid::kForward); // Brings the pneumatic piston backward to raise the retrieval arm winchMotor.Set(0.1); Wait(0.3); ratchetSolenoid.Set(DoubleSolenoid::kForward); // Pushes the pneumatic piston forward to lower the retrieval arm while(leftLimitSwitch.Get()==1 && rightLimitSwitch.Get()==1) { winchMotor.Set(1); } } if (logitech.GetRawButton(2)) { autonomousCatapultRelease(); } if (logitech.GetRawButton(3)) { stopDriving(); } if (logitech.GetRawButton(5)) { turnLeft(); } if (logitech.GetRawButton(7)) { turnLeftMore(); } if (logitech.GetRawButton(6)) { turnRight(); } if (logitech.GetRawButton(8)) { turnRightMore(); } }// If real autonomous is not engaged end HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image imgpointer = camera.GetImage(); //tells camera to capture image ringLight.Set(Relay::kForward); //turns ringlight on BinaryImage* binIMG = NULL; // declares a container to hold a binary image binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255); // thresholds HSL image and places in the binary image container delete imgpointer; // deletes the HSL image to free up memory on the cRIO Image* modifiedImage = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create a binary 8-bit format shell for the image filterFunction = imaqParticleFilter4(modifiedImage, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated) borderSetting = imaqSetBorderSize(modifiedImage, borderSize); // Sets a border size so DriverStation is happy delete binIMG; //Deletes the Binary image int functionCountParticles; // stores number of particles int particleAmount; // stores the number of particles for the measure particle function functionCountParticles = imaqCountParticles(modifiedImage, TRUE, &particleAmount); // Counts the number of particles int functionOne; // The first measuring particle function (specifically for particle #1) int functionTwo; // The second measuring particle function (specifically for particle #2) double particleOneOrientation; // TRULY ARBITRARY name of the first particle it find double particleTwoOrientation; // TRULY ARBITRARY name of the second particle it finds functionOne = imaqMeasureParticle(modifiedImage, 0, FALSE, IMAQ_MT_ORIENTATION, &particleOneOrientation); // Measures orientation of particle 1 functionTwo = imaqMeasureParticle(modifiedImage, 1, FALSE, IMAQ_MT_ORIENTATION, &particleTwoOrientation); // Measures orientation of particle 2 screen->PrintfLine(DriverStationLCD::kUser_Line2,"P1: %f", particleOneOrientation); // Prints particle 1's orientation screen->PrintfLine(DriverStationLCD::kUser_Line3,"P2: %f", particleTwoOrientation); // Prints particle 2's orientation imaqDispose(modifiedImage); // Deletes the filtered image /**LEFT POSITION**/ if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 0)) // Left switch set on, switch set off { screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position:F"); // Left position and facing forward if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal. /* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */ { screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Hot!"); // These DEFINITELY need to be tested. All of them. Forreal. turnRight(); //driveForward(); Wait(3); stopDriving(); //autonomousCatapultRelease(); } else // The target isn't hot. So it starts going toward this not hot goal. { screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Left Position Not Hot"); // These DEFINITELY need to be tested. All of them. Forreal. turnRight(); driveForward(); Wait(4); stopDriving(); //autonomousCatapultRelease(); } } /**CENTER POSITION**/ else if ((leftPositionSwitch.Get() == 0) && (rightPositionSwitch.Get() == 0)) // Left switch off and right switch off { screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal. /* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */ { screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot"); // These DEFINITELY need to be tested. All of them. Forreal. turnLeftMore(); driveForward(); Wait(3); stopDriving(); autonomousCatapultRelease(); } else // The target isn't hot. So it starts going toward this not hot goal. { screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot"); // These DEFINITELY need to be tested. All of them. Forreal. driveForward(); Wait(3); stopDriving(); autonomousCatapultRelease(); } } /** RIGHT POSITION**/ else if ((leftPositionSwitch.Get() == 1) && (rightPositionSwitch.Get() == 1)) { screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position:R"); // Middle position and facing if ((particleOneOrientation > 0 && particleOneOrientation < 10) || (particleTwoOrientation > 0 && particleTwoOrientation < 10)) // The target should be hot. Now it goes to the other goal. /* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */ { screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Hot"); // These DEFINITELY need to be tested. All of them. Forreal. turnLeftMore(); driveForward(); Wait(3); stopDriving(); autonomousCatapultRelease(); } else // The target isn't hot. So it starts going toward this not hot goal. { screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Middle Position Not Hot"); // These DEFINITELY need to be tested. All of them. Forreal. driveForward(); Wait(3); stopDriving(); autonomousCatapultRelease(); } } else if (((leftPositionSwitch.Get()) == 1) && ((rightPositionSwitch.Get()) == 0)) // Left switch off and switch on { screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Right Position"); // position and facing forward if ((particleOneOrientation > 0 && particleOneOrientation < 10) || ((particleTwoOrientation > 0) && (particleTwoOrientation < 10))) // The target should be hot. Now it goes to the other goal. /* Theoretically particle 1 or 2 should register as exactly 0 (the particle is horizontal). We can edit these later. */ { screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Position Hot"); // These DEFINITELY need to be tested. All of them. Forreal. turnLeft(); driveForward(); Wait(3); stopDriving(); autonomousCatapultRelease(); } else // The target isn't hot. So it starts going toward this not hot goal. { screen -> PrintfLine(DriverStationLCD::kUser_Line4, "Right Position Not Hot"); // These DEFINITELY need to be tested. All of them. Forreal. driveForward(); Wait(3); stopDriving(); autonomousCatapultRelease(); } } counter++; screen -> PrintfLine(DriverStationLCD::kUser_Line5,"R: %f L: %f)", rightFront.Get(), leftFront.Get()); screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Counter %d", counter); screen->UpdateLCD(); } compressor.Stop(); }
inline void setFlipMotor() { //set motors based on state if(armState == HandlerArmState::in) { arm_position_motor.Set(-0.008f); } else if(armState == HandlerArmState::out) { if(arm_grab_limit.Get() == NOT_PRESSED) { arm_position_motor.Set(0.35f); } else { arm_position_motor.Set(0.0f); } } else if(armState == HandlerArmState::folding_in) { if(arm_in_limit.Get() == PRESSED) { armState = HandlerArmState::in; arm_position_motor.Set(-0.008f); } else { arm_position_motor.Set(-0.35f); } } else if(armState == HandlerArmState::folding_out) { if(arm_grab_limit.Get() == PRESSED) { armState = HandlerArmState::out; arm_position_motor.Set(0.0f); } else { arm_position_motor.Set(0.35f); } } else if(armState == HandlerArmState::arm_down) { if(arm_down_limit.Get() == NOT_PRESSED) { arm_position_motor.Set(0.35f); } else { arm_position_motor.Set(0.0f); } } else if (armState == HandlerArmState::folding_down) { if(arm_down_limit.Get() == PRESSED) { armState = HandlerArmState::arm_down; arm_position_motor.Set(0.0f); } else { arm_position_motor.Set(0.35f); } } else { arm_position_motor.Set(0.01f); } }