diff --git a/src/main.cpp b/src/main.cpp index 7ca4664..dbefd00 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,22 +10,23 @@ // ---- START VEXCODE CONFIGURED DEVICES ---- // Robot Configuration: // [Name] [Type] [Port(s)] -// Drivetrain drivetrain 1, 2 -// LiftMotor motor 18 -// LiftMotor2 motor 12 -// TiltMotor motor 3 -// Controller1 controller -// IntakeR motor 17 -// IntakeL motor 8 +// Drivetrain drivetrain 1, 2 +// LiftMotor motor 18 +// LiftMotor2 motor 12 +// TiltMotor motor 3 +// Controller1 controller +// IntakeR motor 17 +// IntakeL motor 8 // ---- END VEXCODE CONFIGURED DEVICES ---- #include "vex.h" #include -#include -#include #include +#include +#include #include + using namespace vex; bool speedin = true; double driveMultiplier = 1.0; @@ -37,11 +38,11 @@ competition Competition; motor LeftDriveSmart = motor(PORT1, ratio18_1, false); motor RightDriveSmart = motor(PORT2, ratio18_1, true); -drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295, 130, mm, 1); +drivetrain Drivetrain = + drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295, 130, mm, 1); motor LiftMotor = motor(PORT18, ratio18_1, true); motor LiftMotor2 = motor(PORT12, ratio18_1, false); motor TiltMotor = motor(PORT3, ratio18_1, false); -motor TiltMotor2 = motor(PORT10, ratio18_1, true); controller Controller1 = controller(primary); motor IntakeR = motor(PORT17, ratio18_1, false); motor IntakeL = motor(PORT8, ratio18_1, true); @@ -84,6 +85,9 @@ void pre_auton(void) { /* You must modify the code to add your own robot specific commands here. */ /*---------------------------------------------------------------------------*/ std::ifstream ifs; +void goToPos(motor m, double vel, double pos); +void playback(); +std::string instructions; void autonomous(void) { // .......................................................................... // Insert autonomous user code here. @@ -96,7 +100,6 @@ void autonomous(void) { LiftMotor.resetRotation(); LiftMotor2.resetRotation(); TiltMotor.resetRotation(); - TiltMotor2.resetRotation(); IntakeL.resetRotation(); IntakeR.resetRotation(); LeftDriveSmart.resetRotation(); @@ -104,80 +107,32 @@ void autonomous(void) { LiftMotor.setStopping(hold); LiftMotor2.setStopping(hold); - //STOPCODE_DEBUG(); - //uint8_t * instructions = nullptr; - //Brain.SDcard.loadfile("recording.bin", instructions, 2500); // size in bytes + // STOPCODE_DEBUG(); + // uint8_t * instructions = nullptr; + // Brain.SDcard.loadfile("recording.bin", instructions, 2500); // size in + // bytes ifs.open("recording.txt", std::ofstream::in); Controller1.Screen.print("File opened"); - std::string instructions; - while(std::getline(ifs, instructions)) { - int y = 0; - y = instructions.find(" ", 5); - double vel = strtod(instructions.substr(0, y).c_str(), NULL); - instructions = instructions.substr(y + 1); - y = instructions.find(" ", 5); - double pos = strtod(instructions.substr(0, y).c_str(), NULL); - instructions = instructions.substr(y + 1); - Controller1.Screen.print(vel); - LeftDriveSmart.setVelocity(vel, percent); - if(LeftDriveSmart.rotation(degrees) < pos && vel > 0) - LeftDriveSmart.spin(forward); - else if(LeftDriveSmart.rotation(degrees) > pos && vel < 0) - LeftDriveSmart.spin(forward); - else - LeftDriveSmart.stop(); - //LeftDriveSmart.startSpinTo(pos, degrees); - - y = instructions.find(" ", 5); - vel = strtod(instructions.substr(0, y).c_str(), NULL); - instructions = instructions.substr(y + 1); - y = instructions.find(" ", 5); - pos = strtod(instructions.substr(0, y).c_str(), NULL); - instructions = instructions.substr(y + 1); - Controller1.Screen.print(vel); - RightDriveSmart.setVelocity(vel, percent); - if(RightDriveSmart.rotation(degrees) < pos && vel > 0) - RightDriveSmart.spin(forward); - else if(RightDriveSmart.rotation(degrees) > pos && vel < 0) - RightDriveSmart.spin(forward); - else - RightDriveSmart.stop(); - - y = instructions.find(" ", 5); - vel = strtod(instructions.substr(0, y).c_str(), NULL); - instructions = instructions.substr(y + 1); - IntakeL.spin(forward, vel, percent); - IntakeR.spin(forward, vel, percent); - - y = instructions.find(" ", 5); - vel = strtod(instructions.substr(0, y).c_str(), NULL); - instructions = instructions.substr(y + 1); - y = instructions.find(" ", 5); - pos = strtod(instructions.substr(0, y).c_str(), NULL); - instructions = instructions.substr(y + 1); - LiftMotor.rotateTo(pos, degrees, vel, velocityUnits::pct, false); - LiftMotor2.rotateTo(pos, degrees, vel, velocityUnits::pct, false); - - y = instructions.find(" ", 5); - vel = strtod(instructions.substr(0, y).c_str(), NULL); - instructions = instructions.substr(y + 1); - y = instructions.find(" ", 5); - pos = strtod(instructions.substr(0, y).c_str(), NULL); - TiltMotor.rotateTo(pos, degrees, vel, velocityUnits::pct, false); - TiltMotor2.rotateTo(pos, degrees, vel, velocityUnits::pct, false); - wait(10, msec); + timer tim; + tim.clear(); + while (std::getline(ifs, instructions)) { + while (tim.value() < 0.075) { + wait(1, msec); + } + tim.clear(); + playback(); } ifs.close(); STOPCODE_DEBUG(); Drivetrain.setDriveVelocity(40, percent); Drivetrain.setTurnVelocity(50, percent); - // Drivetrain.turnFor(5, degrees); + // Drivetrain.turnFor(5, degrees); Drivetrain.driveFor(15, inches, false); wait(2, sec); Drivetrain.setDriveVelocity(80, percent); Drivetrain.driveFor(reverse, 16, inches); wait(1, sec); - //Drivetrain.turnFor(-45, degrees); + // Drivetrain.turnFor(-45, degrees); STOPCODE_DEBUG(); LiftMotor.startSpinTo(40 * 7, degrees); // unlock everything LiftMotor2.startSpinTo(40 * 7, degrees); @@ -188,22 +143,23 @@ void autonomous(void) { waitUntil(LiftMotor.isDone() && LiftMotor2.isDone() && TiltMotor.isDone()); IntakeL.setVelocity(100, percent); IntakeR.setVelocity(100, percent); - IntakeL.startSpinTo(-5, rev); + IntakeL.startSpinTo(-5, rev); IntakeR.startSpinTo(-5, rev); TiltMotor.startSpinTo(5, degrees); waitUntil(TiltMotor.isDone()); - //STOPCODE_DEBUG(); + // STOPCODE_DEBUG(); STOPCODE_DEBUG(); IntakeL.setVelocity(100, percent); IntakeR.setVelocity(100, percent); - //IntakeL.startSpinTo(5, rev); // grab preload - //IntakeR.startSpinTo(5, rev); + // IntakeL.startSpinTo(5, rev); // grab preload + // IntakeR.startSpinTo(5, rev); Drivetrain.setDriveVelocity(80, percent); Drivetrain.setTurnVelocity(100, percent); - //Drivetrain.driveFor(24 * 3.5, inches); // drive to the row of 4 (if on that tile) + // Drivetrain.driveFor(24 * 3.5, inches); // drive to the row of 4 (if on that + // tile) waitUntil(IntakeL.isDone() && IntakeR.isDone()); - //Drivetrain.turnFor(-90, degrees); // turn LEFT + // Drivetrain.turnFor(-90, degrees); // turn LEFT IntakeL.spin(forward); IntakeR.spin(forward); Drivetrain.setDriveVelocity(50, percent); @@ -217,16 +173,62 @@ void autonomous(void) { Drivetrain.turnFor(130, degrees); // turn RIGHT towards corner Drivetrain.setDriveVelocity(33, percent); Drivetrain.driveFor(24 * 0.8, inches); // go to corner - TiltMotor.spinTo(50 * 7, degrees); // tilt cubes + TiltMotor.spinTo(50 * 7, degrees); // tilt cubes IntakeL.setVelocity(20, percent); IntakeR.setVelocity(20, percent); IntakeL.startSpinFor(-3, rev); IntakeR.startSpinFor(-3, rev); // start pushing out cubes Drivetrain.setDriveVelocity(25, percent); Drivetrain.driveFor(reverse, 24 * 0.8, inches); // back away slowly - } +void playback() { + int y = 0; + y = instructions.find(" ", 5); + double vel = strtod(instructions.substr(0, y).c_str(), NULL); + instructions = instructions.substr(y + 1); + y = instructions.find(" ", 5); + double pos = strtod(instructions.substr(0, y).c_str(), NULL); + instructions = instructions.substr(y + 1); + Controller1.Screen.print(vel); + + // LeftDriveSmart.setVelocity(vel, percent); + // LeftDriveSmart.startSpinTo(pos, degrees); + + y = instructions.find(" ", 5); + double vel2 = strtod(instructions.substr(0, y).c_str(), NULL); + instructions = instructions.substr(y + 1); + y = instructions.find(" ", 5); + double pos2 = strtod(instructions.substr(0, y).c_str(), NULL); + instructions = instructions.substr(y + 1); + //Controller1.Screen.print(vel); + goToPos(LeftDriveSmart, vel, pos); + goToPos(RightDriveSmart, vel2, pos2); + + y = instructions.find(" ", 5); + vel = strtod(instructions.substr(0, y).c_str(), NULL); + instructions = instructions.substr(y + 1); + IntakeL.spin(forward, vel, percent); + IntakeR.spin(forward, vel, percent); + y = instructions.find(" ", 5); + vel = strtod(instructions.substr(0, y).c_str(), NULL); + instructions = instructions.substr(y + 1); + y = instructions.find(" ", 5); + pos = strtod(instructions.substr(0, y).c_str(), NULL); + instructions = instructions.substr(y + 1); + goToPos(LiftMotor, vel, pos); + goToPos(LiftMotor2, vel, pos); + //LiftMotor2.rotateTo(pos, degrees, vel, velocityUnits::pct, false); + + y = instructions.find(" ", 5); + vel = strtod(instructions.substr(0, y).c_str(), NULL); + instructions = instructions.substr(y + 1); + y = instructions.find(" ", 5); + pos = strtod(instructions.substr(0, y).c_str(), NULL); + goToPos(TiltMotor, vel, pos); + //TiltMotor.rotateTo(pos, degrees, vel, velocityUnits::pct, false); + //wait(10, msec); +} /*---------------------------------------------------------------------------*/ /* */ /* User Control Task */ @@ -242,14 +244,13 @@ void usercontrol(void) { LiftMotor.setVelocity(100, percent); LiftMotor2.setVelocity(100, percent); TiltMotor.setVelocity(30, percent); - TiltMotor2.setVelocity(30, percent); Controller1.Screen.clearScreen(); Controller1.Screen.setCursor(0, 0); Controller1.Screen.clearLine(); Controller1.Screen.print("Normal Driving "); bool recording = false; bool donerecording = false; - + timer tim2; // User control code here, inside the loop while (1) { // This is the main execution loop for the user control program. @@ -260,133 +261,139 @@ void usercontrol(void) { // Insert user code here. This is where you use the joystick values to // update your motors, etc. // ........................................................................ - if(Controller1.ButtonY.pressing()) { + if (Controller1.ButtonY.pressing()) { speedin = false; driveMultiplier = 0.33; Controller1.Screen.setCursor(0, 0); Controller1.Screen.clearLine(); Controller1.Screen.print("Precise Driving "); - } else if(Controller1.ButtonX.pressing()) { + } else if (Controller1.ButtonX.pressing()) { speedin = true; driveMultiplier = 1; - Controller1.Screen.setCursor(0, 0); + Controller1.Screen.setCursor(0, 0); Controller1.Screen.clearLine(); Controller1.Screen.print("Normal Driving "); } - if(Controller1.ButtonA.pressing() && !apressed) { + if (Controller1.ButtonA.pressing() && !apressed) { speedin = !speedin; apressed = true; - } - else if(!Controller1.ButtonA.pressing()) apressed = false; + } else if (!Controller1.ButtonA.pressing()) + apressed = false; - if(Controller1.ButtonB.pressing() && !bpressed) { - if(driveMultiplier == 1.0) { + if (Controller1.ButtonB.pressing() && !bpressed) { + if (driveMultiplier == 1.0) { driveMultiplier = 0.33; Controller1.Screen.setCursor(0, 0); - //Controller1.Screen.clearLine(); + // Controller1.Screen.clearLine(); Controller1.Screen.print("Precise Driving"); - } - else if(driveMultiplier == 0.33) { + } else if (driveMultiplier == 0.33) { driveMultiplier = 1; Controller1.Screen.setCursor(0, 0); - //Controller1.Screen.clearLine(); + // Controller1.Screen.clearLine(); Controller1.Screen.print("Normal Driving "); } bpressed = true; - } - else if(!Controller1.ButtonB.pressing()) bpressed = false; - //uint8_t * data = (uint8_t*)malloc(sizeof(uint8_t) * 5); - if(Controller1.ButtonLeft.pressing() && !recording) { + } else if (!Controller1.ButtonB.pressing()) + bpressed = false; + // uint8_t * data = (uint8_t*)malloc(sizeof(uint8_t) * 5); + if (Controller1.ButtonLeft.pressing() && !recording) { recording = true; - LeftDriveSmart.resetRotation(); + LeftDriveSmart.resetRotation(); RightDriveSmart.resetRotation(); LiftMotor.resetRotation(); LiftMotor2.resetRotation(); TiltMotor.resetRotation(); - TiltMotor2.resetRotation(); IntakeL.resetRotation(); IntakeR.resetRotation(); if (Brain.SDcard.isInserted()) { - // create a file with long filename + // create a file with long filename ofs.open("recording.txt", std::ofstream::out); } + tim2.clear(); } - if(Controller1.ButtonRight.pressing()) { + if (Controller1.ButtonRight.pressing()) { donerecording = true; } - if(recording) { - //count ++; + controllerBaseInput(); + if (recording) { + while (tim2.value() < 0.075) { + wait(1, msec); + } + tim2.clear(); + // count ++; std::string thisReading = ""; char output[12]; snprintf(output, 12, "%f", LeftDriveSmart.velocity(percent)); thisReading += output; thisReading += " "; - //output[12] = ""; + // output[12] = ""; snprintf(output, 12, "%f", LeftDriveSmart.rotation(degrees)); thisReading += output; thisReading += " "; - //output[12] = ""; + // output[12] = ""; snprintf(output, 12, "%f", RightDriveSmart.velocity(percent)); thisReading += output; thisReading += " "; - //output[12] = ""; + // output[12] = ""; snprintf(output, 12, "%f", RightDriveSmart.rotation(degrees)); thisReading += output; thisReading += " "; - //char output[12]; + // char output[12]; snprintf(output, 12, "%f", IntakeL.velocity(percent)); thisReading += output; thisReading += " "; - //output[12] = ""; + // output[12] = ""; snprintf(output, 12, "%f", LiftMotor.velocity(percent)); thisReading += output; thisReading += " "; - //output[12] = ""; + // output[12] = ""; snprintf(output, 12, "%f", LiftMotor.rotation(degrees)); thisReading += output; thisReading += " "; - //output[12] = ""; + // output[12] = ""; snprintf(output, 12, "%f", TiltMotor.velocity(percent)); thisReading += output; thisReading += " "; - //output[12] = ""; + // output[12] = ""; snprintf(output, 12, "%f", TiltMotor.rotation(degrees)); thisReading += output; thisReading += " \n"; ofs << thisReading; - //wait(30, msec); - //data = (uint8_t*)realloc(data, sizeof(uint8_t) * 5 * count + 5); - //uint8_t* thisReading; - //thisReading = new uint8_t[5]; - /*thisReading = {(uint8_t) (LeftDriveSmart.velocity(percent)+100), - (uint8_t) (RightDriveSmart.velocity(percent)+100), - (uint8_t) (IntakeL.velocity(percent)+100), - (uint8_t) (LiftMotor.velocity(percent)+100), - (uint8_t) (TiltMotor.velocity(percent)+100) + + // data = (uint8_t*)realloc(data, sizeof(uint8_t) * 5 * count + 5); + // uint8_t* thisReading; + // thisReading = new uint8_t[5]; + /*thisReading = {(uint8_t) (LeftDriveSmart.velocity(percent)+100), + (uint8_t) + (RightDriveSmart.velocity(percent)+100), (uint8_t) + (IntakeL.velocity(percent)+100), (uint8_t) + (LiftMotor.velocity(percent)+100), (uint8_t) + (TiltMotor.velocity(percent)+100) };*/ //*(thisReading) = (uint8_t) (LeftDriveSmart.velocity(percent)+100); //*(thisReading+1) = (uint8_t) (RightDriveSmart.velocity(percent)+100); //*(thisReading+2) = (uint8_t) (IntakeL.velocity(percent)+100); //*(thisReading+3) = (uint8_t) (LiftMotor.velocity(percent)+100); //*(thisReading+4) = (uint8_t) (TiltMotor.velocity(percent)+100); - //int startIndex = count * 5; - //for(int i = 0; i < 5; i++) { + // int startIndex = count * 5; + // for(int i = 0; i < 5; i++) { // data[startIndex + i] = thisReading[i]; //} - if(donerecording) { - //Brain.SDcard.savefile("recording.bin", data, sizeof(data)); + if (donerecording) { + // Brain.SDcard.savefile("recording.bin", data, sizeof(data)); recording = false; ofs.close(); } + } else { + wait(20, msec); // Sleep the task for a short amount of time to + // prevent wasted resources. } - controllerBaseInput(); - wait(50, msec); // Sleep the task for a short amount of time to - // prevent wasted resources. + // controllerBaseInput(); } } @@ -413,48 +420,87 @@ void STOPCODE_DEBUG() { IntakeR.stop(); IntakeL.stop(); TiltMotor.stop(); - TiltMotor2.stop(); - while(true) wait(20, msec); + while (true) + wait(20, msec); } +/*void goToPos(motor m, double vel, double pos) { + m.setVelocity(vel, percent); + if (true || pos > m.rotation(degrees)) { + if (vel > 0 && !m.isSpinning()) { + m.spin(forward); + } + } else { + m.stop(); + } + + if (true || pos < m.rotation(degrees)) { + if (vel < 0 && !m.isSpinning()) { + m.spin(forward); + } + } else { + m.stop(); + } +}*/ +void goToPos(motor m, double vel, double pos) { + m.setVelocity(vel, percent); + if (vel != 0 && !m.isSpinning()) { + m.spin(forward); + } +} void controllerBaseInput() { - - //int drivetrainLeftSideSpeed = Controller1.Axis3.position(); - //int drivetrainRightSideSpeed = Controller1.Axis2.position(); - double axisAngle = atan2(Controller1.Axis3.position(), Controller1.Axis4.position()); + + // int drivetrainLeftSideSpeed = Controller1.Axis3.position(); + // int drivetrainRightSideSpeed = Controller1.Axis2.position(); + double axisAngle = + atan2(Controller1.Axis3.position(), Controller1.Axis4.position()); axisAngle *= 180 / 3.1415926; - - //if(Controller1.Axis4.position() < 0) axisAngle += 180; - //Controller1.Screen.setCursor(0, 50); - //Controller1.Screen.clearLine(); - //Controller1.Screen.print(axisAngle); + + // if(Controller1.Axis4.position() < 0) axisAngle += 180; + // Controller1.Screen.setCursor(0, 50); + // Controller1.Screen.clearLine(); + // Controller1.Screen.print(axisAngle); int drivetrainLeftSideSpeed = Controller1.Axis3.position(); int drivetrainRightSideSpeed = Controller1.Axis4.position(); - int powerLevel = sqrt(Controller1.Axis3.position() * Controller1.Axis3.position() + Controller1.Axis4.position() * Controller1.Axis4.position()); + int powerLevel = + sqrt(Controller1.Axis3.position() * Controller1.Axis3.position() + + Controller1.Axis4.position() * Controller1.Axis4.position()); // left side - if(axisAngle < 90 && axisAngle >= 0) drivetrainLeftSideSpeed = powerLevel; - else if(axisAngle <= 181 && axisAngle >= 90) drivetrainLeftSideSpeed = int(powerLevel * (135 - axisAngle) / 45); - else if(axisAngle >= -181 && axisAngle <= -90) drivetrainLeftSideSpeed = -powerLevel; - else if(axisAngle > -90 && axisAngle < 0) drivetrainLeftSideSpeed = int(powerLevel * (45 + axisAngle) / 45); - - if(axisAngle <= 181 && axisAngle >= 90) drivetrainRightSideSpeed = powerLevel; - else if(axisAngle < 90 && axisAngle >= 0) drivetrainRightSideSpeed = -int(powerLevel * (45 - axisAngle) / 45); - else if(axisAngle > -90 && axisAngle < 0) drivetrainRightSideSpeed = -powerLevel; - else if(axisAngle >= -181 && axisAngle <= -90) drivetrainRightSideSpeed = -int(powerLevel * (135 + axisAngle) / 45); - - if(Controller1.Axis4.position() == 0 && Controller1.Axis3.position() > 0 && (int(axisAngle) == 90 || int(axisAngle) == 0)) { + if (axisAngle < 90 && axisAngle >= 0) + drivetrainLeftSideSpeed = powerLevel; + else if (axisAngle <= 181 && axisAngle >= 90) + drivetrainLeftSideSpeed = int(powerLevel * (135 - axisAngle) / 45); + else if (axisAngle >= -181 && axisAngle <= -90) + drivetrainLeftSideSpeed = -powerLevel; + else if (axisAngle > -90 && axisAngle < 0) + drivetrainLeftSideSpeed = int(powerLevel * (45 + axisAngle) / 45); + + if (axisAngle <= 181 && axisAngle >= 90) + drivetrainRightSideSpeed = powerLevel; + else if (axisAngle < 90 && axisAngle >= 0) + drivetrainRightSideSpeed = -int(powerLevel * (45 - axisAngle) / 45); + else if (axisAngle > -90 && axisAngle < 0) + drivetrainRightSideSpeed = -powerLevel; + else if (axisAngle >= -181 && axisAngle <= -90) + drivetrainRightSideSpeed = -int(powerLevel * (135 + axisAngle) / 45); + + if (Controller1.Axis4.position() == 0 && Controller1.Axis3.position() > 0 && + (int(axisAngle) == 90 || int(axisAngle) == 0)) { drivetrainLeftSideSpeed = powerLevel; drivetrainRightSideSpeed = powerLevel; } - if(Controller1.Axis4.position() == 0 && Controller1.Axis3.position() < 0 && (int(axisAngle) == -90 || int(axisAngle) == 0)) { + if (Controller1.Axis4.position() == 0 && Controller1.Axis3.position() < 0 && + (int(axisAngle) == -90 || int(axisAngle) == 0)) { drivetrainLeftSideSpeed = -powerLevel; drivetrainRightSideSpeed = -powerLevel; } - if(Controller1.Axis4.position() < 0 && (int(axisAngle) == 180 || int(axisAngle) == -180 || int(axisAngle) == 0)) { + if (Controller1.Axis4.position() < 0 && + (int(axisAngle) == 180 || int(axisAngle) == -180 || + int(axisAngle) == 0)) { drivetrainLeftSideSpeed = -powerLevel; drivetrainRightSideSpeed = powerLevel; } - int IntakeSpeed = Controller1.Axis2.position(); + int IntakeSpeed = Controller1.Axis2.position(); if (IntakeSpeed < 5 && IntakeSpeed > -5) { // check if the left motor has already been stopped if (IntakeStop) { @@ -465,34 +511,34 @@ void controllerBaseInput() { IntakeStop = false; } } else { - // reset the toggle so that the deadband code knows to stop the left motor next time the input is in the deadband range + // reset the toggle so that the deadband code knows to stop the left motor + // next time the input is in the deadband range IntakeStop = true; } - // only tell the left drive motor to spin if the values are not in the deadband range - if(speedin) { + // only tell the left drive motor to spin if the values are not in the + // deadband range + if (speedin) { Controller1.Screen.setCursor(2, 0); - //Controller1.Screen.clearLine(); + // Controller1.Screen.clearLine(); Controller1.Screen.print("Fast intake"); } else { Controller1.Screen.setCursor(2, 0); - //Controller1.Screen.clearLine(); + // Controller1.Screen.clearLine(); Controller1.Screen.print("Slow intake"); IntakeSpeed /= 4; } - if(Controller1.ButtonDown.pressing()) { + if (Controller1.ButtonDown.pressing()) { drivetrainLeftSideSpeed = -20; drivetrainRightSideSpeed = -20; IntakeStop = true; IntakeSpeed = -20; - } - else if(Controller1.ButtonUp.pressing()) { + } else if (Controller1.ButtonUp.pressing()) { drivetrainLeftSideSpeed = 20; drivetrainRightSideSpeed = 20; IntakeStop = true; IntakeSpeed = 20; - } - else { + } else { drivetrainLeftSideSpeed *= driveMultiplier; drivetrainRightSideSpeed *= driveMultiplier; } @@ -512,7 +558,8 @@ void controllerBaseInput() { DrivetrainLNeedsToBeStopped_Controller1 = false; } } else { - // reset the toggle so that the deadband code knows to stop the left motor next time the input is in the deadband range + // reset the toggle so that the deadband code knows to stop the left motor + // next time the input is in the deadband range DrivetrainLNeedsToBeStopped_Controller1 = true; } // check if the value is inside of the deadband range @@ -525,15 +572,18 @@ void controllerBaseInput() { DrivetrainRNeedsToBeStopped_Controller1 = false; } } else { - // reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range + // reset the toggle so that the deadband code knows to stop the right motor + // next time the input is in the deadband range DrivetrainRNeedsToBeStopped_Controller1 = true; } - // only tell the left drive motor to spin if the values are not in the deadband range + // only tell the left drive motor to spin if the values are not in the + // deadband range if (DrivetrainLNeedsToBeStopped_Controller1) { LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent); LeftDriveSmart.spin(forward); } - // only tell the right drive motor to spin if the values are not in the deadband range + // only tell the right drive motor to spin if the values are not in the + // deadband range if (DrivetrainRNeedsToBeStopped_Controller1) { RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent); RightDriveSmart.spin(forward); @@ -550,12 +600,14 @@ void controllerBaseInput() { } else if (!Controller1LeftShoulderControlMotorsStopped) { LiftMotor.stop(hold); LiftMotor2.stop(hold); - // set the toggle so that we don't constantly tell the motor to stop when the buttons are released + // set the toggle so that we don't constantly tell the motor to stop when + // the buttons are released Controller1LeftShoulderControlMotorsStopped = true; } - - // only tell the right drive motor to spin if the values are not in the deadband range - + + // only tell the right drive motor to spin if the values are not in the + // deadband range + // check the ButtonR1/ButtonR2 status to control Intake /*if (Controller1.ButtonR1.pressing()) { IntakeR.spin(forward); @@ -568,22 +620,20 @@ void controllerBaseInput() { } else if (!Controller1RightShoulderControlMotorsStopped) { IntakeR.stop(hold); IntakeL.stop(hold); - // set the toggle so that we don't constantly tell the motor to stop when the buttons are released - Controller1RightShoulderControlMotorsStopped = true; + // set the toggle so that we don't constantly tell the motor to stop when + the buttons are released Controller1RightShoulderControlMotorsStopped = true; }*/ // check the Right Shoulder Buttons status to control TiltMotor if (Controller1.ButtonR1.pressing()) { TiltMotor.spin(forward); - TiltMotor2.spin(forward); Controller1UpDownButtonsControlMotorsStopped = false; } else if (Controller1.ButtonR2.pressing()) { TiltMotor.spin(reverse); - TiltMotor2.spin(reverse); Controller1UpDownButtonsControlMotorsStopped = false; - } else if (!Controller1UpDownButtonsControlMotorsStopped){ + } else if (!Controller1UpDownButtonsControlMotorsStopped) { TiltMotor.stop(hold); - TiltMotor2.stop(hold); - // set the toggle so that we don't constantly tell the motor to stop when the buttons are released + // set the toggle so that we don't constantly tell the motor to stop when + // the buttons are released Controller1UpDownButtonsControlMotorsStopped = true; } } \ No newline at end of file