shark
/
38535A
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
master
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'master'
${ noResults }
38535A/src/main.cpp

684 lines
26 KiB
C++
Raw Permalink Blame History

/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: VEX */
/* Created: Thu Sep 26 2019 */
/* Description: Competition Template */
/* */
/*----------------------------------------------------------------------------*/
// ---- 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
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
#include <cstdio>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>
using namespace vex;
bool speedin = true;
double driveMultiplier = 1.0;
bool apressed = false;
bool bpressed = false;
// A global instance of competition
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);
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(PORT15, ratio18_1, false);
motor IntakeL = motor(PORT8, ratio18_1, true);
bool Controller1LeftShoulderControlMotorsStopped = true;
bool Controller1RightShoulderControlMotorsStopped = true;
bool Controller1UpDownButtonsControlMotorsStopped = true;
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
bool DrivetrainRNeedsToBeStopped_Controller1 = true;
bool IntakeStop = true;
void controllerBaseInput();
void STOPCODE_DEBUG();
// define your global instances of motors and other devices here
/*---------------------------------------------------------------------------*/
/* Pre-Autonomous Functions */
/* */
/* You may want to perform some actions before the competition starts. */
/* Do them in the following function. You must return from this function */
/* or the autonomous and usercontrol tasks will not be started. This */
/* function is only called once after the V5 has been powered on and */
/* not every time that the robot is disabled. */
/*---------------------------------------------------------------------------*/
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
/*---------------------------------------------------------------------------*/
/* */
/* 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. */
/*---------------------------------------------------------------------------*/
std::ifstream ifs;
void goToPos(motor m, double vel, double pos);
void playback();
int filenum = 1;
std::string instructions;
void autonomous(void) {
// ..........................................................................
// Insert autonomous user code here.
// ..........................................................................
/*STOPCODE_DEBUG();
LiftMotor.setVelocity(100, percent);
LiftMotor2.setVelocity(100, percent);
TiltMotor.setVelocity(30, percent);
//Drivetrain.driveFor(6, inches);*/
LiftMotor.resetRotation();
LiftMotor2.resetRotation();
TiltMotor.resetRotation();
IntakeL.resetRotation();
IntakeR.resetRotation();
LeftDriveSmart.resetRotation();
RightDriveSmart.resetRotation();
LiftMotor.setStopping(hold);
LiftMotor2.setStopping(hold);
// STOPCODE_DEBUG();
// uint8_t * instructions = nullptr;
// Brain.SDcard.loadfile("recording.bin", instructions, 2500); // size in
// bytes
char filename[1] = "";
snprintf(filename, 2, "%d", filenum);
std::string fn = filename;
fn += ".txt";
ifs.open(fn, std::ofstream::in);
Controller1.Screen.print("File opened");
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.driveFor(15, inches, false);
wait(2, sec);
Drivetrain.setDriveVelocity(80, percent);
Drivetrain.driveFor(reverse, 16, inches);
wait(1, sec);
// Drivetrain.turnFor(-45, degrees);
STOPCODE_DEBUG();
LiftMotor.startSpinTo(40 * 7, degrees); // unlock everything
LiftMotor2.startSpinTo(40 * 7, degrees);
waitUntil(LiftMotor.isDone() && LiftMotor2.isDone());
TiltMotor.startSpinTo(45 * 7, degrees);
LiftMotor.startSpinTo(10 * 7, degrees);
LiftMotor2.startSpinTo(10 * 7, degrees);
waitUntil(LiftMotor.isDone() && LiftMotor2.isDone() && TiltMotor.isDone());
IntakeL.setVelocity(100, percent);
IntakeR.setVelocity(100, percent);
IntakeL.startSpinTo(-5, rev);
IntakeR.startSpinTo(-5, rev);
TiltMotor.startSpinTo(5, degrees);
waitUntil(TiltMotor.isDone());
// STOPCODE_DEBUG();
STOPCODE_DEBUG();
IntakeL.setVelocity(100, percent);
IntakeR.setVelocity(100, percent);
// 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)
waitUntil(IntakeL.isDone() && IntakeR.isDone());
// Drivetrain.turnFor(-90, degrees); // turn LEFT
IntakeL.spin(forward);
IntakeR.spin(forward);
Drivetrain.setDriveVelocity(50, percent);
Drivetrain.driveFor(24 * 0.8, inches); // grab 4 cubes
wait(1000, msec);
IntakeL.stop(hold);
IntakeR.stop(hold); // hold the cubes
STOPCODE_DEBUG();
Drivetrain.driveFor(reverse, 24 * 1.6, inches); // go back
Drivetrain.setTurnVelocity(50, percent);
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
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);
goToPos(TiltMotor2, -vel, pos);
//TiltMotor.rotateTo(pos, degrees, vel, velocityUnits::pct, false);
//wait(10, msec);
}
/*---------------------------------------------------------------------------*/
/* */
/* 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. */
/*---------------------------------------------------------------------------*/
std::ofstream ofs;
void usercontrol(void) {
Controller1.Screen.setCursor(4, 0);
// Controller1.Screen.clearLine();
Controller1.Screen.print(filenum);
LiftMotor.setVelocity(100, percent);
LiftMotor.stop();
LiftMotor2.setVelocity(100, percent);
LiftMotor2.stop();
TiltMotor.setVelocity(30, percent);
TiltMotor2.setVelocity(30, percent);
TiltMotor.stop();
TiltMotor2.stop();
LeftDriveSmart.stop();
RightDriveSmart.stop();
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.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.
// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................
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()) {
speedin = true;
driveMultiplier = 1;
Controller1.Screen.setCursor(0, 0);
Controller1.Screen.clearLine();
Controller1.Screen.print("Normal Driving ");
}
if (Controller1.ButtonA.pressing() && !apressed) {
speedin = !speedin;
apressed = true;
} else if (!Controller1.ButtonA.pressing())
apressed = false;
if (Controller1.ButtonB.pressing() && !bpressed) {
if (driveMultiplier == 1.0) {
driveMultiplier = 0.33;
Controller1.Screen.setCursor(0, 0);
// Controller1.Screen.clearLine();
Controller1.Screen.print("Precise Driving");
} else if (driveMultiplier == 0.33) {
driveMultiplier = 1;
Controller1.Screen.setCursor(0, 0);
// 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) {
recording = true;
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
char filename[1] = "";
snprintf(filename, 2, "%d", filenum);
std::string fn = filename;
fn += ".txt";
ofs.open(fn, std::ofstream::out);
}
tim2.clear();
}
if (Controller1.ButtonRight.pressing()) {
donerecording = true;
}
if (Controller1.ButtonUp.pressing()) {
filenum ++;
Controller1.Screen.setCursor(4, 0);
// Controller1.Screen.clearLine();
Controller1.Screen.print(filenum);
wait(250, msec);
}
if (Controller1.ButtonDown.pressing()) {
filenum --;
Controller1.Screen.setCursor(4, 0);
// Controller1.Screen.clearLine();
Controller1.Screen.print(filenum);
wait(250, msec);
}
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] = "";
snprintf(output, 12, "%f", LeftDriveSmart.rotation(degrees));
thisReading += output;
thisReading += " ";
// output[12] = "";
snprintf(output, 12, "%f", RightDriveSmart.velocity(percent));
thisReading += output;
thisReading += " ";
// output[12] = "";
snprintf(output, 12, "%f", RightDriveSmart.rotation(degrees));
thisReading += output;
thisReading += " ";
// char output[12];
snprintf(output, 12, "%f", IntakeL.velocity(percent));
thisReading += output;
thisReading += " ";
// output[12] = "";
snprintf(output, 12, "%f", LiftMotor.velocity(percent));
thisReading += output;
thisReading += " ";
// output[12] = "";
snprintf(output, 12, "%f", LiftMotor.rotation(degrees));
thisReading += output;
thisReading += " ";
// output[12] = "";
snprintf(output, 12, "%f", TiltMotor.velocity(percent));
thisReading += output;
thisReading += " ";
// output[12] = "";
snprintf(output, 12, "%f", TiltMotor.rotation(degrees));
thisReading += output;
thisReading += " \n";
ofs << thisReading;
// 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++) {
// data[startIndex + i] = thisReading[i];
//}
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();
}
}
//
// Main will set up the competition functions and callbacks.
//
int main() {
// Set up callbacks for autonomous and driver control periods.
Competition.autonomous(autonomous);
Competition.drivercontrol(usercontrol);
// Run the pre-autonomous function.
pre_auton();
// Prevent main from exiting with an infinite loop.
while (true) {
wait(100, msec);
}
}
void STOPCODE_DEBUG() {
Drivetrain.stop();
LiftMotor.stop();
LiftMotor2.stop();
IntakeR.stop();
IntakeL.stop();
TiltMotor.stop();
TiltMotor2.stop();
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());
axisAngle *= 180 / 3.1415926;
// 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());
// 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)) {
drivetrainLeftSideSpeed = powerLevel;
drivetrainRightSideSpeed = powerLevel;
}
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)) {
drivetrainLeftSideSpeed = -powerLevel;
drivetrainRightSideSpeed = powerLevel;
}
int IntakeSpeed = Controller1.Axis2.position();
if (IntakeSpeed < 5 && IntakeSpeed > -5) {
// check if the left motor has already been stopped
if (IntakeStop) {
// stop the left drive motor
IntakeL.stop();
IntakeR.stop();
// tell the code that the left motor has been stopped
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
IntakeStop = true;
}
// 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.print("Fast intake");
Controller1.Screen.setCursor(4, 0);
Controller1.Screen.print(filenum);
} else {
Controller1.Screen.setCursor(2, 0);
// Controller1.Screen.clearLine();
Controller1.Screen.print("Slow intake");
Controller1.Screen.setCursor(4, 0);
Controller1.Screen.print(filenum);
IntakeSpeed /= 4;
}
/*if (Controller1.ButtonDown.pressing()) {
drivetrainLeftSideSpeed = -20;
drivetrainRightSideSpeed = -20;
IntakeStop = true;
IntakeSpeed = -20;
} else if (Controller1.ButtonUp.pressing()) {
drivetrainLeftSideSpeed = 20;
drivetrainRightSideSpeed = 20;
IntakeStop = true;
IntakeSpeed = 20;
} else {*/
drivetrainLeftSideSpeed *= driveMultiplier;
drivetrainRightSideSpeed *= driveMultiplier;
//}
if (IntakeStop) {
IntakeL.setVelocity(IntakeSpeed, percent);
IntakeR.setVelocity(IntakeSpeed, percent);
IntakeL.spin(forward);
IntakeR.spin(forward);
}
// check if the value is inside of the deadband range
if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
// check if the left motor has already been stopped
if (DrivetrainLNeedsToBeStopped_Controller1) {
// stop the left drive motor
LeftDriveSmart.stop();
// tell the code that the left motor has been stopped
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
DrivetrainLNeedsToBeStopped_Controller1 = true;
}
// check if the value is inside of the deadband range
if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
// check if the right motor has already been stopped
if (DrivetrainRNeedsToBeStopped_Controller1) {
// stop the right drive motor
RightDriveSmart.stop();
// tell the code that the right motor has been stopped
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
DrivetrainRNeedsToBeStopped_Controller1 = true;
}
// 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
if (DrivetrainRNeedsToBeStopped_Controller1) {
RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
RightDriveSmart.spin(forward);
}
// check the ButtonL1/ButtonL2 status to control LiftMotor
if (Controller1.ButtonL1.pressing()) {
LiftMotor.spin(forward);
LiftMotor2.spin(forward);
Controller1LeftShoulderControlMotorsStopped = false;
} else if (Controller1.ButtonL2.pressing()) {
LiftMotor.spin(reverse);
LiftMotor2.spin(reverse);
Controller1LeftShoulderControlMotorsStopped = false;
} 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
Controller1LeftShoulderControlMotorsStopped = true;
}
// 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);
IntakeL.spin(forward);
Controller1RightShoulderControlMotorsStopped = false;
} else if (Controller1.ButtonR2.pressing()) {
IntakeR.spin(reverse);
IntakeL.spin(reverse);
Controller1RightShoulderControlMotorsStopped = false;
} 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;
}*/
// 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) {
TiltMotor.stop(hold);
TiltMotor.stop(hold);
// set the toggle so that we don't constantly tell the motor to stop when
// the buttons are released
Controller1UpDownButtonsControlMotorsStopped = true;
}
}
Reference in New Issue View Git Blame Copy Permalink