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 stopDriving() { leftFront.Set(0); leftBack.Set(0); rightFront.Set(0); rightBack.Set(0); }
//Grab first two and turn to go right void AutonomousType10() { SmartDashboard::PutString("STATUS:", "STARTING AUTO 10"); robotDrive.MecanumDrive_Cartesian(0, -0.2, 0); if (WaitF(1.2)) return; robotDrive.MecanumDrive_Cartesian(0, 0, 0); chainLift.SetSpeed(0.5); while (IsAutonomous() && maxUp.Get() && midPoint.Get()) { } chainLift.SetSpeed(0); robotDrive.MecanumDrive_Cartesian(0, 0.4, 0); if (WaitF(1.6)) return; robotDrive.MecanumDrive_Polar(0, 0, 0.3); if (WaitF(2.6)) return; robotDrive.MecanumDrive_Cartesian(0, -0.4, 0); if (WaitF(1)) return; robotDrive.MecanumDrive_Polar(0, 0, -0.3); if (WaitF(2.6)) return; robotDrive.MecanumDrive_Cartesian(0, -0.4, 0); if (WaitF(1.6)) return; robotDrive.MecanumDrive_Cartesian(0, 0, 0); SmartDashboard::PutString("STATUS:", "AUTO 10 COMPLETE"); }
//Choose which auto to use void AutonomousInit() { chainLift.SetSpeed(0); canGrabber.SetSpeed(0); robotDrive.MecanumDrive_Cartesian(0, 0, 0); SmartDashboard::PutString("STATUS:", "STARTING AUTO"); robotDrive.SetSafetyEnabled(false); chainLift.SetSafetyEnabled(false); SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get()); SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get()); //Select auto type if (autoSwitch1.Get()) { if (autoSwitch2.Get()) AutonomousType4(); else //1 on 2 grab n back AutonomousType8(); } else { if (autoSwitch2.Get()) //1 off, 2 on: grab n turn AutonomousType10(); else { SmartAutoPicker(); } //Do Nothing } }
void AutonomousType4() { SmartDashboard::PutString("STATUS:", "STARTING AUTO 4"); //Lift, turn, drive chainLift.SetSpeed(0.5); while (midPoint.Get() && maxUp.Get()) { } chainLift.SetSpeed(0); robotDrive.MecanumDrive_Polar(0, 0, 0.3); if (WaitF(2.5)) return; robotDrive.MecanumDrive_Polar(0.25, 0, 0); if (WaitF(5.6)) return; robotDrive.MecanumDrive_Polar(0, 0, 0.3); if (WaitF(2)) return; robotDrive.MecanumDrive_Polar(0, 0, 0); //chainLift.SetSpeed(-0.4); //while (maxDown.Get() && IsAutonomous()) { //} chainLift.SetSpeed(0); robotDrive.MecanumDrive_Polar(0, 0, 0); SmartDashboard::PutString("STATUS:", "AUTO 4 COMPLETE"); }
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 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 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); }
/**** 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 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 }
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 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 } }
//Grab first yellow, back up to auto zone, DON'T DROP void AutonomousType12() { SmartDashboard::PutString("STATUS:", "STARTING AUTO 12"); chainLift.SetSpeed(0.5); while (IsAutonomous() && IsEnabled() && maxUp.Get() && midPoint.Get()) { } chainLift.SetSpeed(0); robotDrive.MecanumDrive_Cartesian(0, 0.4, 0); if (WaitF(3)) return; robotDrive.MecanumDrive_Cartesian(0, 0, 0); SmartDashboard::PutString("STATUS:", "AUTO 12 COMPLETE"); }
//Steal cans void AutonomousType13() { SmartDashboard::PutString("STATUS:", "STARTING AUTO 13"); robotDrive.MecanumDrive_Cartesian(0, 0.2, 0); if (WaitF(1.2)) return; robotDrive.MecanumDrive_Cartesian(0, 0, 0); canGrabber.SetSpeed(1); if (WaitF(4)) return; canGrabber.SetSpeed(0); LinearAcceleration(1, 0, 1, 0); SmartDashboard::PutString("STATUS:", "AUTO 13 COMPLETE"); }
virtual void TeleopPeriodic() { rightDrive->SetSpeed(-(Driver->GetRawAxis(2))); leftDrive->SetSpeed((Driver->GetRawAxis(5))); shooterFWD->SetSpeed(-(Operator->GetRawAxis(2))); shooterRear->SetSpeed(-(Operator->GetRawAxis(2))); //shoioter angle if(Operator->GetRawButton(5)) { cout<<"Relay 1 forward"<<endl; shooterAngle->Set(Relay::kForward); } if(Operator->GetRawButton(6)) { cout<<"Relay 1 Reverse"<<endl; shooterAngle->Set(Relay::kReverse); } //Fire button if(Operator->GetRawButton(1)) { cout<<"Relay 1 forward"<<endl; shooterFire->Set(Relay::kForward); } if(Operator->GetRawButton(2)) { cout<<"Relay 1 Reverse"<<endl; shooterFire->Set(Relay::kReverse); } if(CompressorSwitch->Get() == 0){ CompressorRelay->Set(Relay::kForward); }else{ CompressorRelay->Set(Relay::kOff); } //if(canPDP == 0){ // cout << "NULL" << endl; //}else{ //canPDP->GetVoltage(Voltage) ; //cout << "0" << endl; //} }
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 AutonomousType3() { //Grab a bin/trash bin, and move forward SmartDashboard::PutString("STATUS:", "STARTING AUTO 3"); chainLift.SetSpeed(0.5); while (midPoint.Get() && maxUp.Get()) { } chainLift.SetSpeed(0); robotDrive.MecanumDrive_Cartesian(0, -0.75, 0); if (WaitF(1.75)) return; robotDrive.MecanumDrive_Cartesian(0, 0, 0); chainLift.SetSpeed(-0.5); while (maxDown.Get()) { } chainLift.SetSpeed(0); SmartDashboard::PutString("STATUS:", "AUTO 3 COMPLETE"); }
/** * 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 } }
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); }
void AutonomousType2() { //Pick tote and bin, move to auto zone SmartDashboard::PutString("STATUS:", "STARTING AUTO 2"); chainLift.SetSpeed(0.5); while (midPoint.Get() && maxUp.Get()) { } chainLift.SetSpeed(0); robotDrive.MecanumDrive_Polar(0.3, 0, 0); if (WaitF(1.6)) return; robotDrive.MecanumDrive_Polar(0, 0, 0); chainLift.SetSpeed(-0.2); if (WaitF(0.8)) return; chainLift.SetSpeed(0); robotDrive.MecanumDrive_Polar(-0.3, 0, 0); if (WaitF(1.6)) return; robotDrive.MecanumDrive_Polar(0, 0, 0); chainLift.SetSpeed(-0.3); while (maxDown.Get()) { } chainLift.SetSpeed(0); robotDrive.MecanumDrive_Polar(0.2, 0, 0); if (WaitF(2)) return; robotDrive.MecanumDrive_Polar(0, 0, 0); chainLift.SetSpeed(0.4); while (midPoint.Get() && maxUp.Get()) { } chainLift.SetSpeed(0); //turn 90 deg robotDrive.MecanumDrive_Polar(0, 0, -0.3); if (WaitF(4)) return; robotDrive.MecanumDrive_Polar(0.5, 0, 0); if (WaitF(2.5)) return; robotDrive.MecanumDrive_Polar(0, 0, 0); chainLift.SetSpeed(-0.4); while (maxDown.Get() && IsAutonomous()) { } chainLift.SetSpeed(0); SmartDashboard::PutString("STATUS:", "AUTO 2 COMPLETE"); }
/** * 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(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); }
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 }
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 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 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 }
RobotDemo(): logitech(1), // Joystick Port [Laptop USB] leftFront(1), leftBack(3), rightFront(2), rightBack(4), // Tank Drive Ports [PWMOUT] retrievalMotor(6), winchMotor(5), // Retrieval and Catapult Winch Motor Ports [PWMOUT] compressor(5,2), // Compressor connection ports [Relay, Digital IO] leftArmSolenoid(5,6), // Left Arm Solenoid ports [Solenoid Breakout, Solenoid Breakout] rightArmSolenoid(7,8), // Arm Solenoid ports [Solenoid Breakout, Solenoid Breakout] dogSolenoid(1,2), // Dog Solenoid ports [Solenoid Breakout, Solenoid Breakout] ratchetSolenoid(3,4), // Ratchet Solenoid ports [Solenoid Breakout, Solenoid Breakout] leftLimitSwitch(8), rightLimitSwitch(9), // Catapult Limit Switch ports [Digital IO] /****** AUTONOMOUS ******/ leftPositionSwitch(6), // Left Autonomous Switch [Digital IO] rightPositionSwitch(7), // Right Autonomous Switch [Digital IO] ringLight(1, Relay::kBothDirections), // Ringlight [Relay] camera(AxisCamera::GetInstance("10.28.53.11")) //IP address of the camera { //Expirations can be put here. WindRiver gets mad if you remove these braces. camera.WriteResolution(AxisCamera::kResolution_320x240); // sets the camera's resolution rightFront.SetExpiration(0.1); rightBack.SetExpiration(0.1); leftFront.SetExpiration(0.1); leftBack.SetExpiration(0.1); winchMotor.SetExpiration(0.1); retrievalMotor.SetExpiration(0.1); }
void CupidShuffle() { SmartDashboard::PutString("STATUS:", "TIME 2 GET DOWWWWWWN"); //Tempo of song static double tempo = 0.41666666667; //Repeat # of times for (int j = 0; j < 10 && IsAutonomous() && IsEnabled(); j++) { //to the left to the left to the left to the left for (int k = 0; k < 4; k++) { robotDrive.MecanumDrive_Cartesian(-0.2, 0, 0); Wait(tempo); robotDrive.MecanumDrive_Cartesian(0, 0, 0); Wait(tempo); } //to the right to the right to the right for (int k = 0; k < 4; k++) { robotDrive.MecanumDrive_Cartesian(0.2, 0, 0); Wait(tempo); robotDrive.MecanumDrive_Cartesian(0, 0, 0); Wait(tempo); } //kick kick kick kick for (int k = 0; k < 4; k++) { chainLift.SetSpeed(0.3); Wait(tempo); chainLift.SetSpeed(-0.3); Wait(tempo); } chainLift.SetSpeed(0); //walk it by uself (turn 90) robotDrive.MecanumDrive_Polar(0, 0, 0.3); Wait(tempo * 8); } SmartDashboard::PutString("STATUS:", "GIT GUD"); }