Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "robot-config.h"
- #include <cmath>
- #include "math.h"
- enum class TurnDir { Left, Right };
- enum class StartSide { Blue, Red };
- enum class AutoRoutine { Routine1, Routine2 };
- int count = 0;
- int target = 0;
- bool driveMode = false;
- int PreviousErrorLeft = 0;
- int PreviousErrorRight = 0;
- int PreviousErrorArm =0;
- vex::digital_in AutoStartSide = vex::digital_in(Brain.ThreeWirePort.B);
- vex::digital_in Routine = vex::digital_in(Brain.ThreeWirePort.C);
- vex::competition Competition;
- /*---------------------------------------------------------------------------*/
- void pre_auton(void) {
- }
- /*---------------------------------------------------------------------------*/
- /* */
- /* Autonomous Task */
- /* */
- /* This task is used to control your robot during the autonomous phase of */
- /* a VEX Competition. */
- /* */
- /* You must modify the code to add your own robot specific commands here. */
- /*---------------------------------------------------------------------------*/
- void RightMotorFB(double speed) {
- RightMotorF.spin(directionType::fwd,speed,velocityUnits::pct);
- RightMotorB.spin(directionType::fwd,speed,velocityUnits::pct);
- }
- void LeftMotorFB(double speed) {
- LeftMotorF.spin(directionType::fwd,speed,velocityUnits::pct);
- LeftMotorB.spin(directionType::fwd,speed,velocityUnits::pct);
- }
- void slop(int distance) {
- if (distance < 0) {
- LeftMotorFB(-40);
- vex::task::sleep(35);
- }
- }
- const int accel_step = 18;
- const int deccel_step = 100; // no decel slew
- static int leftSpeed = 0;
- static int rightSpeed = 0;
- void LeftMotorFBS(int leftTarget){
- int step;
- if(abs(leftSpeed) < abs(leftTarget))
- step = accel_step;
- else
- step = deccel_step;
- if(leftTarget > leftSpeed + step)
- leftSpeed += step;
- else if(leftTarget < leftSpeed - step)
- leftSpeed -= step;
- else
- leftSpeed = leftTarget;
- LeftMotorFB(leftSpeed);
- }
- //slew control
- void RightMotorFBS(int rightTarget){
- int step;
- if(abs(rightSpeed) < abs(rightTarget))
- step = accel_step;
- else
- step = deccel_step;
- if(rightTarget > rightSpeed + step)
- rightSpeed += step;
- else if(rightTarget < rightSpeed - step)
- rightSpeed -= step;
- else
- rightSpeed = rightTarget;
- RightMotorFB(rightSpeed);
- }
- int drivePos() {
- return (LeftMotorF.rotation(rotationUnits::deg) + RightMotorF.rotation(rotationUnits::deg))/2;
- }
- bool isDriving() {
- return(LeftMotorF.isSpinning() || LeftMotorB.isSpinning() || RightMotorF.isSpinning() || RightMotorB.isSpinning());
- }
- void PidBasicAsync(int distance) {
- slop(distance);
- count = 0;
- target = distance;
- driveMode = true;
- LeftMotorF.resetRotation();
- RightMotorF.resetRotation();
- LeftMotorB.resetRotation();
- RightMotorB.resetRotation();
- PreviousErrorLeft = 0;
- PreviousErrorRight = 0;
- LeftMotorFBS(distance);
- RightMotorFBS(distance);
- }
- void BigArm (int degrees) {
- double kp = 0.2;
- int LiftPos= P2.value(rotationUnits::deg);
- int power = (degrees - LiftPos) * kp;
- LiftPos= P2.value(rotationUnits::deg);
- if (abs(degrees - LiftPos) <= 44) {
- Arm2000.stop();
- } else {
- Arm2000.spin(directionType::fwd, power, velocityUnits::pct);
- }
- }
- /*
- int LiftPos1=Arm2000.rotation(rotationUnits::deg);
- int ArmError = Arm - power;
- Arm=Arm2000.rotation(rotationUnits::deg);
- int ArmProError = Kp * ArmError;
- PreviousErrorArm = ;
- if (abs(ArmError) < 10 {
- break;
- }
- */
- void PidBasic (int distance) {
- if (driveMode) {
- double Kp = 0.16192;///0.2 or .215 .162
- double Kd = .4995;//465 or .52 or .45
- int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
- int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
- int ErrorLeft = distance - Leftdegrees;
- int ErrorRight = distance - Rightdegrees;
- Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
- Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
- ErrorLeft = distance - Leftdegrees;
- ErrorRight = distance - Rightdegrees;
- int LeftsideProportionalError = Kp * ErrorLeft;
- int RightsideProportionalError = Kp * ErrorRight;
- double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
- double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);
- LeftMotorFB(LeftsideProportionalError + LeftsidedDerivativeError);
- RightMotorFB(RightsideProportionalError + RightsidedDerivativeError);
- PreviousErrorLeft = ErrorLeft;
- PreviousErrorRight = ErrorRight;
- /*
- if (abs(ErrorLeft) < 6 && abs(ErrorRight) < 6) {
- break;
- }
- */
- if (!isDriving()) {
- count++;
- }
- if (count > 30) {
- driveMode = false;
- }
- vex::task::sleep(30);
- }
- }
- void Turning (int distance) {
- LeftMotorF.resetRotation();
- RightMotorF.resetRotation();
- LeftMotorB.resetRotation();
- RightMotorB.resetRotation();
- float Kp = .17; //// best.17 //// ..30
- float Kd = .01;/// best .35 //// .34 ////next best .32 next best// 25 //smallest movement.1
- int Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
- int Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
- int ErrorLeft = -1*Leftdegrees - distance;
- int ErrorRight = distance - Rightdegrees;
- int PreviousErrorLeft = 0;
- int PreviousErrorRight = 0;
- while(1) {
- Leftdegrees=LeftMotorF.rotation(rotationUnits::deg);
- Rightdegrees=RightMotorF.rotation(rotationUnits::deg);
- ErrorLeft = -1*Leftdegrees - distance;
- ErrorRight = distance - Rightdegrees;
- int LeftsideProportionalError = Kp * ErrorLeft;
- int RightsideProportionalError = Kp * ErrorRight;
- double LeftsidedDerivativeError = Kd * (ErrorLeft - PreviousErrorLeft);
- double RightsidedDerivativeError = Kd * (ErrorRight - PreviousErrorRight);
- LeftMotorFB(LeftsideProportionalError + LeftsidedDerivativeError);
- RightMotorFB(RightsideProportionalError + RightsidedDerivativeError);
- PreviousErrorLeft = ErrorLeft;
- PreviousErrorRight = ErrorRight;
- if (abs(ErrorLeft) < 10 && abs(ErrorRight) < 10) {
- break;
- }
- vex::task::sleep(30);
- }
- }
- /////////////////////////////////////////////////////////////////////////////////////////////
- void StopMotor(vex::brakeType type = brakeType::coast){
- LeftMotorF.stop(type);
- RightMotorF.stop(type);
- LeftMotorB.stop(type);
- RightMotorB.stop(type);
- }
- void SetMotorBrakingType(brakeType type = brakeType::coast) {
- LeftMotorF.setStopping(type);
- RightMotorF.setStopping(type);
- LeftMotorB.setStopping(type);
- RightMotorB.setStopping(type);
- }
- int CubeIntake2() {
- int timeout = 4000;
- Scooper.setStopping(brakeType::coast);
- int sleepTimeout = 20;
- Scooper.spin(directionType::rev, 100, velocityUnits::pct);
- Scooper.spin(directionType::rev, 100, velocityUnits::pct);
- while (timeout > 0) {
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- }
- Scooper.stop(brakeType::coast);
- return(0);
- }
- int CubeIntake() {
- int timeout = 4000;
- Scooper.setStopping(brakeType::coast);
- Scooper2.setStopping(brakeType::coast);
- int sleepTimeout = 20;
- Scooper.spin(directionType::fwd, 100, velocityUnits::pct);
- Scooper2.spin(directionType::fwd, 100, velocityUnits::pct);
- while (timeout > 0) {
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- }
- Scooper.stop(brakeType::coast);
- Scooper2.stop(brakeType::coast);
- return(0);
- }
- int AutoBallIntakeTakeBall(int timeout = 5000) {
- Scooper.setStopping(brakeType::coast);
- int sleepTimeout = 20;
- Scooper.spin(directionType::fwd, 60, velocityUnits::pct);
- while (timeout > 0) {
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- }
- Scooper.stop(brakeType::coast);
- return(0);
- }
- void Drive(double numOfRevs, int velocity = 50, bool fwd = true, int timeout = 5000)
- {
- Brain.Screen.printAt(1, 40, "numofrevs: %f, vel: %f", numOfRevs, velocity);
- SetMotorBrakingType();
- int sleepTimeout = 20;
- if (fwd == false) {
- numOfRevs = numOfRevs * -1;
- }
- LeftMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
- RightMotorF.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
- LeftMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
- RightMotorB.startRotateFor(numOfRevs, rotationUnits::rev, velocity, velocityUnits::pct);
- while(LeftMotorF.isSpinning()||RightMotorF.isSpinning()||
- LeftMotorB.isSpinning()||RightMotorB.isSpinning()) {
- if (timeout <= 0) {
- break;
- }
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- }
- StopMotor();
- }
- void DriveInCM(double distanceInCM, int velocity, bool fwd = true, int timeout = 5000) {
- // double gearRatio = 1.67;
- // double wheelDiameter = 10.16;
- // double circumference = wheelDiameter * M_PI;
- // double numOfRevs = (distanceInCM)/ (circumference * gearRatio);
- double numOfRevs = distanceInCM/47.9;////old 53.2
- Drive(numOfRevs, velocity, fwd, timeout);
- }
- void Turn(double degrees, TurnDir turnDir, int velocity = 25, int timeout = 5000){
- int sleepTimeout = 10;
- SetMotorBrakingType();
- LeftMotorF.resetRotation();
- RightMotorF.resetRotation();
- if (turnDir == TurnDir::Right) {
- LeftMotorF.spin(directionType::fwd, velocity, velocityUnits::pct);
- LeftMotorB.spin(directionType::fwd, velocity, velocityUnits::pct);
- RightMotorF.spin(directionType::rev, velocity, velocityUnits::pct);
- RightMotorB.spin(directionType::rev, velocity, velocityUnits::pct);
- }
- else
- {
- LeftMotorF.spin(directionType::rev, velocity, velocityUnits::pct);
- LeftMotorB.spin(directionType::rev, velocity, velocityUnits::pct);
- RightMotorF.spin(directionType::fwd, velocity, velocityUnits::pct);
- RightMotorB.spin(directionType::fwd, velocity, velocityUnits::pct);
- }
- while(true) {
- double flmDeg = std::abs(LeftMotorF.rotation(rotationUnits::deg));
- double frmDeg = std::abs(RightMotorF.rotation(rotationUnits::deg));
- Brain.Screen.clearScreen();
- Brain.Screen.printAt(1, 20, "FL: %f FR: %f", flmDeg, frmDeg);
- if (frmDeg > degrees) {
- Brain.Screen.printAt(1, 40, "Deg Break: FL: %f FR: %f", flmDeg, frmDeg);
- break;
- }
- if (timeout <= 0) {
- Brain.Screen.printAt(1, 40, "Timeout Break: FL: %f FR: %f", flmDeg, frmDeg);
- break;
- }
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- }
- StopMotor(brakeType::coast);
- }
- void TurnRight90Degrees()
- {
- Turn(85, TurnDir::Right);
- }
- void TurnLeft90Degrees()
- {
- Turn(85, TurnDir::Left);
- }
- /////////////////////////////////////////////////////////////////////////////////
- void LiftPosition1 () {
- int timeout = 5000;
- Arm2000.setStopping(brakeType::coast);
- int sleepTimeout = 30;
- while (timeout > 0) {
- Brain.Screen.clearScreen();
- float LiftPos = P2.value(rotationUnits::deg);
- Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);
- if (LiftPos == 0)
- {
- task::sleep(sleepTimeout);
- continue;
- }
- // Lowest position for launch
- if (LiftPos >= 107)
- {
- Brain.Screen.printAt(2, 80, "Stopped: %f", LiftPos);
- break;
- }
- Arm2000.spin(directionType::fwd, 100, velocityUnits::pct);
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
- }
- Arm2000.stop(brakeType::coast);
- }
- void LiftPosition2() {
- int timeout = 5000;
- Arm2000.setStopping(brakeType::coast);
- int sleepTimeout = 30;
- while (timeout > 0) {
- Brain.Screen.clearScreen();
- float LiftPos = P2.value(rotationUnits::deg);
- Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);
- if (LiftPos == 0)
- {
- task::sleep(sleepTimeout);
- continue;
- }
- // load position
- if (LiftPos <= 44)
- {
- break;
- }
- /*
- double kp = 0.1;
- double target = 1000;
- int power = (target - LiftPos) * kp + 10;;
- */
- Arm2000.spin(directionType::rev, 45, velocityUnits::pct);
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
- }
- Arm2000.stop(brakeType::hold);
- }
- void LiftPosition3() {
- int timeout = 2500;
- Arm2000.setStopping(brakeType::coast);
- int sleepTimeout = 30;
- while (timeout > 0) {
- Brain.Screen.clearScreen();
- float LiftPos = P2.value(rotationUnits::deg);
- Brain.Screen.printAt(2, 40, "LiftPos: %f", LiftPos);
- if(P2.value(rotationUnits::deg) <44) {
- while(P2.value(rotationUnits::deg) <44) { ///////////13
- Arm2000.spin(vex::directionType::fwd, 45, vex::velocityUnits::pct);
- }
- break;
- }
- else
- if(P2.value(rotationUnits::deg) > 45) {
- while(P2.value(rotationUnits::deg) > 45) { /////////16
- Arm2000.spin(vex::directionType::rev, 45, vex::velocityUnits::pct);
- }
- break;
- }
- Arm2000.spin(directionType::fwd, 40, velocityUnits::pct);
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
- }
- Arm2000.stop(brakeType::coast);
- }
- ///////////////////////////////////////////////??///////////////////////
- void Lift10() {
- int timeout = 2500;
- Lift.setStopping(brakeType::hold);
- int sleepTimeout = 30;
- while (timeout > 0) {
- Brain.Screen.clearScreen();
- float LiftCube = P1.value(rotationUnits::deg);
- Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);
- if (LiftCube >= 87)
- {
- Brain.Screen.printAt(2, 80, "Stopped: %f", LiftCube);
- break;
- }
- Lift.spin(directionType::rev, 127, velocityUnits::pct);
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
- }
- Lift.stop(brakeType::hold);
- }
- void Lift11() {
- int timeout = 2500;
- Lift.setStopping(brakeType::hold);
- int sleepTimeout = 30;
- while (timeout > 0) {
- Brain.Screen.clearScreen();
- float LiftCube = P1.value(rotationUnits::deg);
- Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);
- if (LiftCube >= 69)
- {
- Brain.Screen.printAt(2, 80, "Stopped: %f", LiftCube);
- break;
- }
- Lift.spin(directionType::rev, 127, velocityUnits::pct);
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
- }
- Lift.stop(brakeType::hold);
- }
- void Lift12() {
- int timeout = 2500;
- Lift.setStopping(brakeType::coast);
- int sleepTimeout = 30;
- while (timeout > 0) {
- Brain.Screen.clearScreen();
- float LiftCube = P1.value(rotationUnits::deg);
- Brain.Screen.printAt(2, 40, "LiftCube %f", LiftCube);
- if(P1.value(rotationUnits::deg) <11) {
- while(P1.value(rotationUnits::deg) <11) { ///////////13
- Lift.spin(vex::directionType::rev, 127, vex::velocityUnits::pct);
- }
- break;
- }
- else
- if(P1.value(rotationUnits::deg) > 12) {
- while(P1.value(rotationUnits::deg) > 12) { /////////16
- Lift.spin(vex::directionType::fwd, 100, vex::velocityUnits::pct);
- }
- break;
- }
- Lift.spin(directionType::rev, 100, velocityUnits::pct);
- task::sleep(sleepTimeout);
- timeout = timeout - sleepTimeout;
- Brain.Screen.printAt(2, 20, "Timeout: %d", timeout);
- }
- Lift.stop(brakeType::coast);
- }
- //////////////
- /////////////////////////
- void AutoRoutine1(StartSide startSide)
- {
- Scooper.spin(directionType::rev, 100, velocityUnits::pct);
- Scooper2.spin(directionType::rev, 100, velocityUnits::pct);
- vex::task::sleep(250);
- Scooper.stop();
- Scooper2.stop();
- Scooper.spin(directionType::fwd, 100, velocityUnits::pct);
- Scooper2.spin(directionType::fwd, 100, velocityUnits::pct);
- vex::task::sleep(100);
- DriveInCM(60, 40);
- LeftMotorF.setStopping(brakeType::coast);
- LeftMotorB.setStopping(brakeType::coast);
- RightMotorF.setStopping(brakeType::coast);
- RightMotorB.setStopping(brakeType::coast);
- vex::task::sleep(50);
- DriveInCM(10, 50);
- LeftMotorF.setStopping(brakeType::brake);
- LeftMotorB.setStopping(brakeType::brake);
- RightMotorF.setStopping(brakeType::brake);
- RightMotorB.setStopping(brakeType::brake);
- vex::task::sleep(600);
- DriveInCM(-20, 30);
- LeftMotorF.setStopping(brakeType::brake);
- LeftMotorB.setStopping(brakeType::brake);
- RightMotorF.setStopping(brakeType::brake);
- RightMotorB.setStopping(brakeType::brake);
- Scooper.stop();
- Scooper2.stop();
- return;
- }
- void CheckLiftPotValues() {
- while (1)
- {
- Brain.Screen.clearScreen();
- Brain.Screen.printAt(1, 30, "CheckLiftPot");
- float LiftPos = P2.value(rotationUnits::deg);
- Brain.Screen.printAt(1, 20, "rotation: %f degrees", LiftPos);
- //Sleep the task for a short amount of time to prevent wasted resources.
- task::sleep(20);
- }
- }
- ///////////////////////////
- void AutoRoutine2(StartSide startSide)
- {
- Scooper.spin(directionType::rev, 100, velocityUnits::pct);
- Scooper2.spin(directionType::rev, 100, velocityUnits::pct);
- vex::task::sleep(250);
- Scooper.stop();
- Scooper2.stop();
- Scooper.spin(directionType::fwd, 100, velocityUnits::pct);
- Scooper2.spin(directionType::fwd, 100, velocityUnits::pct);
- vex::task::sleep(250);
- DriveInCM(100, 30);
- DriveInCM(10, 30);
- vex::task::sleep(100);
- DriveInCM(-48, 30);
- LeftMotorF.setStopping(brakeType::brake);
- LeftMotorB.setStopping(brakeType::brake);
- RightMotorF.setStopping(brakeType::brake);
- RightMotorB.setStopping(brakeType::brake);
- vex::task::sleep(100);
- Scooper.stop();
- Scooper2.stop();
- if ( startSide == StartSide::Blue)
- {
- Turning(237);
- }
- else
- {
- Turning(-237);
- }
- vex::task::sleep(50);
- DriveInCM(56, 30);
- LeftMotorF.setStopping(brakeType::brake);
- LeftMotorB.setStopping(brakeType::brake);
- RightMotorF.setStopping(brakeType::brake);
- RightMotorB.setStopping(brakeType::brake);
- LiftPosition2();
- vex::task::sleep(50);
- DriveInCM(-20, 30);
- }
- void autonomous(void) {
- int startSideValue = AutoStartSide.value();
- StartSide startSide;
- if (startSideValue == 1)
- {
- startSide = StartSide::Blue;
- }
- else
- {
- startSide = StartSide::Red;
- }
- int routineValue = Routine.value();
- AutoRoutine routine;
- // no pin
- if (routineValue == 1)
- {
- routine = AutoRoutine::Routine1;
- }
- else
- {
- routine = AutoRoutine::Routine2;
- }
- if (routine == AutoRoutine::Routine1)
- {
- AutoRoutine1(startSide);
- }
- else
- {
- AutoRoutine2(startSide);
- }
- return;
- }
- int DriveTask() {
- while (true) {
- if (driveMode) {
- PidBasic(target);
- }
- }
- }
- int LiftTask()
- {
- while (true) {
- Brain.Screen.clearScreen();
- float LiftPos = P2.value(rotationUnits::deg);
- Brain.Screen.printAt(1, 40, "LiftPos: %f", LiftPos);
- if (Controller1.ButtonY.pressing()) {
- // Load
- LiftPosition1();
- }
- else if (Controller1.ButtonB.pressing()) {
- // Launch
- BigArm(45);
- } else {
- Arm2000.stop();
- }
- if (Controller2.ButtonL1.pressing()) {
- // Launch
- Lift10();
- }
- else if (Controller2.ButtonL2.pressing()) {
- // Launch
- Lift11();
- }
- else if (Controller2.ButtonDown.pressing()) {
- // Launch
- Lift12();
- }
- task::sleep(30);
- }
- return (0);
- }
- /*----------------------------------------------------------------------------*/
- /* */
- /* User Control Task */
- /* */
- /* This task is used to control your robot during the user control phase of */
- /* a VEX Competition. */
- /* */
- /* You must modify the code to add your own robot specific commands here. */
- /*----------------------------------------------------------------------------*/
- void usercontrol(void) {
- task CallLiftTask(LiftTask);
- while (true) {
- RightMotorF.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct), velocityUnits::pct);
- RightMotorB.spin(directionType::fwd, Controller1.Axis2.position(percentUnits::pct), velocityUnits::pct);
- LeftMotorF.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct), velocityUnits::pct);
- LeftMotorB.spin(directionType::fwd, Controller1.Axis3.position(percentUnits::pct), velocityUnits::pct);
- LeftMotorF.setStopping(brakeType::coast);
- RightMotorB.setStopping(brakeType::coast);
- LeftMotorB.setStopping(brakeType::coast);
- RightMotorF.setStopping(brakeType::coast);
- if (Controller1.ButtonL2.pressing()) {
- //If button up is pressed...Spin the arm motor forward
- Scooper.spin(vex::directionType::fwd, 127, velocityUnits::pct);
- Scooper2.spin(vex::directionType::fwd, 127, velocityUnits::pct);
- }
- else if (Controller1.ButtonL1.pressing()) { //If the down button is pressed...
- Scooper.spin(vex::directionType::rev, 70, velocityUnits::pct);
- Scooper2.spin(vex::directionType::rev, 70, velocityUnits::pct);
- }
- else { //If the the up or down button is not pressed...
- Scooper.stop(brakeType::brake);
- Scooper2.stop(brakeType::brake);
- }
- }
- }
- int main() {
- Brain.Screen.clearScreen();
- float LiftPos = P2.value(rotationUnits::deg);
- Brain.Screen.printAt(1, 40, "LiftPos: %f", LiftPos);
- Brain.Screen.clearScreen();
- float LiftCube = P1.value(rotationUnits::deg);
- Brain.Screen.printAt(1, 40, "LiftCube: %e", LiftCube);
- pre_auton();
- Competition.autonomous(autonomous);
- Competition.drivercontrol(usercontrol);
- //Prevent main from exiting with an infinite loop.
- while (1) {
- vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement