diff --git a/compile_commands.json b/compile_commands.json index d347dc3..4cb0058 100644 --- a/compile_commands.json +++ b/compile_commands.json @@ -31,7 +31,7 @@ "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" }, { @@ -66,7 +66,7 @@ "bin/pid.cpp.o", "src\\pid.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", + "directory": "C:\\Users\\shao4\\Downloads\\Terrebonne\\2381WCode-2", "file": "src\\pid.cpp" }, { @@ -98,11 +98,46 @@ "-fdiagnostics-color", "--std=gnu++17", "-o", - "bin/autonomous.cpp.o", - "src\\autonomous.cpp" + "bin/motors.cpp.o", + "src\\motors.cpp" ], - "directory": "C:\\Users\\David\\Documents\\2381WCode", - "file": "src\\autonomous.cpp" + "directory": "C:\\Users\\shao4\\Downloads\\Terrebonne\\2381WCode-2", + "file": "src\\motors.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/adi.cpp.o", + "src\\adi.cpp" + ], + "directory": "C:\\Users\\shao4\\Downloads\\Terrebonne\\2381WCode-2", + "file": "src\\adi.cpp" }, { "arguments": [ @@ -136,7 +171,42 @@ "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 c995433..d471c09 100644 --- a/include/main.h +++ b/include/main.h @@ -15,6 +15,22 @@ #ifndef _PROS_MAIN_H_ #define _PROS_MAIN_H_ +//defines the ports for each motor +#define MOTOR1 12 //L1 +#define MOTOR2 9 //L2 +#define MOTOR3 11 //R1 +#define MOTOR4 2 //R2 + +#define MOTOR5 13 //Angler +#define MOTOR6 4 //Arm +#define MOTOR7 7 //LIntake +#define MOTOR8 19 //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 * 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..3cea6cc 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": "Robosuck", + "slot": 2 + } } } \ No newline at end of file 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 5eda0a2..eb43835 100644 --- a/src/autonomous.cpp +++ b/src/autonomous.cpp @@ -1,131 +1,88 @@ #include "main.h" - #include - -//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 +#include "motors.h" +#include "adi.h" +#include "pid.h" 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 +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(); + + 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); - 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); +movement(); } 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/initialize.cpp b/src/initialize.cpp index c2bc5fa..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,11 +19,7 @@ void on_center_button() { * to keep execution time for this mode under a few seconds. */ 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); + lcd::initialize(); } /** diff --git a/src/motors.cpp b/src/motors.cpp new file mode 100644 index 0000000..5c3ab08 --- /dev/null +++ b/src/motors.cpp @@ -0,0 +1,14 @@ +#include "main.h" +#include "motors.h" + +using namespace pros; + +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_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 3b872b9..daadc10 100644 --- a/src/opcontrol.cpp +++ b/src/opcontrol.cpp @@ -1,171 +1,142 @@ #include "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 +#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 +Mutex mutex; + //defines controller Controller master (CONTROLLER_MASTER); -Mutex mutex; //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) { - 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) { + 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); + + int power = master.get_analog(ANALOG_LEFT_Y); + 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); + pros::delay(20); } } void intake(void*) { - int intake_power = 100; + int intake_power = 127; + int outtake_power = 85; //moves intakes while(true) { - 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)) { - delay(20); - } - } - else { - intake_left.move(0); - intake_right.move(0); - } - delay(20); + std::string il = std::to_string(intake_left.get_position()); + lcd::set_text(4, il); + std::string ir = std::to_string(intake_right.get_position()); + lcd::set_text(5, ir); + + if(master.get_digital(DIGITAL_L2)) { + intake_left.move(outtake_power); + intake_right.move(outtake_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; - - while(true) { - //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); - } - delay(20); +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(600, 30); + while (!((angler.get_position() < 605) && (angler.get_position() > 595))) { + delay(20); + } + } + else{ + angler.move(0); + angler.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); + angler.move(0); + } + + pros::delay(20); } - - } -void anglerMove(void*) { +void arms(void*) { - int angler_power = 100; + int arm_power = 127; while(true) { - //moves angler - if(master.get_digital(DIGITAL_R1)) { - angler.move(angler_power); - } - else if(master.get_digital(DIGITAL_L1)) { - angler.move(-angler_power); - } - else { - angler.move(0); + 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); } - delay(20); - } -} - -void towerScore(void*) { - - 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*) { - - 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(); - } - } void opcontrol() { @@ -173,6 +144,4 @@ void opcontrol() { 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"); } 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; -} 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