From bc5406a4104cd6a24e378c53a73d4e53b28e74d5 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 17 Nov 2019 20:58:12 -0500 Subject: [PATCH 1/8] Split-arcade drive --- compile_commands.json | 18 +++++++++--------- src/autonomous.cpp | 5 +++++ src/opcontrol.cpp | 25 +++++++++++-------------- 3 files changed, 25 insertions(+), 23 deletions(-) diff --git a/compile_commands.json b/compile_commands.json index d347dc3..c49582f 100644 --- a/compile_commands.json +++ b/compile_commands.json @@ -28,11 +28,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "bin/initialize.cpp.o", - "src\\initialize.cpp" + "bin/pid.cpp.o", + "src\\pid.cpp" ], "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\initialize.cpp" + "file": "src\\pid.cpp" }, { "arguments": [ @@ -63,11 +63,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "bin/pid.cpp.o", - "src\\pid.cpp" + "bin/autonomous.cpp.o", + "src\\autonomous.cpp" ], "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\pid.cpp" + "file": "src\\autonomous.cpp" }, { "arguments": [ @@ -98,11 +98,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "bin/autonomous.cpp.o", - "src\\autonomous.cpp" + "bin/initialize.cpp.o", + "src\\initialize.cpp" ], "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\autonomous.cpp" + "file": "src\\initialize.cpp" }, { "arguments": [ diff --git a/src/autonomous.cpp b/src/autonomous.cpp index 5eda0a2..9e91cf4 100644 --- a/src/autonomous.cpp +++ b/src/autonomous.cpp @@ -127,5 +127,10 @@ void autonomous() { left_wheels_2.move(-20); right_wheels_1.move(-20); right_wheels_2.move(-20); + delay(1000); + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); } diff --git a/src/opcontrol.cpp b/src/opcontrol.cpp index 3b872b9..51af7fe 100644 --- a/src/opcontrol.cpp +++ b/src/opcontrol.cpp @@ -30,21 +30,18 @@ Motor angler (MOTOR5, 1); void drive(void*) { + int power = master.get_analog(ANALOG_LEFT_Y); + int turn = master.get_analog(ANALOG_RIGHT_X); //loop reads the joystick controlls and powers the motors accordingly - while(true) { - int powerLeft = master.get_analog(ANALOG_LEFT_Y); - int powerRight = master.get_analog(ANALOG_RIGHT_Y); - - //If there is a small difference, then program will assume person wants to go straight and will set power to motors as equal - if((powerLeft - powerRight) >= -7 && (powerLeft - powerRight) <= 7) - powerLeft = powerRight; - - left_wheels_1.move(powerLeft); - left_wheels_2.move(powerLeft); - right_wheels_1.move(powerRight); - right_wheels_2.move(powerRight); - delay(20); - } + while (true) { + int left = power + turn; + int right = power - turn; + left_wheels_1.move(left); + left_wheels_2.move(left); + right_wheels_1.move(right); + right_wheels_2.move(right); + delay(20); + } } void intake(void*) { From d845ef1cbdd89a64f362ef02a2a46e4a73b129ad Mon Sep 17 00:00:00 2001 From: unknown Date: Mon, 18 Nov 2019 22:32:55 -0500 Subject: [PATCH 2/8] Actual working drive code --- src/opcontrol.cpp | 92 +++++++++++++++++++++++++---------------------- 1 file changed, 50 insertions(+), 42 deletions(-) diff --git a/src/opcontrol.cpp b/src/opcontrol.cpp index 51af7fe..de5524f 100644 --- a/src/opcontrol.cpp +++ b/src/opcontrol.cpp @@ -30,18 +30,18 @@ Motor angler (MOTOR5, 1); void drive(void*) { - int power = master.get_analog(ANALOG_LEFT_Y); - int turn = master.get_analog(ANALOG_RIGHT_X); //loop reads the joystick controlls and powers the motors accordingly while (true) { - int left = power + turn; - int right = power - turn; - left_wheels_1.move(left); - left_wheels_2.move(left); - right_wheels_1.move(right); - right_wheels_2.move(right); - delay(20); - } + int power = master.get_analog(ANALOG_LEFT_Y); + int turn = master.get_analog(ANALOG_RIGHT_X); + int left = power + turn; + int right = power - turn; + left_wheels_1.move(left); + left_wheels_2.move(left); + right_wheels_1.move(right); + right_wheels_2.move(right); + delay(20); + } } void intake(void*) { @@ -114,53 +114,61 @@ void anglerMove(void*) { void towerScore(void*) { - if(master.get_digital(DIGITAL_A)) { + while(true) { + + if(master.get_digital(DIGITAL_A)) { + + mutex.take(TIMEOUT_MAX); - mutex.take(TIMEOUT_MAX); + arm.set_zero_position(0); + arm.move_absolute(100, 100); //change values later to not damage robot! + while (!((arm.get_position() < 105) && (arm.get_position() > 95))) { + delay(20); + } - arm.set_zero_position(0); - arm.move_absolute(100, 100); //change values later to not damage robot! - while (!((arm.get_position() < 105) && (arm.get_position() > 95))) { - delay(20); - } + intake_left.move_relative(360, -100); //change values later to not damage robot! + intake_right.move_relative(360, -100); //change values later to not damage robot! + while (!((intake_left.get_position() < 365) && (intake_left.get_position() > 355))) { + delay(20); + } - intake_left.move_relative(360, -100); //change values later to not damage robot! - intake_right.move_relative(360, -100); //change values later to not damage robot! - while (!((intake_left.get_position() < 365) && (intake_left.get_position() > 355))) { - delay(20); - } + arm.move_absolute(0, -100); //change values later to not damage robot! + while (!((arm.get_position() < 5) && (arm.get_position() > -5))) { + delay(20); + } - arm.move_absolute(0, -100); //change values later to not damage robot! - while (!((arm.get_position() < 5) && (arm.get_position() > -5))) { - delay(20); - } + mutex.give(); + } - mutex.give(); } } void stacker(void*) { - if(master.get_digital(DIGITAL_X)) { + while(true) { - mutex.take(TIMEOUT_MAX); + if(master.get_digital(DIGITAL_X)) { - angler.set_zero_position(0); - angler.move_absolute(100, 100); //change values later to not damage robot! - while (!((angler.get_position() < 105) && (angler.get_position() > 95))) { - delay(20); - } + mutex.take(TIMEOUT_MAX); - left_wheels_1.move_relative(45, 50); //change values later to not damage robot! - left_wheels_2.move_relative(45, 50); //change values later to not damage robot! - right_wheels_1.move_relative(45, 50); //change values later to not damage robot! - right_wheels_2.move_relative(45, 50); //change values later to not damage robot! - while (!((left_wheels_1.get_position() < 50) && (left_wheels_1.get_position() > 40))) { - delay(20); - } + angler.set_zero_position(0); + angler.move_absolute(100, 100); //change values later to not damage robot! + while (!((angler.get_position() < 105) && (angler.get_position() > 95))) { + delay(20); + } + + left_wheels_1.move_relative(45, 50); //change values later to not damage robot! + left_wheels_2.move_relative(45, 50); //change values later to not damage robot! + right_wheels_1.move_relative(45, 50); //change values later to not damage robot! + right_wheels_2.move_relative(45, 50); //change values later to not damage robot! + while (!((left_wheels_1.get_position() < 50) && (left_wheels_1.get_position() > 40))) { + delay(20); + } + + mutex.give(); + } - mutex.give(); } } From 6792aa5b2fcf266d5bdefd88690254055d17d5cd Mon Sep 17 00:00:00 2001 From: Kirbydude27 Date: Fri, 22 Nov 2019 23:36:56 -0500 Subject: [PATCH 3/8] Last commits for the road --- compile_commands.json | 63 ++++++-- include/main.h | 11 ++ include/motors.h | 16 ++ include/pid.h | 25 +++ project.pros | 7 +- src/autonomous.cpp | 344 ++++++++++++++++++++++++++---------------- src/initialize.cpp | 4 - src/motors.cpp | 12 ++ src/opcontrol.cpp | 84 ++++++----- src/pid.cpp | 24 ++- 10 files changed, 392 insertions(+), 198 deletions(-) create mode 100644 include/motors.h create mode 100644 include/pid.h create mode 100644 src/motors.cpp diff --git a/compile_commands.json b/compile_commands.json index c49582f..d642a9a 100644 --- a/compile_commands.json +++ b/compile_commands.json @@ -28,11 +28,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "bin/pid.cpp.o", - "src\\pid.cpp" + "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/opcontrol.cpp.o", + "src\\opcontrol.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\pid.cpp" + "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", + "file": "src\\opcontrol.cpp" }, { "arguments": [ @@ -63,11 +63,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "bin/autonomous.cpp.o", - "src\\autonomous.cpp" + "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/motors.cpp.o", + "src\\motors.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\autonomous.cpp" + "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", + "file": "src\\motors.cpp" }, { "arguments": [ @@ -98,10 +98,45 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "bin/initialize.cpp.o", + "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/pid.cpp.o", + "src\\pid.cpp" + ], + "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", + "file": "src\\pid.cpp" + }, + { + "arguments": [ + "clang++", + "-c", + "-target", + "armv7ar-none-none-eabi", + "-fno-ms-extensions", + "-fno-ms-compatibility", + "-fno-delayed-template-parsing", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/../../../../arm-none-eabi/include/c++/7.2.1", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/../../../../arm-none-eabi/include/c++/7.2.1/arm-none-eabi/thumb/v7-ar/fpv3/softfp", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/../../../../arm-none-eabi/include/c++/7.2.1/backward", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/include", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/include-fixed", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/../../../../arm-none-eabi/include", + "-iquote./include", + "-iquote./include/./", + "-mcpu=cortex-a9", + "-mfpu=neon-fp16", + "-mfloat-abi=softfp", + "-D_POSIX_THREADS", + "-D_UNIX98_THREAD_MUTEX_ATTRIBUTES", + "-Os", + "-funwind-tables", + "-ffunction-sections", + "-fdata-sections", + "-fdiagnostics-color", + "--std=gnu++17", + "-o", + "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/initialize.cpp.o", "src\\initialize.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", + "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", "file": "src\\initialize.cpp" }, { @@ -133,10 +168,10 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "bin/opcontrol.cpp.o", - "src\\opcontrol.cpp" + "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/autonomous.cpp.o", + "src\\autonomous.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\opcontrol.cpp" + "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", + "file": "src\\autonomous.cpp" } ] \ No newline at end of file diff --git a/include/main.h b/include/main.h index c995433..62481cc 100644 --- a/include/main.h +++ b/include/main.h @@ -15,6 +15,17 @@ #ifndef _PROS_MAIN_H_ #define _PROS_MAIN_H_ +//defines the ports for each motor +#define MOTOR1 2 //L1 +#define MOTOR2 3 //L2 +#define MOTOR3 11 //R1 +#define MOTOR4 12 //R2 + +#define MOTOR5 13 //Angler +#define MOTOR6 4 //Arm +#define MOTOR7 5 //LIntake +#define MOTOR8 14 //RIntake + /** * If defined, some commonly used enums will have preprocessor macros which give * a shorter, more convenient naming pattern. If this isn't desired, simply diff --git a/include/motors.h b/include/motors.h new file mode 100644 index 0000000..470683d --- /dev/null +++ b/include/motors.h @@ -0,0 +1,16 @@ +#include "main.h" + +#ifndef _MOTORS_H_ +#define _MOTORS_H_ + +extern pros::Motor left_wheels_1; //L1 +extern pros::Motor right_wheels_1; //R1 +extern pros::Motor left_wheels_2; //L2 +extern pros::Motor right_wheels_2; //R2 + +extern pros::Motor intake_left; +extern pros::Motor intake_right; +extern pros::Motor arm; +extern pros::Motor angler; + +#endif diff --git a/include/pid.h b/include/pid.h new file mode 100644 index 0000000..076dba3 --- /dev/null +++ b/include/pid.h @@ -0,0 +1,25 @@ +#ifndef _PID_H_ +#define _PID_H_ + +class PIDImpl; +class PID +{ + public: + // Kp - proportional gain + // Ki - Integral gain + // Kd - derivative gain + // dt - loop interval time + // max - maximum value of manipulated variable + // min - minimum value of manipulated variable + PID( double dt, double max, double min, double Kp, double Kd, double Ki ); + void resetError(); + + // Returns the manipulated variable given a setpoint and current process value + double calculate( double setpoint, double pv ); + ~PID(); + + private: + PIDImpl *pimpl; +}; + +#endif diff --git a/project.pros b/project.pros index 781f495..2f0df20 100644 --- a/project.pros +++ b/project.pros @@ -248,6 +248,11 @@ "version": "3.3.13" } }, - "upload_options": {} + "upload_options": { + "compress_bin": true, + "description": "Created with PROS", + "remote_name": "Robocheck", + "slot": 2 + } } } \ No newline at end of file diff --git a/src/autonomous.cpp b/src/autonomous.cpp index 9e91cf4..08e6504 100644 --- a/src/autonomous.cpp +++ b/src/autonomous.cpp @@ -1,136 +1,220 @@ #include "main.h" - #include +#include "motors.h" -//define ports for motors -#define MOTOR1 2 //L1 -#define MOTOR2 3 //L2 -#define MOTOR3 11 //R1 -#define MOTOR4 12 //R2 - -#define MOTOR5 13 //Angler -#define MOTOR6 4 //Arm -#define MOTOR7 5 //LIntake -#define MOTOR8 14 //RIntake - -using namespace pros; - -void expand() { - Motor arm(MOTOR6, 1); - Motor angler(MOTOR5, 1); - int armPosition = 0; //SET A VALUE - int anglerPosition = 0; //SET A VALUE - - //Moves motors to let robot expand. I DON'T WANT TO BREAK THE MOTORS SO WE HAVE TO CHANGE THE VALUES LATER - arm.move_absolute(armPosition, 50); //change values later - angler.move_absolute(anglerPosition, 50); //change values later - - //Moves motors to original position - arm.move_absolute(armPosition, -50); //change values later - angler.move_absolute(anglerPosition, -50); //change values later - - delay(20); -} - -//Will be used to call functions which will run the robot void autonomous() { - //resetPosition(gPosition); - //pros::Task task1(trackPositionTask, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "TrackingPosition"); - - //Define motors - Motor left_wheels_1(MOTOR1, 0); //L1 - Motor right_wheels_1(MOTOR3, 1); //R1 - Motor left_wheels_2(MOTOR2, 1); //L2 - Motor right_wheels_2(MOTOR4, 0); //R2 - Motor intake_left(MOTOR7, 0); - Motor intake_right(MOTOR8, 1); - Motor angler(MOTOR5, 1); - Motor arm(MOTOR6, 1); - - expand(); - - //basically, robot moves to cubes, picks up, stops to let cube go up, moves towards next cube, picks up and stops, and it repeats that. We can change #of repeats or if the robot has to stop at all - left_wheels_1.move(70); - left_wheels_2.move(70); - right_wheels_1.move(70); - right_wheels_2.move(70); - intake_left.move(100); - intake_right.move(100); - delay(1000); //idk change this value later - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - delay(1000); //idk change this value later - left_wheels_1.move(20); - left_wheels_2.move(20); - right_wheels_1.move(20); - right_wheels_2.move(20); - delay(1000); //idk change this value later - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - delay(1000); //idk change this value later - left_wheels_1.move(20); - left_wheels_2.move(20); - right_wheels_1.move(20); - right_wheels_2.move(20); - delay(1000); //idk change this value later - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - delay(1000); - intake_left.move(0); - intake_right.move(0); - - //Robot turns around CCW - left_wheels_1.move(-70); - left_wheels_2.move(-70); - right_wheels_1.move(70); - right_wheels_2.move(70); - delay(1000); //idk change this value later - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - - //Robot moves to the goal zone - left_wheels_1.move(70); - left_wheels_2.move(70); - right_wheels_1.move(70); - right_wheels_2.move(70); - delay(1000); //idk change this value later - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - - //THERE IS A CHANCE WE NEED TO DO THe last part DIFFERENTLY SO THAT THE ROBOT IS ALIGNED MORE PROPERLY. LET'S JUST PRAY THAT THIS WORKS - - //Robot places cubes in the goal zone - arm.move(30); //arm moves out to protect the blocks from falling - angler.move(30); - delay(1000); //idk change this value later - arm.move(0); - delay(1000); //idl change this value later - angler.move(0); - delay(100); - arm.move(100); //moves arms all the way up so that they go over the blocks (hopefully)... if they don't, we will have a very tricky situation on our hands... - delay(1000); //idk change this value later - arm.move(0); - - //Robot moves away from the stack :) - left_wheels_1.move(-20); - left_wheels_2.move(-20); - right_wheels_1.move(-20); - right_wheels_2.move(-20); - delay(1000); - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - + // left_wheels_1.tare_position(); + // left_wheels_2.tare_position(); + // right_wheels_1.tare_position(); + // right_wheels_2.tare_position(); + // + // left_wheels_1.move_absolute(450, 100); + // left_wheels_2.move_absolute(450, 100); + // right_wheels_1.move_absolute(450, 100); + // right_wheels_2.move_absolute(450, 100); + // while (!((left_wheels_1.get_position() < 0.6) && (left_wheels_1.get_position() > 0.5))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // left_wheels_1.move(0); + // left_wheels_2.move(0); + // right_wheels_1.move(0); + // right_wheels_2.move(0); + + arm.tare_position(); + arm.move_absolute(400, 100); + while (!((arm.get_position() < 405) && (arm.get_position() > 395))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(2); + } + angler.tare_position(); + angler.move_absolute(-50, 100); + while (!((angler.get_position() < -45) && (angler.get_position() > -55))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(2); + } + arm.move_absolute(0, -70); + while (!((arm.get_position() < 5) && (arm.get_position() > -5))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(2); + } + angler.move_absolute(0, -70); + while (!((angler.get_position() < 5) && (angler.get_position() > -5))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(2); + } + // + // left_wheels_1_2.move_absolute(1, 100); + // left_wheels_2_2.move_absolute(1, 100); + // right_wheels_1_2.move_absolute(1, 100); + // right_wheels_2_2.move_absolute(1, 100); + // while (!((left_wheels_1_2.get_position() < 1.05) && (left_wheels_1_2.get_position() > 0.95))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // left_wheels_1_2.move(0); + // left_wheels_2_2.move(0); + // right_wheels_1_2.move(0); + // right_wheels_2_2.move(0); + // + // //sets current positions to zero + // left_wheels_1_2.tare_position(); + // left_wheels_2_2.tare_position(); + // right_wheels_1_2.tare_position(); + // right_wheels_2_2.tare_position(); + // + // intake_left.move(100); + // intake_right.move(100); + // left_wheels_1_2.move_absolute(1.75, 20); + // left_wheels_2_2.move_absolute(1.75, 20); + // right_wheels_1_2.move_absolute(1.75, 20); + // right_wheels_2_2.move_absolute(1.75, 20); + // + // while (!((left_wheels_1_2.get_position() < 1.80) && (left_wheels_1_2.get_position() > 1.70))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // left_wheels_1_2.move(0); + // left_wheels_2_2.move(0); + // right_wheels_1_2.move(0); + // right_wheels_2_2.move(0); + // + // //sets current positions to zero + // left_wheels_1_2.tare_position(); + // left_wheels_2_2.tare_position(); + // right_wheels_1_2.tare_position(); + // right_wheels_2_2.tare_position(); + // + // //Code to rotate ccw 90º + // left_wheels_1_2.move_absolute(0.5, -100); + // left_wheels_2_2.move_absolute(0.5, -100); + // right_wheels_1_2.move_absolute(0.5, 100); + // right_wheels_2_2.move_absolute(0.5, 100); + // while (!((left_wheels_1_2.get_position() < 0.55) && (left_wheels_1_2.get_position() > 0.45))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // left_wheels_1_2.tare_position(); + // left_wheels_2_2.tare_position(); + // right_wheels_1_2.tare_position(); + // right_wheels_2_2.tare_position(); + // + // left_wheels_1_2.move_absolute(1.7, 100); + // left_wheels_2_2.move_absolute(1.7, 100); + // right_wheels_1_2.move_absolute(1.7, 100); + // right_wheels_2_2.move_absolute(1.7, 100); + // + // while (!((left_wheels_1_2.get_position() < 1.75) && (left_wheels_1_2.get_position() > 1.65))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // left_wheels_1_2.move(0); + // left_wheels_2_2.move(0); + // right_wheels_1_2.move(0); + // right_wheels_2_2.move(0); + // + // //sets current positions to zero + // left_wheels_1_2.tare_position(); + // left_wheels_2_2.tare_position(); + // right_wheels_1_2.tare_position(); + // right_wheels_2_2.tare_position(); + // + // //Code to rotate ccw 90º + // left_wheels_1_2.move_absolute(0.5, -100); + // left_wheels_2_2.move_absolute(0.5, -100); + // right_wheels_1_2.move_absolute(0.5, 100); + // right_wheels_2_2.move_absolute(0.5, 100); + // while (!((left_wheels_1_2.get_position() < 0.55) && (left_wheels_1_2.get_position() > 0.45))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // left_wheels_1_2.move_absolute(2.3, 100); + // left_wheels_2_2.move_absolute(2.3, 100); + // right_wheels_1_2.move_absolute(2.3, 100); + // right_wheels_2_2.move_absolute(2.3, 100); + // + // while (!((left_wheels_1_2.get_position() < 2.35) && (left_wheels_1_2.get_position() > 2.25))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // left_wheels_1_2.move(0); + // left_wheels_2_2.move(0); + // right_wheels_1_2.move(0); + // right_wheels_2_2.move(0); + // + // //Code to place the cubes in the goal zone + // angler.tare_position(); + // angler.move_absolute(1, 100); + // while (!((angler.get_position() < 1.05) && (angler.get_position() > 0.95))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // angler.tare_position(); + // angler.move_absolute(1, 90); + // while (!((angler.get_position() < 1.05) && (angler.get_position() > 0.95))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // angler.tare_position(); + // angler.move_absolute(0.33, 70); + // while (!((angler.get_position() < 0.35) && (angler.get_position() > 0.30))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // angler.move(0); + // + // left_wheels_1_2.tare_position(); + // left_wheels_2_2.tare_position(); + // right_wheels_1_2.tare_position(); + // right_wheels_2_2.tare_position(); + // + // //moves forward to make sure tht the the stack is stable + // left_wheels_1_2.move_absolute(0.1, 20); + // left_wheels_2_2.move_absolute(0.1, 20); + // right_wheels_1_2.move_absolute(0.1, 20); + // right_wheels_2_2.move_absolute(0.1, 20); + // while (!((left_wheels_1_2.get_position() < 0.05) && (left_wheels_1_2.get_position() > 0.15))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // left_wheels_1_2.move(0); + // left_wheels_2_2.move(0); + // right_wheels_1_2.move(0); + // right_wheels_2_2.move(0); + // + // left_wheels_1_2.tare_position(); + // left_wheels_2_2.tare_position(); + // right_wheels_1_2.tare_position(); + // right_wheels_2_2.tare_position(); + // + // //backs away + // //intake_right.move(-20); + // //intake_right.move(-20); + // left_wheels_1_2.move_absolute(1, -20); + // left_wheels_2_2.move_absolute(1, -20); + // right_wheels_1_2.move_absolute(1, -20); + // right_wheels_2_2.move_absolute(1, -20); + // while (!((angler.get_position() < 1.05) && (angler.get_position() > 0.95))) { + // // Continue running this loop as long as the motor is not within +-5 units of its goal + // pros::delay(2); + // } + // + // left_wheels_1_2.move(0); + // left_wheels_2_2.move(0); + // right_wheels_1_2.move(0); + // right_wheels_2_2.move(0); + // //intake_left.move(0); + // //intake_right.move(0); } diff --git a/src/initialize.cpp b/src/initialize.cpp index c2bc5fa..359bc4d 100644 --- a/src/initialize.cpp +++ b/src/initialize.cpp @@ -18,10 +18,6 @@ void on_center_button() { */ void initialize() { pros::lcd::initialize(); - pros::lcd::set_text(1, "Hello PROS User!"); - pros::delay(1000); - pros::lcd::clear_line(1); - pros::lcd::register_btn1_cb(on_center_button); } /** diff --git a/src/motors.cpp b/src/motors.cpp new file mode 100644 index 0000000..368ac20 --- /dev/null +++ b/src/motors.cpp @@ -0,0 +1,12 @@ +#include "main.h" +#include "motors.h" + +pros::Motor left_wheels_1 (MOTOR1, pros::E_MOTOR_GEARSET_18, 0, pros::E_MOTOR_ENCODER_DEGREES); //L1 +pros::Motor right_wheels_1 (MOTOR3, pros::E_MOTOR_GEARSET_18, 1, pros::E_MOTOR_ENCODER_DEGREES); //R1 +pros::Motor left_wheels_2 (MOTOR2, pros::E_MOTOR_GEARSET_18, 1, pros::E_MOTOR_ENCODER_DEGREES); //L2 +pros::Motor right_wheels_2 (MOTOR4, pros::E_MOTOR_GEARSET_18, 0, pros::E_MOTOR_ENCODER_DEGREES); //R2 + +pros::Motor intake_left (MOTOR7, pros::E_MOTOR_GEARSET_36, 0, pros::E_MOTOR_ENCODER_DEGREES); +pros::Motor intake_right (MOTOR8, pros::E_MOTOR_GEARSET_36, 1, pros::E_MOTOR_ENCODER_DEGREES); +pros::Motor arm (MOTOR6, pros::E_MOTOR_GEARSET_36, 1, pros::E_MOTOR_ENCODER_DEGREES); +pros::Motor angler (MOTOR5, pros::E_MOTOR_GEARSET_36, 1, pros::E_MOTOR_ENCODER_DEGREES); diff --git a/src/opcontrol.cpp b/src/opcontrol.cpp index de5524f..64c05b4 100644 --- a/src/opcontrol.cpp +++ b/src/opcontrol.cpp @@ -1,37 +1,30 @@ #include "main.h" +#include "pid.h" +#include "motors.h" -//defines the ports for each motor -#define MOTOR1 2 //L1 -#define MOTOR2 3 //L2 -#define MOTOR3 11 //R1 -#define MOTOR4 12 //R2 - -#define MOTOR5 13 //Angler -#define MOTOR6 4 //Arm -#define MOTOR7 5 //LIntake -#define MOTOR8 14 //RIntake - -using namespace pros; +PID pid = PID(0.1, 75, -75, 0.1, 0.01, 0.5);//constructs PID object +pros::Mutex mutex; //defines controller -Controller master (CONTROLLER_MASTER); -Mutex mutex; +pros::Controller master (CONTROLLER_MASTER); //defines the ports that are associated with each wheel -Motor left_wheels_1 (MOTOR1, 0); //L1 -Motor right_wheels_1 (MOTOR3, 1); //R1 -Motor left_wheels_2 (MOTOR2, 1); //L2 -Motor right_wheels_2 (MOTOR4, 0); //R2 - -Motor intake_left (MOTOR7, 0); -Motor intake_right (MOTOR8, 1); -Motor arm (MOTOR6, 1); -Motor angler (MOTOR5, 1); void drive(void*) { //loop reads the joystick controlls and powers the motors accordingly while (true) { +<<<<<<< HEAD + std::string d1 = std::to_string(left_wheels_1.get_position()); + pros::lcd::set_text(0, d1); + std::string d2 = std::to_string(right_wheels_1.get_position()); + pros::lcd::set_text(1, d2); + std::string d3 = std::to_string(left_wheels_2.get_position()); + pros::lcd::set_text(2, d3); + std::string d4 = std::to_string(right_wheels_2.get_position()); + pros::lcd::set_text(3, d4); +======= +>>>>>>> d845ef1cbdd89a64f362ef02a2a46e4a73b129ad int power = master.get_analog(ANALOG_LEFT_Y); int turn = master.get_analog(ANALOG_RIGHT_X); int left = power + turn; @@ -40,7 +33,11 @@ void drive(void*) { left_wheels_2.move(left); right_wheels_1.move(right); right_wheels_2.move(right); +<<<<<<< HEAD + pros::delay(20); +======= delay(20); +>>>>>>> d845ef1cbdd89a64f362ef02a2a46e4a73b129ad } } @@ -49,6 +46,11 @@ void intake(void*) { int intake_power = 100; //moves intakes while(true) { + std::string il = std::to_string(intake_left.get_position()); + pros::lcd::set_text(4, il); + std::string ir = std::to_string(intake_right.get_position()); + pros::lcd::set_text(5, ir); + if(master.get_digital(DIGITAL_R2)) { intake_left.move(intake_power); intake_right.move(intake_power); @@ -61,14 +63,14 @@ void intake(void*) { intake_left.move(intake_power/3); intake_right.move(intake_power/3); while (!master.get_digital(DIGITAL_B)) { - delay(20); + pros::delay(20); } } else { intake_left.move(0); intake_right.move(0); } - delay(20); + pros::delay(20); } } @@ -77,6 +79,8 @@ void arms(void*) { int arm_power = 100; while(true) { + std::string b = std::to_string(arm.get_position()); + pros::lcd::set_text(6, b); //moves arm if(master.get_digital(DIGITAL_UP)) { arm.move(arm_power); @@ -87,7 +91,7 @@ void arms(void*) { else { arm.move(0); } - delay(20); + pros::delay(20); } @@ -96,22 +100,32 @@ void arms(void*) { void anglerMove(void*) { int angler_power = 100; + angler.tare_position(); while(true) { + std::string a = std::to_string(angler.get_position()); + pros::lcd::set_text(7, a); //moves angler if(master.get_digital(DIGITAL_R1)) { angler.move(angler_power); - } + } else if(master.get_digital(DIGITAL_L1)) { - angler.move(-angler_power); + if(angler.get_position() > -450) + angler.move(-angler_power); + else if(angler.get_position() < -600) + angler.move(0); + else + angler.move(-50); } else { angler.move(0); } - delay(20); + pros::delay(20); } } +<<<<<<< HEAD +======= void towerScore(void*) { while(true) { @@ -173,11 +187,11 @@ void stacker(void*) { } +>>>>>>> d845ef1cbdd89a64f362ef02a2a46e4a73b129ad void opcontrol() { - Task task1 (drive, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Driving"); - Task task2 (intake, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Intake"); - Task task3 (anglerMove, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Angler"); - Task task4 (arms, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Arms"); - Task task5 (towerScore, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Tower scoring"); - Task task6 (stacker, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Cube stacking"); + autonomous(); + pros::Task task1 (drive, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Driving"); + pros::Task task2 (intake, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Intake"); + pros::Task task3 (anglerMove, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Angler"); + pros::Task task4 (arms, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Arms"); } diff --git a/src/pid.cpp b/src/pid.cpp index a49b66a..e9b48d9 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -14,6 +14,7 @@ class PIDImpl public: PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ); ~PIDImpl(); + void resetError(); double calculate( double setpoint, double pv ); private: @@ -36,6 +37,10 @@ double PID::calculate( double setpoint, double pv ) { return pimpl->calculate(setpoint,pv); } +void PID::resetError() +{ + return pimpl->resetError(); +} PID::~PID() { delete pimpl; @@ -59,6 +64,11 @@ PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, doubl { } +void PIDImpl::resetError() { + _integral = 0; + _pre_error = 0; +} + double PIDImpl::calculate( double setpoint, double pv ) { @@ -96,17 +106,3 @@ PIDImpl::~PIDImpl() } #endif - -int calc() { - - PID pid = PID(0.1, 100, -100, 0.1, 0.01, 0.5);//constructs PID object - - double val = 0; //Defines current process value - for (int i = 0; i < 100; i++) { - double inc = pid.calculate(0, val); //calculates the output - printf("val:% 7.3f inc:% 7.3f\n", val, inc); //prints values - val += inc; //updates process value - } - - return 0; -} From 080b3547ea24394eb84cce4aa7fab486e9d44f5f Mon Sep 17 00:00:00 2001 From: David Hao <49774531+Kirbydude27@users.noreply.github.com> Date: Fri, 22 Nov 2019 23:47:32 -0500 Subject: [PATCH 4/8] Update opcontrol.cpp --- src/opcontrol.cpp | 72 +---------------------------------------------- 1 file changed, 1 insertion(+), 71 deletions(-) diff --git a/src/opcontrol.cpp b/src/opcontrol.cpp index 64c05b4..831d373 100644 --- a/src/opcontrol.cpp +++ b/src/opcontrol.cpp @@ -14,7 +14,6 @@ void drive(void*) { //loop reads the joystick controlls and powers the motors accordingly while (true) { -<<<<<<< HEAD std::string d1 = std::to_string(left_wheels_1.get_position()); pros::lcd::set_text(0, d1); std::string d2 = std::to_string(right_wheels_1.get_position()); @@ -23,8 +22,7 @@ void drive(void*) { pros::lcd::set_text(2, d3); std::string d4 = std::to_string(right_wheels_2.get_position()); pros::lcd::set_text(3, d4); -======= ->>>>>>> d845ef1cbdd89a64f362ef02a2a46e4a73b129ad + int power = master.get_analog(ANALOG_LEFT_Y); int turn = master.get_analog(ANALOG_RIGHT_X); int left = power + turn; @@ -33,11 +31,7 @@ void drive(void*) { left_wheels_2.move(left); right_wheels_1.move(right); right_wheels_2.move(right); -<<<<<<< HEAD pros::delay(20); -======= - delay(20); ->>>>>>> d845ef1cbdd89a64f362ef02a2a46e4a73b129ad } } @@ -124,70 +118,6 @@ void anglerMove(void*) { } } -<<<<<<< HEAD -======= -void towerScore(void*) { - - while(true) { - - if(master.get_digital(DIGITAL_A)) { - - mutex.take(TIMEOUT_MAX); - - arm.set_zero_position(0); - arm.move_absolute(100, 100); //change values later to not damage robot! - while (!((arm.get_position() < 105) && (arm.get_position() > 95))) { - delay(20); - } - - intake_left.move_relative(360, -100); //change values later to not damage robot! - intake_right.move_relative(360, -100); //change values later to not damage robot! - while (!((intake_left.get_position() < 365) && (intake_left.get_position() > 355))) { - delay(20); - } - - arm.move_absolute(0, -100); //change values later to not damage robot! - while (!((arm.get_position() < 5) && (arm.get_position() > -5))) { - delay(20); - } - - mutex.give(); - } - - } - -} - -void stacker(void*) { - - while(true) { - - if(master.get_digital(DIGITAL_X)) { - - mutex.take(TIMEOUT_MAX); - - angler.set_zero_position(0); - angler.move_absolute(100, 100); //change values later to not damage robot! - while (!((angler.get_position() < 105) && (angler.get_position() > 95))) { - delay(20); - } - - left_wheels_1.move_relative(45, 50); //change values later to not damage robot! - left_wheels_2.move_relative(45, 50); //change values later to not damage robot! - right_wheels_1.move_relative(45, 50); //change values later to not damage robot! - right_wheels_2.move_relative(45, 50); //change values later to not damage robot! - while (!((left_wheels_1.get_position() < 50) && (left_wheels_1.get_position() > 40))) { - delay(20); - } - - mutex.give(); - } - - } - -} - ->>>>>>> d845ef1cbdd89a64f362ef02a2a46e4a73b129ad void opcontrol() { autonomous(); pros::Task task1 (drive, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Driving"); From 1b50b97a38efdd61b0228f44da8236f31faf38ce Mon Sep 17 00:00:00 2001 From: unknown Date: Sat, 23 Nov 2019 00:25:50 -0500 Subject: [PATCH 5/8] Changes to autonomous --- compile_commands.json | 32 +++--- src/autonomous.cpp | 237 +++++++++++++++++++----------------------- 2 files changed, 125 insertions(+), 144 deletions(-) diff --git a/compile_commands.json b/compile_commands.json index d642a9a..fec0265 100644 --- a/compile_commands.json +++ b/compile_commands.json @@ -28,11 +28,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/opcontrol.cpp.o", - "src\\opcontrol.cpp" + "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/initialize.cpp.o", + "src\\initialize.cpp" ], - "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", - "file": "src\\opcontrol.cpp" + "directory": "C:\\Users\\David\\Documents\\2381WCode", + "file": "src\\initialize.cpp" }, { "arguments": [ @@ -63,10 +63,10 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/motors.cpp.o", + "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/motors.cpp.o", "src\\motors.cpp" ], - "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", + "directory": "C:\\Users\\David\\Documents\\2381WCode", "file": "src\\motors.cpp" }, { @@ -98,10 +98,10 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/pid.cpp.o", + "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/pid.cpp.o", "src\\pid.cpp" ], - "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", + "directory": "C:\\Users\\David\\Documents\\2381WCode", "file": "src\\pid.cpp" }, { @@ -133,11 +133,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/initialize.cpp.o", - "src\\initialize.cpp" + "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/autonomous.cpp.o", + "src\\autonomous.cpp" ], - "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", - "file": "src\\initialize.cpp" + "directory": "C:\\Users\\David\\Documents\\2381WCode", + "file": "src\\autonomous.cpp" }, { "arguments": [ @@ -168,10 +168,10 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/shao4/AppData/Local/Temp/tmppmp3a4mx/autonomous.cpp.o", - "src\\autonomous.cpp" + "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/opcontrol.cpp.o", + "src\\opcontrol.cpp" ], - "directory": "C:\\Users\\shao4\\Robo4\\2381WCode", - "file": "src\\autonomous.cpp" + "directory": "C:\\Users\\David\\Documents\\2381WCode", + "file": "src\\opcontrol.cpp" } ] \ No newline at end of file diff --git a/src/autonomous.cpp b/src/autonomous.cpp index 08e6504..dc85b0b 100644 --- a/src/autonomous.cpp +++ b/src/autonomous.cpp @@ -4,84 +4,69 @@ void autonomous() { - // left_wheels_1.tare_position(); - // left_wheels_2.tare_position(); - // right_wheels_1.tare_position(); - // right_wheels_2.tare_position(); - // - // left_wheels_1.move_absolute(450, 100); - // left_wheels_2.move_absolute(450, 100); - // right_wheels_1.move_absolute(450, 100); - // right_wheels_2.move_absolute(450, 100); - // while (!((left_wheels_1.get_position() < 0.6) && (left_wheels_1.get_position() > 0.5))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // left_wheels_1.move(0); - // left_wheels_2.move(0); - // right_wheels_1.move(0); - // right_wheels_2.move(0); + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(100, 100); + left_wheels_2.move_absolute(100, 100); + right_wheels_1.move_absolute(100, 100); + right_wheels_2.move_absolute(100, 100); + while (!((left_wheels_1.get_position() < 105) && (left_wheels_1.get_position() > 95))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); arm.tare_position(); arm.move_absolute(400, 100); while (!((arm.get_position() < 405) && (arm.get_position() > 395))) { // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(2); + pros::delay(20); } angler.tare_position(); - angler.move_absolute(-50, 100); + angler.move_absolute(-50, -100); while (!((angler.get_position() < -45) && (angler.get_position() > -55))) { // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(2); + pros::delay(20); } arm.move_absolute(0, -70); while (!((arm.get_position() < 5) && (arm.get_position() > -5))) { // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(2); + pros::delay(20); } - angler.move_absolute(0, -70); + angler.move_absolute(0, 70); while (!((angler.get_position() < 5) && (angler.get_position() > -5))) { // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(2); + pros::delay(20); } - // - // left_wheels_1_2.move_absolute(1, 100); - // left_wheels_2_2.move_absolute(1, 100); - // right_wheels_1_2.move_absolute(1, 100); - // right_wheels_2_2.move_absolute(1, 100); - // while (!((left_wheels_1_2.get_position() < 1.05) && (left_wheels_1_2.get_position() > 0.95))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // left_wheels_1_2.move(0); - // left_wheels_2_2.move(0); - // right_wheels_1_2.move(0); - // right_wheels_2_2.move(0); - // - // //sets current positions to zero - // left_wheels_1_2.tare_position(); - // left_wheels_2_2.tare_position(); - // right_wheels_1_2.tare_position(); - // right_wheels_2_2.tare_position(); - // - // intake_left.move(100); - // intake_right.move(100); - // left_wheels_1_2.move_absolute(1.75, 20); - // left_wheels_2_2.move_absolute(1.75, 20); - // right_wheels_1_2.move_absolute(1.75, 20); - // right_wheels_2_2.move_absolute(1.75, 20); - // - // while (!((left_wheels_1_2.get_position() < 1.80) && (left_wheels_1_2.get_position() > 1.70))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // left_wheels_1_2.move(0); - // left_wheels_2_2.move(0); - // right_wheels_1_2.move(0); - // right_wheels_2_2.move(0); + + for(int i = 0; i < 4; i++) + { + left_wheels_1.move_absolute(350, 100); + left_wheels_2.move_absolute(350, 100); + right_wheels_1.move_absolute(350, 100); + right_wheels_2.move_absolute(350, 100); + while (!((left_wheels_1.get_position() < 355) && (left_wheels_1.get_position() > 345))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + + intake_left.move(100); + intake_right.move(100); + pros::delay(200); + } + // // //sets current positions to zero // left_wheels_1_2.tare_position(); @@ -150,71 +135,67 @@ void autonomous() { // right_wheels_1_2.move(0); // right_wheels_2_2.move(0); // - // //Code to place the cubes in the goal zone - // angler.tare_position(); - // angler.move_absolute(1, 100); - // while (!((angler.get_position() < 1.05) && (angler.get_position() > 0.95))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // angler.tare_position(); - // angler.move_absolute(1, 90); - // while (!((angler.get_position() < 1.05) && (angler.get_position() > 0.95))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // angler.tare_position(); - // angler.move_absolute(0.33, 70); - // while (!((angler.get_position() < 0.35) && (angler.get_position() > 0.30))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // angler.move(0); - // - // left_wheels_1_2.tare_position(); - // left_wheels_2_2.tare_position(); - // right_wheels_1_2.tare_position(); - // right_wheels_2_2.tare_position(); - // - // //moves forward to make sure tht the the stack is stable - // left_wheels_1_2.move_absolute(0.1, 20); - // left_wheels_2_2.move_absolute(0.1, 20); - // right_wheels_1_2.move_absolute(0.1, 20); - // right_wheels_2_2.move_absolute(0.1, 20); - // while (!((left_wheels_1_2.get_position() < 0.05) && (left_wheels_1_2.get_position() > 0.15))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // left_wheels_1_2.move(0); - // left_wheels_2_2.move(0); - // right_wheels_1_2.move(0); - // right_wheels_2_2.move(0); - // - // left_wheels_1_2.tare_position(); - // left_wheels_2_2.tare_position(); - // right_wheels_1_2.tare_position(); - // right_wheels_2_2.tare_position(); - // - // //backs away - // //intake_right.move(-20); - // //intake_right.move(-20); - // left_wheels_1_2.move_absolute(1, -20); - // left_wheels_2_2.move_absolute(1, -20); - // right_wheels_1_2.move_absolute(1, -20); - // right_wheels_2_2.move_absolute(1, -20); - // while (!((angler.get_position() < 1.05) && (angler.get_position() > 0.95))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // left_wheels_1_2.move(0); - // left_wheels_2_2.move(0); - // right_wheels_1_2.move(0); - // right_wheels_2_2.move(0); - // //intake_left.move(0); - // //intake_right.move(0); + //Code to place the cubes in the goal zone + angler.tare_position(); + angler.move_absolute(-400, -100); + while (!((angler.get_position() < -395) && (angler.get_position() > -405))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(20); + } + + angler.tare_position(); + angler.move_absolute(-500, -75); + while (!((angler.get_position() < -495) && (angler.get_position() > -505))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(20); + } + + angler.tare_position(); + angler.move_absolute(-600, -50); + while (!((angler.get_position() < -595) && (angler.get_position() > -605))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(20); + } + + angler.move(0); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + //moves forward to make sure tht the the stack is stable + left_wheels_1.move_absolute(20, 20); + left_wheels_2.move_absolute(20, 20); + right_wheels_1.move_absolute(20, 20); + right_wheels_2.move_absolute(20, 20); + while (!((left_wheels_1.get_position() < 35) && (left_wheels_1.get_position() > 25))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + //backs away + left_wheels_1.move_absolute(-200, -20); + left_wheels_2.move_absolute(-200, -20); + right_wheels_1.move_absolute(-200, -20); + right_wheels_2.move_absolute(-200, -20); + while (!((left_wheels_1.get_position() < -195) && (left_wheels_1.get_position() > -205))) { + // Continue running this loop as long as the motor is not within +-5 units of its goal + pros::delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); } From 75e76c507b23c21db4b947e5fca345f4e0fa38a4 Mon Sep 17 00:00:00 2001 From: Kirbydude27 Date: Thu, 6 Feb 2020 22:19:49 -0500 Subject: [PATCH 6/8] Post-Terrebonne --- compile_commands.json | 67 +++++++-- include/adi.h | 9 ++ include/main.h | 13 +- project.pros | 2 +- src/adi.cpp | 7 + src/autonomous.cpp | 334 +++++++++++++++++++++--------------------- src/initialize.cpp | 8 +- src/motors.cpp | 18 ++- src/opcontrol.cpp | 175 ++++++++++++---------- 9 files changed, 352 insertions(+), 281 deletions(-) create mode 100644 include/adi.h create mode 100644 src/adi.cpp diff --git a/compile_commands.json b/compile_commands.json index fec0265..4cb0058 100644 --- a/compile_commands.json +++ b/compile_commands.json @@ -28,10 +28,10 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/initialize.cpp.o", + "bin/initialize.cpp.o", "src\\initialize.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", + "directory": "C:\\Users\\shao4\\Downloads\\Terrebonne\\2381WCode-2", "file": "src\\initialize.cpp" }, { @@ -63,11 +63,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/motors.cpp.o", - "src\\motors.cpp" + "bin/pid.cpp.o", + "src\\pid.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\motors.cpp" + "directory": "C:\\Users\\shao4\\Downloads\\Terrebonne\\2381WCode-2", + "file": "src\\pid.cpp" }, { "arguments": [ @@ -98,11 +98,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/pid.cpp.o", - "src\\pid.cpp" + "bin/motors.cpp.o", + "src\\motors.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\pid.cpp" + "directory": "C:\\Users\\shao4\\Downloads\\Terrebonne\\2381WCode-2", + "file": "src\\motors.cpp" }, { "arguments": [ @@ -133,11 +133,11 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/autonomous.cpp.o", - "src\\autonomous.cpp" + "bin/adi.cpp.o", + "src\\adi.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\autonomous.cpp" + "directory": "C:\\Users\\shao4\\Downloads\\Terrebonne\\2381WCode-2", + "file": "src\\adi.cpp" }, { "arguments": [ @@ -168,10 +168,45 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "C:/Users/David/AppData/Local/Temp/tmprtlc3_yn/opcontrol.cpp.o", + "bin/opcontrol.cpp.o", "src\\opcontrol.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", + "directory": "C:\\Users\\shao4\\Downloads\\Terrebonne\\2381WCode-2", "file": "src\\opcontrol.cpp" + }, + { + "arguments": [ + "clang++", + "-c", + "-target", + "armv7ar-none-none-eabi", + "-fno-ms-extensions", + "-fno-ms-compatibility", + "-fno-delayed-template-parsing", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/../../../../arm-none-eabi/include/c++/7.2.1", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/../../../../arm-none-eabi/include/c++/7.2.1/arm-none-eabi/thumb/v7-ar/fpv3/softfp", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/../../../../arm-none-eabi/include/c++/7.2.1/backward", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/include", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/include-fixed", + "-isystemc:\\program files\\pros\\toolchain\\usr\\bin\\../lib/gcc/arm-none-eabi/7.2.1/../../../../arm-none-eabi/include", + "-iquote./include", + "-iquote./include/./", + "-mcpu=cortex-a9", + "-mfpu=neon-fp16", + "-mfloat-abi=softfp", + "-D_POSIX_THREADS", + "-D_UNIX98_THREAD_MUTEX_ATTRIBUTES", + "-Os", + "-funwind-tables", + "-ffunction-sections", + "-fdata-sections", + "-fdiagnostics-color", + "--std=gnu++17", + "-o", + "bin/autonomous.cpp.o", + "src\\autonomous.cpp" + ], + "directory": "C:\\Users\\shao4\\Downloads\\Terrebonne\\2381WCode-2", + "file": "src\\autonomous.cpp" } ] \ No newline at end of file diff --git a/include/adi.h b/include/adi.h new file mode 100644 index 0000000..18592e0 --- /dev/null +++ b/include/adi.h @@ -0,0 +1,9 @@ +#include "main.h" + +#ifndef _ADI_H_ +#define _ADI_H_ + +extern pros::ADIUltrasonic ultra_left; //left ultrasonic +extern pros::ADIUltrasonic ultra_right; //right ultrasonic + +#endif diff --git a/include/main.h b/include/main.h index 62481cc..a24af0a 100644 --- a/include/main.h +++ b/include/main.h @@ -16,15 +16,20 @@ #define _PROS_MAIN_H_ //defines the ports for each motor -#define MOTOR1 2 //L1 -#define MOTOR2 3 //L2 +#define MOTOR1 12 //L1 +#define MOTOR2 9 //L2 #define MOTOR3 11 //R1 -#define MOTOR4 12 //R2 +#define MOTOR4 2 //R2 #define MOTOR5 13 //Angler #define MOTOR6 4 //Arm #define MOTOR7 5 //LIntake -#define MOTOR8 14 //RIntake +#define MOTOR8 20 //RIntake + +#define ULTRA1_PING 1 //LUltrasonic +#define ULTRA1_ECHO 2 +#define ULTRA2_PING 3 //RUltrasonic +#define ULTRA2_ECHO 4 /** * If defined, some commonly used enums will have preprocessor macros which give diff --git a/project.pros b/project.pros index 2f0df20..3cea6cc 100644 --- a/project.pros +++ b/project.pros @@ -251,7 +251,7 @@ "upload_options": { "compress_bin": true, "description": "Created with PROS", - "remote_name": "Robocheck", + "remote_name": "Robosuck", "slot": 2 } } diff --git a/src/adi.cpp b/src/adi.cpp new file mode 100644 index 0000000..0381fd2 --- /dev/null +++ b/src/adi.cpp @@ -0,0 +1,7 @@ +#include "main.h" +#include "adi.h" + +using namespace pros; + +ADIUltrasonic ultra_left (ULTRA1_PING, ULTRA1_ECHO); //LUltra +ADIUltrasonic ultra_right (ULTRA2_PING, ULTRA2_ECHO); //RUltra diff --git a/src/autonomous.cpp b/src/autonomous.cpp index dc85b0b..fead2c7 100644 --- a/src/autonomous.cpp +++ b/src/autonomous.cpp @@ -1,201 +1,193 @@ #include "main.h" #include #include "motors.h" - -void autonomous() { - - left_wheels_1.tare_position(); - left_wheels_2.tare_position(); - right_wheels_1.tare_position(); - right_wheels_2.tare_position(); - - left_wheels_1.move_absolute(100, 100); - left_wheels_2.move_absolute(100, 100); - right_wheels_1.move_absolute(100, 100); - right_wheels_2.move_absolute(100, 100); - while (!((left_wheels_1.get_position() < 105) && (left_wheels_1.get_position() > 95))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); +#include "adi.h" +#include "pid.h" + +using namespace pros; + +PID pid2 = PID(0.1, 75, -75, 0.1, 0.01, 0.5);//constructs PID object +// void ultrasonic(void*) { + +// while (true) { +// while (ultra_left.get_value() > 100 && ultra_right.get_value() > 100) { +// // Move forward until the robot is 100 cm from a solid object +// angler.move(127); +// pros::delay(50); +// } +// } +// +// } + +void movement() { + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + intake_left.move(-100); + intake_right.move(-100); + delay(100); + + left_wheels_1.move_absolute(-3350, 50); + left_wheels_2.move_absolute(-3350, 50); + right_wheels_1.move_absolute(-3350, 50); + right_wheels_2.move_absolute(-3350, 50); + + while (!((left_wheels_1.get_position() < -3345) && (left_wheels_1.get_position() > -3455))) { + delay(20); } + while (!((left_wheels_2.get_position() < -3345) && (left_wheels_2.get_position() > -3455))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -3345) && (right_wheels_1.get_position() > -3455))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -3345) && (right_wheels_2.get_position() > -3455))) { + delay(20); + } left_wheels_1.move(0); left_wheels_2.move(0); right_wheels_1.move(0); right_wheels_2.move(0); + intake_left.move(0); + intake_right.move(0); + delay(100); + std::string d1 = std::to_string(left_wheels_1.get_position()); + lcd::set_text(0, d1); + std::string d2 = std::to_string(right_wheels_1.get_position()); + lcd::set_text(1, d2); + std::string d3 = std::to_string(left_wheels_2.get_position()); + lcd::set_text(2, d3); + std::string d4 = std::to_string(right_wheels_2.get_position()); + lcd::set_text(3, d4); + + //Code to rotate ccw 90º + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + lcd::set_text(4, "Fuck!"); + + left_wheels_1.move_absolute(-160, 60); + left_wheels_2.move_absolute(-160, 60); + right_wheels_1.move_absolute(140, 60); + right_wheels_2.move_absolute(140, 60); + while (!((left_wheels_1.get_position() < -155) && (left_wheels_1.get_position() > -165))) { + delay(20); + } + while (!((left_wheels_2.get_position() < -155) && (left_wheels_2.get_position() > -165))) { + delay(20); + } + while (!((right_wheels_1.get_position() < 145) && (right_wheels_1.get_position() > 135))) { + delay(20); + } + while (!((right_wheels_2.get_position() < 145) && (right_wheels_2.get_position() > 135))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + intake_left.move(0); + intake_right.move(0); + delay(50); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + +left_wheels_1.move_absolute(-250, 50); +left_wheels_2.move_absolute(-250, 50); +right_wheels_1.move_absolute(-250, 50); +right_wheels_2.move_absolute(-250, 50); + +while (!((left_wheels_1.get_position() < -245) && (left_wheels_1.get_position() > -255))) { + delay(20); +} +while (!((left_wheels_2.get_position() < -245) && (left_wheels_2.get_position() > -255))) { +delay(20); +} +while (!((right_wheels_1.get_position() < -245) && (right_wheels_1.get_position() > -255))) { +delay(20); +} +while (!((right_wheels_2.get_position() < -245) && (right_wheels_2.get_position() > -255))) { +delay(20); +} - arm.tare_position(); - arm.move_absolute(400, 100); - while (!((arm.get_position() < 405) && (arm.get_position() > 395))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); - } - angler.tare_position(); - angler.move_absolute(-50, -100); - while (!((angler.get_position() < -45) && (angler.get_position() > -55))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); - } - arm.move_absolute(0, -70); - while (!((arm.get_position() < 5) && (arm.get_position() > -5))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); - } - angler.move_absolute(0, 70); - while (!((angler.get_position() < 5) && (angler.get_position() > -5))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); - } - - for(int i = 0; i < 4; i++) - { - left_wheels_1.move_absolute(350, 100); - left_wheels_2.move_absolute(350, 100); - right_wheels_1.move_absolute(350, 100); - right_wheels_2.move_absolute(350, 100); - while (!((left_wheels_1.get_position() < 355) && (left_wheels_1.get_position() > 345))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); - } - - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - - intake_left.move(100); - intake_right.move(100); - pros::delay(200); - } +left_wheels_1.move(0); +left_wheels_2.move(0); +right_wheels_1.move(0); +right_wheels_2.move(0); - // - // //sets current positions to zero - // left_wheels_1_2.tare_position(); - // left_wheels_2_2.tare_position(); - // right_wheels_1_2.tare_position(); - // right_wheels_2_2.tare_position(); - // - // //Code to rotate ccw 90º - // left_wheels_1_2.move_absolute(0.5, -100); - // left_wheels_2_2.move_absolute(0.5, -100); - // right_wheels_1_2.move_absolute(0.5, 100); - // right_wheels_2_2.move_absolute(0.5, 100); - // while (!((left_wheels_1_2.get_position() < 0.55) && (left_wheels_1_2.get_position() > 0.45))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // left_wheels_1_2.tare_position(); - // left_wheels_2_2.tare_position(); - // right_wheels_1_2.tare_position(); - // right_wheels_2_2.tare_position(); - // - // left_wheels_1_2.move_absolute(1.7, 100); - // left_wheels_2_2.move_absolute(1.7, 100); - // right_wheels_1_2.move_absolute(1.7, 100); - // right_wheels_2_2.move_absolute(1.7, 100); - // - // while (!((left_wheels_1_2.get_position() < 1.75) && (left_wheels_1_2.get_position() > 1.65))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // left_wheels_1_2.move(0); - // left_wheels_2_2.move(0); - // right_wheels_1_2.move(0); - // right_wheels_2_2.move(0); - // - // //sets current positions to zero - // left_wheels_1_2.tare_position(); - // left_wheels_2_2.tare_position(); - // right_wheels_1_2.tare_position(); - // right_wheels_2_2.tare_position(); - // - // //Code to rotate ccw 90º - // left_wheels_1_2.move_absolute(0.5, -100); - // left_wheels_2_2.move_absolute(0.5, -100); - // right_wheels_1_2.move_absolute(0.5, 100); - // right_wheels_2_2.move_absolute(0.5, 100); - // while (!((left_wheels_1_2.get_position() < 0.55) && (left_wheels_1_2.get_position() > 0.45))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // left_wheels_1_2.move_absolute(2.3, 100); - // left_wheels_2_2.move_absolute(2.3, 100); - // right_wheels_1_2.move_absolute(2.3, 100); - // right_wheels_2_2.move_absolute(2.3, 100); - // - // while (!((left_wheels_1_2.get_position() < 2.35) && (left_wheels_1_2.get_position() > 2.25))) { - // // Continue running this loop as long as the motor is not within +-5 units of its goal - // pros::delay(2); - // } - // - // left_wheels_1_2.move(0); - // left_wheels_2_2.move(0); - // right_wheels_1_2.move(0); - // right_wheels_2_2.move(0); - // //Code to place the cubes in the goal zone angler.tare_position(); - angler.move_absolute(-400, -100); - while (!((angler.get_position() < -395) && (angler.get_position() > -405))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); + angler.move_absolute(300, 100); + while (!((angler.get_position() < 305) && (angler.get_position() > 295))) { + delay(20); } - angler.tare_position(); - angler.move_absolute(-500, -75); - while (!((angler.get_position() < -495) && (angler.get_position() > -505))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); + angler.move_absolute(450, 75); + while (!((angler.get_position() < 455) && (angler.get_position() > 445))) { + delay(20); } - angler.tare_position(); - angler.move_absolute(-600, -50); - while (!((angler.get_position() < -595) && (angler.get_position() > -605))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); + angler.move_absolute(550, 50); + while (!((angler.get_position() < 555) && (angler.get_position() > 545))) { + delay(20); } angler.move(0); - left_wheels_1.tare_position(); - left_wheels_2.tare_position(); - right_wheels_1.tare_position(); - right_wheels_2.tare_position(); - - //moves forward to make sure tht the the stack is stable - left_wheels_1.move_absolute(20, 20); - left_wheels_2.move_absolute(20, 20); - right_wheels_1.move_absolute(20, 20); - right_wheels_2.move_absolute(20, 20); - while (!((left_wheels_1.get_position() < 35) && (left_wheels_1.get_position() > 25))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); - } + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + //backs away + left_wheels_1.move_absolute(400, 80); + left_wheels_2.move_absolute(400, 80); + right_wheels_1.move_absolute(400, 80); + right_wheels_2.move_absolute(400, 80); + delay(50); + + while (!((left_wheels_1.get_position() < 405) && (left_wheels_1.get_position() > 395))) { + delay(20); + } + while (!((left_wheels_2.get_position() < 405) && (left_wheels_2.get_position() > 395))) { + delay(20); + } + while (!((right_wheels_1.get_position() < 405) && (right_wheels_1.get_position() > 395))) { + delay(20); + } + while (!((right_wheels_2.get_position() < 405) && (right_wheels_2.get_position() > 395))) { + delay(20); + } left_wheels_1.move(0); left_wheels_2.move(0); right_wheels_1.move(0); right_wheels_2.move(0); - left_wheels_1.tare_position(); - left_wheels_2.tare_position(); - right_wheels_1.tare_position(); - right_wheels_2.tare_position(); + //...turn + //...move to tower + //...cube and tower (one motion) - //backs away - left_wheels_1.move_absolute(-200, -20); - left_wheels_2.move_absolute(-200, -20); - right_wheels_1.move_absolute(-200, -20); - right_wheels_2.move_absolute(-200, -20); - while (!((left_wheels_1.get_position() < -195) && (left_wheels_1.get_position() > -205))) { - // Continue running this loop as long as the motor is not within +-5 units of its goal - pros::delay(20); - } + //...turn + //...move to tower + //...cube and tower (one motion) + //27 points! + +} + +void autonomous() { + +movement(); - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); } diff --git a/src/initialize.cpp b/src/initialize.cpp index 359bc4d..62d2c38 100644 --- a/src/initialize.cpp +++ b/src/initialize.cpp @@ -1,12 +1,14 @@ #include "main.h" +using namespace pros; + void on_center_button() { static bool pressed = false; pressed = !pressed; if (pressed) { - pros::lcd::set_text(2, "I was pressed!"); + lcd::set_text(2, "I was pressed!"); } else { - pros::lcd::clear_line(2); + lcd::clear_line(2); } } @@ -17,7 +19,7 @@ void on_center_button() { * to keep execution time for this mode under a few seconds. */ void initialize() { - pros::lcd::initialize(); + lcd::initialize(); } /** diff --git a/src/motors.cpp b/src/motors.cpp index 368ac20..d9073da 100644 --- a/src/motors.cpp +++ b/src/motors.cpp @@ -1,12 +1,14 @@ #include "main.h" #include "motors.h" -pros::Motor left_wheels_1 (MOTOR1, pros::E_MOTOR_GEARSET_18, 0, pros::E_MOTOR_ENCODER_DEGREES); //L1 -pros::Motor right_wheels_1 (MOTOR3, pros::E_MOTOR_GEARSET_18, 1, pros::E_MOTOR_ENCODER_DEGREES); //R1 -pros::Motor left_wheels_2 (MOTOR2, pros::E_MOTOR_GEARSET_18, 1, pros::E_MOTOR_ENCODER_DEGREES); //L2 -pros::Motor right_wheels_2 (MOTOR4, pros::E_MOTOR_GEARSET_18, 0, pros::E_MOTOR_ENCODER_DEGREES); //R2 +using namespace pros; -pros::Motor intake_left (MOTOR7, pros::E_MOTOR_GEARSET_36, 0, pros::E_MOTOR_ENCODER_DEGREES); -pros::Motor intake_right (MOTOR8, pros::E_MOTOR_GEARSET_36, 1, pros::E_MOTOR_ENCODER_DEGREES); -pros::Motor arm (MOTOR6, pros::E_MOTOR_GEARSET_36, 1, pros::E_MOTOR_ENCODER_DEGREES); -pros::Motor angler (MOTOR5, pros::E_MOTOR_GEARSET_36, 1, pros::E_MOTOR_ENCODER_DEGREES); +Motor left_wheels_1 (MOTOR1, E_MOTOR_GEARSET_18, 1, E_MOTOR_ENCODER_DEGREES); //L1 +Motor right_wheels_1 (MOTOR3, E_MOTOR_GEARSET_18, 0, E_MOTOR_ENCODER_DEGREES); //R1 +Motor left_wheels_2 (MOTOR2, E_MOTOR_GEARSET_18, 1, E_MOTOR_ENCODER_DEGREES); //L2 +Motor right_wheels_2 (MOTOR4, E_MOTOR_GEARSET_18, 0, E_MOTOR_ENCODER_DEGREES); //R2 + +Motor intake_left (MOTOR7, E_MOTOR_GEARSET_36, 1, E_MOTOR_ENCODER_DEGREES); +Motor intake_right (MOTOR8, E_MOTOR_GEARSET_36, 0, E_MOTOR_ENCODER_DEGREES); +Motor arm (MOTOR6, E_MOTOR_GEARSET_36, 1, E_MOTOR_ENCODER_DEGREES); +Motor angler (MOTOR5, E_MOTOR_GEARSET_36, 0, E_MOTOR_ENCODER_DEGREES); diff --git a/src/opcontrol.cpp b/src/opcontrol.cpp index 831d373..a09eee2 100644 --- a/src/opcontrol.cpp +++ b/src/opcontrol.cpp @@ -1,12 +1,15 @@ #include "main.h" #include "pid.h" #include "motors.h" +#include "adi.h" + +using namespace pros; PID pid = PID(0.1, 75, -75, 0.1, 0.01, 0.5);//constructs PID object -pros::Mutex mutex; +Mutex mutex; //defines controller -pros::Controller master (CONTROLLER_MASTER); +Controller master (CONTROLLER_MASTER); //defines the ports that are associated with each wheel @@ -15,113 +18,129 @@ void drive(void*) { //loop reads the joystick controlls and powers the motors accordingly while (true) { std::string d1 = std::to_string(left_wheels_1.get_position()); - pros::lcd::set_text(0, d1); + lcd::set_text(0, d1); std::string d2 = std::to_string(right_wheels_1.get_position()); - pros::lcd::set_text(1, d2); + lcd::set_text(1, d2); std::string d3 = std::to_string(left_wheels_2.get_position()); - pros::lcd::set_text(2, d3); + lcd::set_text(2, d3); std::string d4 = std::to_string(right_wheels_2.get_position()); - pros::lcd::set_text(3, d4); + lcd::set_text(3, d4); int power = master.get_analog(ANALOG_LEFT_Y); - int turn = master.get_analog(ANALOG_RIGHT_X); + int turn = 0; + if(!(master.get_digital(DIGITAL_RIGHT))){ + turn = master.get_analog(ANALOG_RIGHT_X); + } int left = power + turn; int right = power - turn; - left_wheels_1.move(left); - left_wheels_2.move(left); - right_wheels_1.move(right); - right_wheels_2.move(right); + left_wheels_1.move(-left); + left_wheels_2.move(-left); + right_wheels_1.move(-right); + right_wheels_2.move(-right); pros::delay(20); } } void intake(void*) { - int intake_power = 100; + int intake_power = 127; //moves intakes while(true) { std::string il = std::to_string(intake_left.get_position()); - pros::lcd::set_text(4, il); + lcd::set_text(4, il); std::string ir = std::to_string(intake_right.get_position()); - pros::lcd::set_text(5, ir); - - if(master.get_digital(DIGITAL_R2)) { - intake_left.move(intake_power); - intake_right.move(intake_power); - } - else if(master.get_digital(DIGITAL_L2)) { - intake_left.move(-intake_power); - intake_right.move(-intake_power); - } - else if(master.get_digital(DIGITAL_B)) { - intake_left.move(intake_power/3); - intake_right.move(intake_power/3); - while (!master.get_digital(DIGITAL_B)) { - pros::delay(20); - } - } - else { - intake_left.move(0); - intake_right.move(0); - } + lcd::set_text(5, ir); + + if(master.get_digital(DIGITAL_L2)) { + intake_left.move(intake_power); + intake_right.move(intake_power); + } + else if(master.get_digital(DIGITAL_R2)) { + intake_left.move(-intake_power); + intake_right.move(-intake_power); + } + else if(master.get_digital(DIGITAL_X)) { + intake_left.move(intake_power/3); + intake_right.move(intake_power/3); + while (!master.get_digital(DIGITAL_X)) { + pros::delay(20); + } + } + else { + //ensures the intakes do not move + intake_left.move(0); + intake_right.move(0); + intake_left.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); + intake_right.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); + } pros::delay(20); } } -void arms(void*) { - - int arm_power = 100; +void anglerMove(void*){ + int angler_power = 0; + while(true){ + std::string a = std::to_string(angler.get_position()); + lcd::set_text(7, a); + if(master.get_digital(DIGITAL_L1)){ + angler.move(127); + } + else if(master.get_digital(DIGITAL_R1)){ + angler.move(-127); + } + else if(master.get_digital(DIGITAL_RIGHT)){ //overrides so you can use the right joystick for angler + //Code to place the cubes in the goal zone + angler.tare_position(); + angler.move_absolute(300, 100); + while (!((angler.get_position() < 305) && (angler.get_position() > 295))) { + delay(20); + } + + angler.move_absolute(450, 60); + while (!((angler.get_position() < 455) && (angler.get_position() > 445))) { + delay(20); + } + + angler.move_absolute(550, 40); + while (!((angler.get_position() < 555) && (angler.get_position() > 545))) { + delay(20); + } + } + else{ + angler.move(0); + angler.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); + angler.move(0); + } - while(true) { - std::string b = std::to_string(arm.get_position()); - pros::lcd::set_text(6, b); - //moves arm - if(master.get_digital(DIGITAL_UP)) { - arm.move(arm_power); - } - else if(master.get_digital(DIGITAL_DOWN)) { - arm.move(-arm_power); - } - else { - arm.move(0); - } pros::delay(20); } - - } -void anglerMove(void*) { +void arms(void*) { - int angler_power = 100; - angler.tare_position(); + int arm_power = 127; while(true) { - std::string a = std::to_string(angler.get_position()); - pros::lcd::set_text(7, a); - //moves angler - if(master.get_digital(DIGITAL_R1)) { - angler.move(angler_power); - } - else if(master.get_digital(DIGITAL_L1)) { - if(angler.get_position() > -450) - angler.move(-angler_power); - else if(angler.get_position() < -600) - angler.move(0); - else - angler.move(-50); - } - else { - angler.move(0); - } - pros::delay(20); + std::string b = std::to_string(arm.get_position()); + lcd::set_text(6, b); + //moves arm + if(master.get_digital(DIGITAL_Y)) { + arm.move(arm_power); + } + else if(master.get_digital(DIGITAL_B)) { + arm.move(-arm_power); + } + else { + arm.move(0); + arm.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); + } + pros::delay(20); } } void opcontrol() { - autonomous(); - pros::Task task1 (drive, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Driving"); - pros::Task task2 (intake, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Intake"); - pros::Task task3 (anglerMove, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Angler"); - pros::Task task4 (arms, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Arms"); + Task task1 (drive, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Driving"); + Task task2 (intake, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Intake"); + Task task3 (anglerMove, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Angler"); + Task task4 (arms, NULL, TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "Arms"); } From b057e911ac5383eab7cb1cd2a9edcbe32b93d02c Mon Sep 17 00:00:00 2001 From: Kirbydude27 Date: Mon, 24 Feb 2020 18:03:46 -0500 Subject: [PATCH 7/8] Code from 2nd Ottawa --- include/main.h | 4 +- src/autonomous.cpp | 217 ++++++++++++--------------------------------- src/motors.cpp | 4 +- src/opcontrol.cpp | 9 +- 4 files changed, 65 insertions(+), 169 deletions(-) diff --git a/include/main.h b/include/main.h index a24af0a..d471c09 100644 --- a/include/main.h +++ b/include/main.h @@ -23,8 +23,8 @@ #define MOTOR5 13 //Angler #define MOTOR6 4 //Arm -#define MOTOR7 5 //LIntake -#define MOTOR8 20 //RIntake +#define MOTOR7 7 //LIntake +#define MOTOR8 19 //RIntake #define ULTRA1_PING 1 //LUltrasonic #define ULTRA1_ECHO 2 diff --git a/src/autonomous.cpp b/src/autonomous.cpp index fead2c7..eb43835 100644 --- a/src/autonomous.cpp +++ b/src/autonomous.cpp @@ -21,168 +21,63 @@ PID pid2 = PID(0.1, 75, -75, 0.1, 0.01, 0.5);//constructs PID object void movement() { - left_wheels_1.tare_position(); - left_wheels_2.tare_position(); - right_wheels_1.tare_position(); - right_wheels_2.tare_position(); - - intake_left.move(-100); - intake_right.move(-100); - delay(100); - - left_wheels_1.move_absolute(-3350, 50); - left_wheels_2.move_absolute(-3350, 50); - right_wheels_1.move_absolute(-3350, 50); - right_wheels_2.move_absolute(-3350, 50); - - while (!((left_wheels_1.get_position() < -3345) && (left_wheels_1.get_position() > -3455))) { - delay(20); - } - while (!((left_wheels_2.get_position() < -3345) && (left_wheels_2.get_position() > -3455))) { - delay(20); - } - while (!((right_wheels_1.get_position() < -3345) && (right_wheels_1.get_position() > -3455))) { - delay(20); - } - while (!((right_wheels_2.get_position() < -3345) && (right_wheels_2.get_position() > -3455))) { - delay(20); - } - - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - intake_left.move(0); - intake_right.move(0); - delay(100); - std::string d1 = std::to_string(left_wheels_1.get_position()); - lcd::set_text(0, d1); - std::string d2 = std::to_string(right_wheels_1.get_position()); - lcd::set_text(1, d2); - std::string d3 = std::to_string(left_wheels_2.get_position()); - lcd::set_text(2, d3); - std::string d4 = std::to_string(right_wheels_2.get_position()); - lcd::set_text(3, d4); - - //Code to rotate ccw 90º - - left_wheels_1.tare_position(); - left_wheels_2.tare_position(); - right_wheels_1.tare_position(); - right_wheels_2.tare_position(); - - lcd::set_text(4, "Fuck!"); - - left_wheels_1.move_absolute(-160, 60); - left_wheels_2.move_absolute(-160, 60); - right_wheels_1.move_absolute(140, 60); - right_wheels_2.move_absolute(140, 60); - while (!((left_wheels_1.get_position() < -155) && (left_wheels_1.get_position() > -165))) { - delay(20); - } - while (!((left_wheels_2.get_position() < -155) && (left_wheels_2.get_position() > -165))) { - delay(20); - } - while (!((right_wheels_1.get_position() < 145) && (right_wheels_1.get_position() > 135))) { - delay(20); - } - while (!((right_wheels_2.get_position() < 145) && (right_wheels_2.get_position() > 135))) { - delay(20); - } - - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - intake_left.move(0); - intake_right.move(0); - delay(50); - left_wheels_1.tare_position(); - left_wheels_2.tare_position(); - right_wheels_1.tare_position(); - right_wheels_2.tare_position(); - -left_wheels_1.move_absolute(-250, 50); -left_wheels_2.move_absolute(-250, 50); -right_wheels_1.move_absolute(-250, 50); -right_wheels_2.move_absolute(-250, 50); - -while (!((left_wheels_1.get_position() < -245) && (left_wheels_1.get_position() > -255))) { - delay(20); -} -while (!((left_wheels_2.get_position() < -245) && (left_wheels_2.get_position() > -255))) { -delay(20); -} -while (!((right_wheels_1.get_position() < -245) && (right_wheels_1.get_position() > -255))) { -delay(20); -} -while (!((right_wheels_2.get_position() < -245) && (right_wheels_2.get_position() > -255))) { -delay(20); -} - -left_wheels_1.move(0); -left_wheels_2.move(0); -right_wheels_1.move(0); -right_wheels_2.move(0); - - //Code to place the cubes in the goal zone - angler.tare_position(); - angler.move_absolute(300, 100); - while (!((angler.get_position() < 305) && (angler.get_position() > 295))) { - delay(20); - } - - angler.move_absolute(450, 75); - while (!((angler.get_position() < 455) && (angler.get_position() > 445))) { - delay(20); - } - - angler.move_absolute(550, 50); - while (!((angler.get_position() < 555) && (angler.get_position() > 545))) { - delay(20); - } - - angler.move(0); - - left_wheels_1.tare_position(); - left_wheels_2.tare_position(); - right_wheels_1.tare_position(); - right_wheels_2.tare_position(); - - //backs away - left_wheels_1.move_absolute(400, 80); - left_wheels_2.move_absolute(400, 80); - right_wheels_1.move_absolute(400, 80); - right_wheels_2.move_absolute(400, 80); - delay(50); - - while (!((left_wheels_1.get_position() < 405) && (left_wheels_1.get_position() > 395))) { - delay(20); - } - while (!((left_wheels_2.get_position() < 405) && (left_wheels_2.get_position() > 395))) { - delay(20); - } - while (!((right_wheels_1.get_position() < 405) && (right_wheels_1.get_position() > 395))) { - delay(20); - } - while (!((right_wheels_2.get_position() < 405) && (right_wheels_2.get_position() > 395))) { - delay(20); - } - - left_wheels_1.move(0); - left_wheels_2.move(0); - right_wheels_1.move(0); - right_wheels_2.move(0); - - //...turn - //...move to tower - //...cube and tower (one motion) - - //...turn - //...move to tower - //...cube and tower (one motion) - //27 points! + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(400, 80); + left_wheels_2.move_absolute(400, 80); + right_wheels_1.move_absolute(400, 80); + right_wheels_2.move_absolute(400, 80); + + while (!((left_wheels_1.get_position() < 405) && (left_wheels_1.get_position() > 395))) { + delay(20); + } + while (!((left_wheels_2.get_position() < 405) && (left_wheels_2.get_position() > 395))) { + delay(20); + } + while (!((right_wheels_1.get_position() < 405) && (right_wheels_1.get_position() > 395))) { + delay(20); + } + while (!((right_wheels_2.get_position() < 405) && (right_wheels_2.get_position() > 395))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(1000); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(-400, 80); + left_wheels_2.move_absolute(-400, 80); + right_wheels_1.move_absolute(-400, 80); + right_wheels_2.move_absolute(-400, 80); + + while (!((left_wheels_1.get_position() < -395) && (left_wheels_1.get_position() > -405))) { + delay(20); + } + while (!((left_wheels_2.get_position() < -395) && (left_wheels_2.get_position() > -405))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -395) && (right_wheels_1.get_position() > -405))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -395) && (right_wheels_2.get_position() > -405))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(1000); } diff --git a/src/motors.cpp b/src/motors.cpp index d9073da..5c3ab08 100644 --- a/src/motors.cpp +++ b/src/motors.cpp @@ -8,7 +8,7 @@ Motor right_wheels_1 (MOTOR3, E_MOTOR_GEARSET_18, 0, E_MOTOR_ENCODER_DEGREES); / Motor left_wheels_2 (MOTOR2, E_MOTOR_GEARSET_18, 1, E_MOTOR_ENCODER_DEGREES); //L2 Motor right_wheels_2 (MOTOR4, E_MOTOR_GEARSET_18, 0, E_MOTOR_ENCODER_DEGREES); //R2 -Motor intake_left (MOTOR7, E_MOTOR_GEARSET_36, 1, E_MOTOR_ENCODER_DEGREES); -Motor intake_right (MOTOR8, E_MOTOR_GEARSET_36, 0, E_MOTOR_ENCODER_DEGREES); +Motor intake_left (MOTOR7, E_MOTOR_GEARSET_18, 1, E_MOTOR_ENCODER_DEGREES); +Motor intake_right (MOTOR8, E_MOTOR_GEARSET_18, 0, E_MOTOR_ENCODER_DEGREES); Motor arm (MOTOR6, E_MOTOR_GEARSET_36, 1, E_MOTOR_ENCODER_DEGREES); Motor angler (MOTOR5, E_MOTOR_GEARSET_36, 0, E_MOTOR_ENCODER_DEGREES); diff --git a/src/opcontrol.cpp b/src/opcontrol.cpp index a09eee2..daadc10 100644 --- a/src/opcontrol.cpp +++ b/src/opcontrol.cpp @@ -44,6 +44,7 @@ void drive(void*) { void intake(void*) { int intake_power = 127; + int outtake_power = 85; //moves intakes while(true) { std::string il = std::to_string(intake_left.get_position()); @@ -52,8 +53,8 @@ void intake(void*) { lcd::set_text(5, ir); if(master.get_digital(DIGITAL_L2)) { - intake_left.move(intake_power); - intake_right.move(intake_power); + intake_left.move(outtake_power); + intake_right.move(outtake_power); } else if(master.get_digital(DIGITAL_R2)) { intake_left.move(-intake_power); @@ -101,8 +102,8 @@ void anglerMove(void*){ delay(20); } - angler.move_absolute(550, 40); - while (!((angler.get_position() < 555) && (angler.get_position() > 545))) { + angler.move_absolute(600, 30); + while (!((angler.get_position() < 605) && (angler.get_position() > 595))) { delay(20); } } From ce50b850a5cecad1d3d57ba7d46273a5e4795998 Mon Sep 17 00:00:00 2001 From: Kirbydude27 Date: Mon, 24 Feb 2020 18:06:23 -0500 Subject: [PATCH 8/8] Autonomous and Prog Skills --- src/autonomous.txt | 214 ++++++++++++++++++++++++ src/progskill.txt | 399 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 613 insertions(+) create mode 100644 src/autonomous.txt create mode 100644 src/progskill.txt diff --git a/src/autonomous.txt b/src/autonomous.txt new file mode 100644 index 0000000..104a290 --- /dev/null +++ b/src/autonomous.txt @@ -0,0 +1,214 @@ + bool red = true; + int leftTurn; + int rightTurn; + + if(red) + { + leftTurn = -475; + rightTurn = 325; + } + + else + { + leftTurn = 175; + rightTurn = -575; + } + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + intake_left.move(-100); + intake_right.move(-100); + delay(100); + + left_wheels_1.move_absolute(-1500, 80); + left_wheels_2.move_absolute(-1500, 80); + right_wheels_1.move_absolute(-1500, 80); + right_wheels_2.move_absolute(-1500, 80); + + while (!((left_wheels_1.get_position() < -1495) && (left_wheels_1.get_position() > -1505))) { + delay(20); + } + while (!((left_wheels_2.get_position() < -1495) && (left_wheels_2.get_position() > -1505))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -1495) && (right_wheels_1.get_position() > -1505))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -1495) && (right_wheels_2.get_position() > -1505))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(1000); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(1050, 80); + left_wheels_2.move_absolute(1050, 80); + right_wheels_1.move_absolute(1050, 80); + right_wheels_2.move_absolute(1050, 80); + + while (!((left_wheels_1.get_position() < 1055) && (left_wheels_1.get_position() > 1045))) { + delay(20); + } + while (!((left_wheels_2.get_position() < 1055) && (left_wheels_2.get_position() > 1045))) { + delay(20); + } + while (!((right_wheels_1.get_position() < 1055) && (right_wheels_1.get_position() > 1045))) { + delay(20); + } + while (!((right_wheels_2.get_position() < 1055) && (right_wheels_2.get_position() > 1045))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(100); + + //Code to rotate ccw 90º + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(leftTurn, 80); + left_wheels_2.move_absolute(leftTurn, 80); + right_wheels_1.move_absolute(rightTurn, 80); + right_wheels_2.move_absolute(rightTurn, 80); + + while (!((left_wheels_1.get_position() < leftTurn + 5) && (left_wheels_1.get_position() > leftTurn - 5))) { + delay(20); + } + while (!((left_wheels_2.get_position() < leftTurn + 5) && (left_wheels_2.get_position() > leftTurn - 5))) { + delay(20); + } + while (!((right_wheels_1.get_position() < rightTurn + 5) && (right_wheels_1.get_position() > rightTurn - 5))) { + delay(20); + } + while (!((right_wheels_2.get_position() < rightTurn + 5) && (right_wheels_2.get_position() > rightTurn - 5))) { + delay(20); + } + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(-50, 30); + left_wheels_2.move_absolute(-50, 30); + right_wheels_1.move_absolute(-50, 30); + right_wheels_2.move_absolute(-50, 30); + + + + while (!((left_wheels_1.get_position() < -45) && (left_wheels_1.get_position() > -55))) { + delay(20); + } + while (!((left_wheels_2.get_position() < -45) && (left_wheels_2.get_position() > -55))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -45) && (right_wheels_1.get_position() > -55))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -45) && (right_wheels_2.get_position() > -55))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + intake_left.move(0); + intake_right.move(0); + delay(50); + + //Code to place the cubes in the goal zone + angler.tare_position(); + angler.move_absolute(300, 100); + while (!((angler.get_position() < 305) && (angler.get_position() > 295))) { + delay(20); + } + + angler.move_absolute(450, 60); + while (!((angler.get_position() < 455) && (angler.get_position() > 445))) { + delay(20); + } + + angler.move_absolute(550, 40); + while (!((angler.get_position() < 555) && (angler.get_position() > 545))) { + delay(20); + } + + angler.move(0); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + //pushes! + left_wheels_1.move_absolute(-50, 25); + left_wheels_2.move_absolute(-50, 25); + right_wheels_1.move_absolute(-50, 25); + right_wheels_2.move_absolute(-50, 25); + + while (!((left_wheels_1.get_position() < -45) && (left_wheels_1.get_position() > -55))) { + delay(20); + } + while (!((left_wheels_2.get_position() < -45) && (left_wheels_2.get_position() > -55))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -45) && (right_wheels_1.get_position() > -55))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -45) && (right_wheels_2.get_position() > -55))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(2000); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + //backs away + left_wheels_1.move_absolute(400, 60); + left_wheels_2.move_absolute(400, 60); + right_wheels_1.move_absolute(400, 60); + right_wheels_2.move_absolute(400, 60); + delay(50); + + while (!((left_wheels_1.get_position() < 405) && (left_wheels_1.get_position() > 395))) { + delay(20); + } + while (!((left_wheels_2.get_position() < 405) && (left_wheels_2.get_position() > 395))) { + delay(20); + } + while (!((right_wheels_1.get_position() < 405) && (right_wheels_1.get_position() > 395))) { + delay(20); + } + while (!((right_wheels_2.get_position() < 405) && (right_wheels_2.get_position() > 395))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); \ No newline at end of file diff --git a/src/progskill.txt b/src/progskill.txt new file mode 100644 index 0000000..ffb546a --- /dev/null +++ b/src/progskill.txt @@ -0,0 +1,399 @@ + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + intake_left.move(-100); + intake_right.move(-100); + delay(100); + + left_wheels_1.move_absolute(-3070, 75); + left_wheels_2.move_absolute(-3070, 75); + right_wheels_1.move_absolute(-3070, 75); + right_wheels_2.move_absolute(-3070, 75); + + while (!((left_wheels_1.get_position() < -3065) && (left_wheels_1.get_position() > -3075))) { + delay(20); + } + while (!((left_wheels_2.get_position() < -3065) && (left_wheels_2.get_position() > -3075))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -3065) && (right_wheels_1.get_position() > -3075))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -3065) && (right_wheels_2.get_position() > -3075))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(100); + std::string d1 = std::to_string(left_wheels_1.get_position()); + lcd::set_text(0, d1); + std::string d2 = std::to_string(right_wheels_1.get_position()); + lcd::set_text(1, d2); + std::string d3 = std::to_string(left_wheels_2.get_position()); + lcd::set_text(2, d3); + std::string d4 = std::to_string(right_wheels_2.get_position()); + lcd::set_text(3, d4); + + //Code to rotate ccw 90º + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + lcd::set_text(4, "Fuck!"); + + left_wheels_1.move_absolute(-110, 60); + left_wheels_2.move_absolute(-110, 60); + right_wheels_1.move_absolute(90, 60); + right_wheels_2.move_absolute(90, 60); + while (!((left_wheels_1.get_position() < -105) && (left_wheels_1.get_position() > -115))) { + delay(20); + } + while (!((left_wheels_2.get_position() < -105) && (left_wheels_2.get_position() > -115))) { + delay(20); + } + while (!((right_wheels_1.get_position() < 95) && (right_wheels_1.get_position() > 85))) { + delay(20); + } + while (!((right_wheels_2.get_position() < 95) && (right_wheels_2.get_position() > 85))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(100); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(-425, 50); + left_wheels_2.move_absolute(-425, 50); + right_wheels_1.move_absolute(-425, 50); + right_wheels_2.move_absolute(-425, 50); + + while (!((left_wheels_1.get_position() < -420) && (left_wheels_1.get_position() > -430))) { + delay(50); + } + while (!((left_wheels_2.get_position() < -420) && (left_wheels_2.get_position() > -430))) { + delay(50); + } + while (!((right_wheels_1.get_position() < -420) && (right_wheels_1.get_position() > -430))) { + delay(50); + } + while (!((right_wheels_2.get_position() < -420) && (right_wheels_2.get_position() > -430))) { + delay(50); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + intake_left.move(0); + intake_right.move(0); + delay(100); + + intake_left.move(25); + intake_right.move(25); + delay(500); + + intake_left.move(0); + intake_right.move(0); + delay(50); + + //Code to place the cubes in the goal zone + angler.tare_position(); + angler.move_absolute(300, 100); + while (!((angler.get_position() < 305) && (angler.get_position() > 295))) { + delay(20); + } + + angler.move_absolute(450, 60); + while (!((angler.get_position() < 455) && (angler.get_position() > 445))) { + delay(20); + } + + angler.move_absolute(570, 40); + while (!((angler.get_position() < 575) && (angler.get_position() > 565))) { + delay(20); + } + + angler.move(0); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + //pushes! + left_wheels_1.move_absolute(-75, 25); + left_wheels_2.move_absolute(-75, 25); + right_wheels_1.move_absolute(-75, 25); + right_wheels_2.move_absolute(-75, 25); + + delay(1000); + + // while (!((left_wheels_1.get_position() < -70) && (left_wheels_1.get_position() > -80))) { + // delay(20); + // } + // while (!((left_wheels_2.get_position() < -70) && (left_wheels_2.get_position() > -80))) { + // delay(20); + // } + // while (!((right_wheels_1.get_position() < -70) && (right_wheels_1.get_position() > -80))) { + // delay(20); + // } + // while (!((right_wheels_2.get_position() < -70) && (right_wheels_2.get_position() > -80))) { + // delay(20); + // } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(1000); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + //backs away + left_wheels_1.move_absolute(400, 60); + left_wheels_2.move_absolute(400, 60); + right_wheels_1.move_absolute(400, 60); + right_wheels_2.move_absolute(400, 60); + delay(50); + + while (!((left_wheels_1.get_position() < 405) && (left_wheels_1.get_position() > 395))) { + delay(20); + } + while (!((left_wheels_2.get_position() < 405) && (left_wheels_2.get_position() > 395))) { + delay(20); + } + while (!((right_wheels_1.get_position() < 405) && (right_wheels_1.get_position() > 395))) { + delay(20); + } + while (!((right_wheels_2.get_position() < 405) && (right_wheels_2.get_position() > 395))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + + //turn... + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(450, 60); + left_wheels_2.move_absolute(450, 60); + right_wheels_1.move_absolute(-300, 60); + right_wheels_2.move_absolute(-300, 60); + + while (!((left_wheels_1.get_position() < 455) && (left_wheels_1.get_position() > 445))) { + delay(20); + } + while (!((left_wheels_2.get_position() < 455) && (left_wheels_2.get_position() > 445))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -295) && (right_wheels_1.get_position() > -305))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -295) && (right_wheels_2.get_position() > -305))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(100); + + //...move to tower + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + intake_left.move(-70); + intake_right.move(-70); + delay(100); + + left_wheels_1.move_absolute(-1200, 75); + left_wheels_2.move_absolute(-1200, 75); + right_wheels_1.move_absolute(-1200, 75); + right_wheels_2.move_absolute(-1200, 75); + + while (!((left_wheels_1.get_position() < -1195) && (left_wheels_1.get_position() > -1205))) { + delay(20); + } + while (!((left_wheels_2.get_position() < -1195) && (left_wheels_2.get_position() > -1205))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -1195) && (right_wheels_1.get_position() > -1205))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -1195) && (right_wheels_2.get_position() > -1205))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + intake_left.move(0); + intake_right.move(0); + delay(100); + + left_wheels_1.move_absolute(-1100, 60); + left_wheels_2.move_absolute(-1100, 60); + right_wheels_1.move_absolute(-1100, 60); + right_wheels_2.move_absolute(-1100, 60); + delay(1000); + + //...cube and tower (one motion) + + angler.move_absolute(0, 80); + while (!((angler.get_position() < 10) && (angler.get_position() > -10))) { + delay(20); + } + + angler.tare_position(); + arm.tare_position(); + + angler.move_absolute(350, 80); + arm.move_absolute(440, 80); + while (!((angler.get_position() < 355) && (angler.get_position() > 345))) { + delay(20); + } + while (!((arm.get_position() < 445) && (arm.get_position() > 435))) { + delay(20); + } + + left_wheels_1.move_absolute(-1200, 60); + left_wheels_2.move_absolute(-1200, 60); + right_wheels_1.move_absolute(-1200, 60); + right_wheels_2.move_absolute(-1200, 60); + delay(1000); + + intake_left.move(75); + intake_right.move(75); + delay(1000); + + intake_left.move(0); + intake_right.move(0); + + //...turn + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(1000, 75); + left_wheels_2.move_absolute(1000, 75); + right_wheels_1.move_absolute(1000, 75); + right_wheels_2.move_absolute(1000, 75); + + while (!((left_wheels_1.get_position() < 1005) && (left_wheels_1.get_position() > 995))) { + delay(20); + } + while (!((left_wheels_2.get_position() < 1005) && (left_wheels_2.get_position() > 995))) { + delay(20); + } + while (!((right_wheels_1.get_position() < 1005) && (right_wheels_1.get_position() > 995))) { + delay(20); + } + while (!((right_wheels_2.get_position() < 1005) && (right_wheels_2.get_position() > 995))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(100); + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + left_wheels_1.move_absolute(200, 60); + left_wheels_2.move_absolute(200, 60); + right_wheels_1.move_absolute(-225, 60); + right_wheels_2.move_absolute(-225, 60); + + while (!((left_wheels_1.get_position() < 205) && (left_wheels_1.get_position() > 195))) { + delay(20); + } + while (!((left_wheels_2.get_position() < 205) && (left_wheels_2.get_position() > 195))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -220) && (right_wheels_1.get_position() > -230))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -220) && (right_wheels_2.get_position() > -230))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + delay(100); + + //...move to tower + + + left_wheels_1.tare_position(); + left_wheels_2.tare_position(); + right_wheels_1.tare_position(); + right_wheels_2.tare_position(); + + intake_left.move(-100); + intake_right.move(-100); + delay(100); + + left_wheels_1.move_absolute(-1000, 75); + left_wheels_2.move_absolute(-1000, 75); + right_wheels_1.move_absolute(-1000, 75); + right_wheels_2.move_absolute(-1000, 75); + + while (!((left_wheels_1.get_position() < -995) && (left_wheels_1.get_position() > -1005))) { + delay(20); + } + while (!((left_wheels_2.get_position() < -995) && (left_wheels_2.get_position() > -1005))) { + delay(20); + } + while (!((right_wheels_1.get_position() < -995) && (right_wheels_1.get_position() > -1005))) { + delay(20); + } + while (!((right_wheels_2.get_position() < -995) && (right_wheels_2.get_position() > -1005))) { + delay(20); + } + + left_wheels_1.move(0); + left_wheels_2.move(0); + right_wheels_1.move(0); + right_wheels_2.move(0); + intake_left.move(0); + intake_right.move(0); + delay(100); + + //...cube and tower (one motion) + //27 points! \ No newline at end of file