shark
/
38535A
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

make it actually work now

Browse Source
master
Cole Deck 4 years ago
parent a64b12e7a7
commit 326a85abd8
1 changed files with 231 additions and 181 deletions
Show Stats Download Patch File Download Diff File

412
src/main.cpp
Unescape Escape View File

@ -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 <cstdio>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>
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;
}
}
Write Preview
Loading…
Cancel
Save