diff --git a/docs/auton.org b/docs/auton.org new file mode 100644 index 0000000..92ebc52 --- /dev/null +++ b/docs/auton.org @@ -0,0 +1,42 @@ +#+OPTIONS: toc:nil +#+LATEX_HEADER: \usepackage{nopageno} +#+STARTUP: inlineimages + + + +* Autonomous Map +0. [@0] AUTON 0 + Auton pushes one tribal into goal, then turns and drives towards the bar and deploys flaps to touch. + + Opposite Side + + Total Score: 5? + + 2/3 of auton win point +1. AUTON 1 + Deploys wings to knock triball out of match load zone, then drives forward + + Close Side + + Total Score +2? + + 1/3 of auton win point +2. AUTON 2 + Deploys wings to knock triball out of match load zone, then drives forward. Retracts wings, and drives towards the bar before redeploying wings to touch the bar. + + Close Side + + Total Score +2? + + 2/3 of auton win point + + Sub-auton of auton 1 +3. AUTON 3 + Uses triball detection distance sensor to shoot 44 triballs over to the other side. Then retracts catapult, moves forward, and pushes triballs under goal. + + Should be setup 30 degress left of straight + + Skills auton + + Currently non functional due to no shooter +4. AUTON 4 + Then retracts catapult, moves forward, and pushes triballs under goal. + + Currently non functional due to no shooter + + Can be run exclusively + + Setup should be 30 degrees left of straight +5. AUTON 5 + Drives forward grabbing triball in center of field. Then pulls triball back, rotates right, and pushes two triballs towards our side. Then reverses and knocks triball into goal. + + Opposing side + + Scores 9 points + + Can counter other teams +6. AUTON 6 + Push auton. Moves 2ft backward then 0.5ft forward. + + Utility auton + diff --git a/src/main.cpp b/src/main.cpp index 0caa71a..43bcb1d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,6 +9,7 @@ /** * You should add more #includes here */ +#include "display/lv_misc/lv_task.h" #include "okapi/api.hpp" #include "okapi/api/device/motor/abstractMotor.hpp" #include "okapi/api/units/QLength.hpp" @@ -23,6 +24,9 @@ #include +// 1 2 3 5 //left +// 7 8 9 10 //right + int autonpick = 0; int automod = 0; lv_res_t sa0(lv_obj_t *btn) { @@ -86,6 +90,7 @@ void shiftarray(bool *arr, bool ap) { bool is_tapped(bool *log, bool ap) { shiftarray(log, ap); if (log[0] == true && log[9] == false && log[1] == false) { + printf("a: %d\n", log[0]); return true; } else { @@ -201,20 +206,21 @@ void autonomous() { fclose(usd_file_read); // always close files when you're done with them autonpick = num; // int autonpick = 3; - std::shared_ptr drive = - ChassisControllerBuilder() - .withMotors( - {-1, -2, -3}, - { - 5, - 6, - 7, - }) - .withDimensions({AbstractMotor::gearset::blue, (60.0 / 48.0)}, - {{3.25_in, 12_in}, imev5BlueTPR}) + std::shared_ptr drive = + // -5 is + ChassisControllerBuilder() + .withMotors({-1, -2, -3, -5}, {7, 8, 9, 10}) + .withDimensions({AbstractMotor::gearset::blue, (48.0 / 36.0)}, + {{3.25_in, 11.2_in}, imev5BlueTPR}) .build(); + pros::ADIDigitalOut funnypiston(3); + funnypiston.set_value(false); + + pros::ADIDigitalOut upflap(8); + upflap.set_value(false); + pros::Imu IMU(19); IMU.reset(true); IMU.tare(); @@ -229,71 +235,75 @@ void autonomous() { pros::ADIDigitalOut piston(2); if (autonpick == 0) { // Auton #1, used for adding tribal to goal - float x = -3.2; + float x = -2.7; drive->moveDistanceAsync(x * 1_ft); pros::delay(2500); - drive->moveDistance(1_ft); + drive->moveDistance(0.5_ft); double yaw = IMU.get_heading(); - std::printf("%f", yaw); - double turn = -92 - yaw; + std::printf("yaw: %f\n", yaw); + // double turn = -92 - yaw; + double turn = 45 - yaw; + std: + printf("turn: %f\n", turn); drive->setMaxVelocity(300); drive->turnAngle(turn * 1_deg); - drive->moveDistanceAsync(-4.1_ft); + drive->moveDistanceAsync(4.1_ft); pros::delay(3000); piston.set_value(true); } else if (autonpick == 1 || autonpick == 2) { // Auton #2, Used for removing triball from match loader piston.set_value(true); + drive->moveDistance(1_ft); if (autonpick == 2) { drive->setMaxVelocity(300); pros::delay(1000); piston.set_value(false); - drive->moveDistance(-1.7_ft); - drive->turnAngle(87_deg); - drive->moveDistance(-3.5_ft); + // drive->moveDistance(-1.7_ft); + drive->turnAngle(115_deg); + drive->moveDistance(3.5_ft); drive->turnAngle(95_deg); - drive->moveDistance(-2_in); + drive->setMaxVelocity(50); + drive->moveDistanceAsync(2_ft); + pros::delay(2500); piston.set_value(true); - drive->moveDistance(-4_in); + drive->moveDistance(4_in); } // drive->turnAngle(45_deg); } else if (autonpick == 3) { + upflap.set_value(true); + bool reqtopull = false; bool good[10]; pros::Distance dist(13); // fixed - int counter = 0; + int counter = 0; pull.tarePosition(); printf("hereasdsadfasdfsdf\n"); - bool reqtopullarr[10]; - for (int i = 0; i<10; i++){ - reqtopullarr[i] = false; - } - - while (counter<44) { + bool reqtopullarr[10]; + for (int i = 0; i < 10; i++) { + reqtopullarr[i] = false; + } + while (counter < 44) { - - if (is_held(good, dist.get() < 115)) { - reqtopull = true; - } + if (is_held(good, dist.get() < 115)) { + reqtopull = true; + } - if(is_tapped_short(reqtopullarr,reqtopull)) { - counter = counter + 1; - } - pros::lcd::set_text(7,std::to_string(counter)); + if (is_tapped_short(reqtopullarr, reqtopull)) { + counter = counter + 1; + } + pros::lcd::set_text(7, std::to_string(counter)); - - - // if (pros::millis() % 2000 <= 10){ - // reqtopull = true; - // } + // if (pros::millis() % 2000 <= 10){ + // reqtopull = true; + // } if (reqtopull) { // pull.moveRelative((1800 - std::fmod(pull.getPosition(), // 1800)), - // 1000); + // 1000); // reqtopull = false; float temp = std::fmod(pull.getPosition(), 900); if ((900 - temp) < 150) { @@ -301,93 +311,129 @@ void autonomous() { } pull.moveRelative( (900 - - temp), // pull.moveRelative((1800 - - // std::fmod(pull.getPosition(), 1800)), + temp), // pull.moveRelative((1800 + //- + //std::fmod(pull.getPosition(), 1800)), 10000000); reqtopull = false; - } pros::delay(10); - } - - - + } } - if (autonpick == 3 || autonpick == 8) { - pros::delay(500); - pros::Distance distcal(20); - bool good = false; - while (!good) { - if (!(distcal.get() < 35)) { + if (autonpick == 3 || autonpick == 4) { + pros::delay(500); + pros::Distance distcal(20); + bool good = false; + while (!good) { + if (!(distcal.get() < 35)) { // pull.moveVoltage(-12000); pull.moveVoltage(12000); } else { pullback = false; ready = true; - good = true; + good = true; pull.moveVoltage(0); pull.tarePosition(); } + } + drive->setMaxVelocity(300); + drive->turnAngle(30_deg); + drive->setMaxVelocity(600); - } - drive->setMaxVelocity(300); - drive->turnAngle(30_deg); - drive->setMaxVelocity(600); + drive->moveDistance(-7.1_ft); - drive->moveDistance(-7.1_ft); + pros::delay(500); - - pros::delay(500); - - double yaw = IMU.get_heading(); + double yaw = IMU.get_heading(); std::printf("%f\n", yaw); double turn = -60 - yaw; - + drive->setMaxVelocity(300); drive->turnAngle(turn * 1_deg); - - pros::delay(300); - drive->moveDistanceAsync(-5_ft); - pros::delay(2000); - piston.set_value(true); + pros::delay(300); + drive->moveDistanceAsync(-5_ft); + pros::delay(2000); + piston.set_value(true); - //drive->turnAngle(90_deg); - yaw = IMU.get_heading(); - turn = 30 - yaw; - drive->turnAngle(turn*1_deg); - - drive->moveDistance(-3_ft); + // drive->turnAngle(90_deg); + yaw = IMU.get_heading(); + turn = 30 - yaw; + drive->turnAngle(turn * 1_deg); - drive->turnAngle(90_deg); - drive->moveDistance(-4_ft); - - - } + drive->moveDistance(-3_ft); - - if (autonpick == -1) { drive->turnAngle(90_deg); - } - if (autonpick == 4) { - float x = sqrt(8); - drive->moveDistance(x * -1_ft); - pros::delay(500); - drive->moveDistance(0.5_ft); + drive->moveDistance(-4_ft); } if (autonpick == 5) { - pull.moveRelative(900, 1000000); + pros::Distance distforward(17); + // drive->moveDistanceAsync(500_ft); + drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::hold); + // piston.set_value(true); + // NEED TO WRTIE IMU CENTER CODE + // update IMU each cycle to get true rotation + while (distforward.get() > 320) { + drive->getModel()->curvature(12000, IMU.get_rotation() * -0.04); + + // printf("%i\n", distforward.get()); + pros::delay(10); + } + // piston.set_value(false); + + drive->stop(); + funnypiston.set_value(true); + pros::delay(500); + + auto starttime = Timer().millis(); + while (Timer().millis() < starttime + 1.5_s ) { + drive->getModel()->curvature(-12000 , IMU.get_rotation() * -0.04); + + // printf("%i\n", distforward.get()); + pros::delay(10); + } + // piston.set_value(false); + drive->stop(); + funnypiston.set_value(true); + pros::delay(500); + + drive->moveDistance(1_in); + // drive->turnAngle(15_deg); + //drive->moveDistance(-3.8_ft); + //drive->turnAngle(90_deg); + bool imuholdarray[10]; + + while (!is_held(imuholdarray, (abs(68.0 - IMU.get_rotation()) < 1)) ) { + drive->getModel()->rotate((68.0 - IMU.get_rotation()) * 0.008 ); + pros::delay(10); + } + + drive->getModel()->stop(); + + funnypiston.set_value(false); + drive->moveDistance(2_ft); + drive->moveDistance(-2_ft); + funnypiston.set_value(true); + drive->moveDistance(2_ft); + drive->moveDistance(-2_ft); + funnypiston.set_value(false); + drive->turnAngle(45_deg); + drive->moveDistance(-2_ft); + drive->moveDistance(1_ft); } - if (autonpick==6) { - drive->moveDistance(-1.95_ft); - drive->moveDistance(1_ft); + if (autonpick == 6) { + drive->setMaxVelocity(600); + float x = 2; + drive->moveDistance(x * -1_ft); + pros::delay(500); + drive->moveDistance(0.5_ft); } } @@ -411,16 +457,11 @@ void opcontrol() { // pros::delay(100); std::shared_ptr drive = ChassisControllerBuilder() - .withMotors( - { - -5, - -6, - -7, - }, - {1, 2, 3}) + .withMotors({-1, -2, -3, -5}, {7, 8, 9, 10}) + // Green gearset, 4 in wheel diam, 11.5 in wheel track .withDimensions({AbstractMotor::gearset::blue, (48.0 / 36.0)}, - {{3.25_in, 12_in}, imev5BlueTPR}) + {{3.25_in, 11.2_in}, imev5BlueTPR}) .build(); Controller controller; @@ -430,32 +471,27 @@ void opcontrol() { pros::ADIDigitalOut funnypiston(3); pros::ADIDigitalOut upflap(8); - Motor leftMotor1(-5); - Motor leftMotor2(-6); - Motor leftMotor3(-7); + Motor leftMotor1(-1); + Motor leftMotor2(-2); + Motor leftMotor3(-3); + Motor leftMotor4(-5); - Motor rightMotor1(1); - Motor rightMotor2(2); - Motor rightMotor3(3); - - //lift on Ltrig + Motor rightMotor1(7); + Motor rightMotor2(8); + Motor rightMotor3(9); + Motor rightMotor4(10); + // lift on Ltrig // intake on A - leftMotor1.setBrakeMode(okapi::AbstractMotor::brakeMode::brake); - leftMotor2.setBrakeMode(okapi::AbstractMotor::brakeMode::brake); - leftMotor3.setBrakeMode(okapi::AbstractMotor::brakeMode::brake); - - rightMotor1.setBrakeMode(okapi::AbstractMotor::brakeMode::brake); - rightMotor2.setBrakeMode(okapi::AbstractMotor::brakeMode::brake); - rightMotor3.setBrakeMode(okapi::AbstractMotor::brakeMode::brake); + drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::brake); bool flipped = false; Motor flywheel(15); Motor pull(12); - //pull.setReversed(true); + // pull.setReversed(true); Motor lift(18); @@ -469,10 +505,10 @@ void opcontrol() { // MotorGroup pull = MotorGroup({pull1, pull2}); flywheel.setBrakeMode(okapi::AbstractMotor::brakeMode::coast); - flywheel.setGearing(okapi::AbstractMotor::gearset::blue); //should be red for cata + flywheel.setGearing( + okapi::AbstractMotor::gearset::blue); // should be red for cata pull.setBrakeMode(okapi::AbstractMotor::brakeMode::coast); - lift.setBrakeMode(okapi::AbstractMotor::brakeMode::hold); ControllerButton R2(ControllerDigital::R2); @@ -489,7 +525,6 @@ void opcontrol() { ControllerButton leftarrow(ControllerDigital::left); ControllerButton Abutton(ControllerDigital::A); - ControllerButton uparrow(ControllerDigital::up); ControllerButton Xbutton(ControllerDigital::X); @@ -505,14 +540,14 @@ void opcontrol() { flaparr[i] = false; } - bool l1toggle[10]; - for (int i=0; i<10; i++) { - l1toggle[i] = false; + bool l1toggle[10]; + for (int i = 0; i < 10; i++) { + l1toggle[i] = false; } bool l2toggle[10]; - for (int i=0; i < 10; i++) { - l2toggle[i] = false; + for (int i = 0; i < 10; i++) { + l2toggle[i] = false; } bool pullback = false; @@ -558,11 +593,10 @@ void opcontrol() { } bool barr[10]; - for (int i = 0; i < 10; i++){ - barr[i] = false; + for (int i = 0; i < 10; i++) { + barr[i] = false; } - bool funnypistonstatus = false; auto IMU = okapi::IMU(19, IMUAxes::z); bool reqtoloop = false; @@ -589,20 +623,20 @@ void opcontrol() { // every 10 ms. float lsum = leftMotor1.getTemperature() + leftMotor2.getTemperature() + - leftMotor3.getTemperature(); - float lavg = lsum / 3; + leftMotor3.getTemperature() + leftMotor4.getTemperature(); + float lavg = lsum / 4; - float rsum = rightMotor1.getTemperature() + - rightMotor2.getTemperature() + - rightMotor3.getTemperature(); - float ravg = rsum / 3; + float rsum = + rightMotor1.getTemperature() + rightMotor2.getTemperature() + + rightMotor3.getTemperature() + rightMotor4.getTemperature(); + float ravg = rsum / 4; - if (lavg > 150) { - lavg = -9; - } - if (ravg >150) { - ravg = -9; - } + if (lavg > 150) { + lavg = -9; + } + if (ravg > 150) { + ravg = -9; + } if (pullback) { if (!(dist.get() < 35)) { @@ -626,9 +660,9 @@ void opcontrol() { tcount++; } - if(is_tapped(l1toggle, L1.isPressed() )) { - flapsOn = !flapsOn; - } + if (is_tapped(l1toggle, L1.isPressed())) { + flapsOn = !flapsOn; + } if (is_tapped(manpullarr, (uparrow.isPressed() && Xbutton.isPressed()))) { @@ -673,7 +707,7 @@ void opcontrol() { tempaselect = tempaselect - 1; } if (is_tapped(a, Abutton.isPressed())) { - // write to file; + // write to file; FILE *autofile = fopen("/usd/auto.txt", "w"); fprintf(autofile, "%d", tempaselect); fclose(autofile); @@ -692,66 +726,64 @@ void opcontrol() { funnypiston.set_value(funnypistonstatus); pros::lcd::set_text(1, std::to_string(tcount)); - if (!flipperstatus) { + if (flipperstatus) { pros::lcd::set_text(0, "flip f flippcmdreceive"); drive = ChassisControllerBuilder() - .withMotors({-5, -6, -7}, {1, 2, 3}) + .withMotors({-7, -8, -9, -10}, {1, 2, 3, 5}) .withDimensions( {AbstractMotor::gearset::blue, (48.0 / 36.0)}, - {{3.25_in, 12_in}, imev5BlueTPR}) - .build(); - flipped = false; - } - if (flipperstatus) { - pros::lcd::set_text(0, "flip b flippcmdreceive"); - - drive = ChassisControllerBuilder() - .withMotors({-1, -2, -3}, {5, 6, 7}) - .withDimensions( - {AbstractMotor::gearset::blue, (48.0 / 36.0)}, - {{3.25_in, 12_in}, imev5BlueTPR}) + {{3.25_in, 11.2_in}, imev5BlueTPR}) .build(); flipped = true; } + if (!flipperstatus) { + pros::lcd::set_text(0, "flip b flippcmdreceive"); + drive = ChassisControllerBuilder() + .withMotors({-1, -2, -3, -5}, {7, 8, 9, 10}) + .withDimensions( + {AbstractMotor::gearset::blue, (48.0 / 36.0)}, + {{3.25_in, 11.2_in}, imev5BlueTPR}) + .build(); + flipped = false; + } - // if (R1.isPressed()) { - // flywheel.moveVoltage(-12000); - // } - // else { - // flywheel.moveVoltage(0); - // } + // if (R1.isPressed()) { + // flywheel.moveVoltage(-12000); + // } + // else { + // flywheel.moveVoltage(0); + // } - - // if (fly.isPressed()) { - // pullback = true; + // pullback = true; - // pros::lcd::set_text(5, "hit while not"); + // pros::lcd::set_text(5, "hit while not"); // } // if (is_tapped(loopmodearr, downarrow.isPressed())) { - // lmode = !lmode; - // //lmode = false; + // lmode = !lmode; + // //lmode = false; // } // // printf("%i",dist.get()); // if (is_held(distarr, dististriball.get() < 70) && lmode) { - // reqtoloop = true; - // // printf("raaaaaaaaaaaaaa"); + // reqtoloop = true; + // // printf("raaaaaaaaaaaaaa"); // } if (is_tapped(r1arr, R1.isPressed()) || reqtoloop == true) { float temp = std::fmod(pull.getPosition(), 900); - pros::lcd::set_text(6,std::to_string(temp)); + pros::lcd::set_text(6, std::to_string(temp)); if ((900 - temp) < 150) { temp = -(900 - temp); - pros::lcd::set_text(7,std::to_string(temp)); + pros::lcd::set_text(7, std::to_string(temp)); } pull.moveRelative( - (900 - - temp), // pull.moveRelative((1800 - - // std::fmod(pull.getPosition(), 1800)), + (900 - + temp), // pull.moveRelative((1800 + //- std::fmod(pull.getPosition(), + // 1800)), 10000000); reqtoloop = false; @@ -767,34 +799,24 @@ void opcontrol() { } // if (dist.get() < 40 && - // (std::abs(pull.getPosition() - pull.getTargetPosition()) < 5)) { - // pull.moveVoltage(0); + // (std::abs(pull.getPosition() - pull.getTargetPosition()) < + // 5)) { pull.moveVoltage(0); // } else { // } - pros::lcd::set_text(4, std::to_string(pull.getTargetPosition())); - - - - //pros::lcd::set_text(5, std::to_string(IMU.get())); - - - - pros::lcd::set_text(5,std::to_string(pull.getPosition())); + // pros::lcd::set_text(5, std::to_string(IMU.get())); + pros::lcd::set_text(5, std::to_string(pull.getPosition())); controller.setText( 0, 0, - std::to_string(static_cast(pull.getTemperature())) + - std::to_string(static_cast(lavg)) + + std::to_string(static_cast(lavg)) + std::to_string(static_cast(ravg)) + - std::to_string(static_cast(lift.getTemperature())) + std::to_string(static_cast(lift.getTemperature())) - - - ); + ); pros::delay(10); } }