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.
562 lines
23 KiB
C++
562 lines
23 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* */
|
|
/* 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 10, 1
|
|
// LiftMotor motor 9
|
|
// TiltMotor motor 8
|
|
// Controller1 controller
|
|
// IntakeR motor 2
|
|
// IntakeL motor 3
|
|
// ---- END VEXCODE CONFIGURED DEVICES ----
|
|
|
|
#include "vex.h"
|
|
#include <cstdio>
|
|
#include <iostream>
|
|
#include <iomanip>
|
|
#include <fstream>
|
|
#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);
|
|
controller Controller1 = controller(primary);
|
|
motor IntakeR = motor(PORT17, 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 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
|
|
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.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);
|
|
instructions = instructions.substr(y + 1);
|
|
Controller1.Screen.print(vel);
|
|
RightDriveSmart.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);
|
|
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);
|
|
instructions = instructions.substr(y + 1);
|
|
TiltMotor.rotateTo(pos, degrees, vel, velocityUnits::pct, false);
|
|
wait(20, msec);
|
|
}
|
|
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
|
|
|
|
}
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* */
|
|
/* 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) {
|
|
LiftMotor.setVelocity(100, percent);
|
|
LiftMotor2.setVelocity(100, percent);
|
|
TiltMotor.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;
|
|
//uint8_t * data = (uint8_t*)malloc(sizeof(uint8_t) * 5);
|
|
|
|
// 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();
|
|
IntakeL.resetRotation();
|
|
IntakeR.resetRotation();
|
|
if (Brain.SDcard.isInserted()) {
|
|
// create a file with long filename
|
|
ofs.open("recording.txt", std::ofstream::out);
|
|
}
|
|
}
|
|
if(Controller1.ButtonRight.pressing()) {
|
|
donerecording = true;
|
|
}
|
|
if(recording) {
|
|
//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();
|
|
}
|
|
}
|
|
controllerBaseInput();
|
|
wait(20, msec); // Sleep the task for a short amount of time to
|
|
// prevent wasted resources.
|
|
}
|
|
}
|
|
|
|
//
|
|
// 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();
|
|
while(true) wait(20, msec);
|
|
}
|
|
|
|
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;
|
|
}
|
|
if(Controller1.ButtonDown.pressing()) {
|
|
drivetrainLeftSideSpeed = -33;
|
|
drivetrainRightSideSpeed = -33;
|
|
}
|
|
else if(Controller1.ButtonUp.pressing()) {
|
|
drivetrainLeftSideSpeed = 33;
|
|
drivetrainRightSideSpeed = 33;
|
|
}
|
|
else {
|
|
drivetrainLeftSideSpeed *= driveMultiplier;
|
|
drivetrainRightSideSpeed *= driveMultiplier;
|
|
}
|
|
// 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;
|
|
}
|
|
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");
|
|
} else {
|
|
Controller1.Screen.setCursor(2, 0);
|
|
//Controller1.Screen.clearLine();
|
|
Controller1.Screen.print("Slow intake");
|
|
IntakeSpeed /= 4;
|
|
}
|
|
if (IntakeStop) {
|
|
IntakeL.setVelocity(IntakeSpeed, percent);
|
|
IntakeR.setVelocity(IntakeSpeed, percent);
|
|
IntakeL.spin(forward);
|
|
IntakeR.spin(forward);
|
|
}
|
|
// 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);
|
|
Controller1UpDownButtonsControlMotorsStopped = false;
|
|
} else if (Controller1.ButtonR2.pressing()) {
|
|
TiltMotor.spin(reverse);
|
|
Controller1UpDownButtonsControlMotorsStopped = false;
|
|
} else if (!Controller1UpDownButtonsControlMotorsStopped){
|
|
TiltMotor.stop(hold);
|
|
// set the toggle so that we don't constantly tell the motor to stop when the buttons are released
|
|
Controller1UpDownButtonsControlMotorsStopped = true;
|
|
}
|
|
} |