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);
}
}
