make it actually work now
This commit is contained in:
Cole Deck
parent
a64b12e7a7
commit
326a85abd8
410
src/main.cpp
410
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 <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 < 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 <= 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;
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user