diff --git a/include/auton.h b/include/auton.h index 8c4a8697..358f067c 100755 --- a/include/auton.h +++ b/include/auton.h @@ -1 +1 @@ -#define AUTON 5 +#define AUTON 0 diff --git a/include/hardware/hardware.hpp b/include/hardware/hardware.hpp index 397f656d..872d1748 100755 --- a/include/hardware/hardware.hpp +++ b/include/hardware/hardware.hpp @@ -93,7 +93,14 @@ namespace Pronounce { } if (portsReport.empty()) { - master.getController().set_text(0, 0, "All good"); + Log(std::to_string(pros::battery::get_capacity())) + if (pros::battery::get_capacity() < 50.0) { + master.getController().rumble("........"); + pros::Task::delay(50); + master.getController().set_text(0, 0, string_format("Battery at: %.2f", pros::battery::get_capacity())); + } else { + master.getController().set_text(0, 0, "All good"); + } } else { master.getController().set_text(0, 0, portsReport.c_str()); pros::Task::delay(50); diff --git a/include/motionControl/rotationController.hpp b/include/motionControl/rotationController.hpp index 7ab3b942..ada8b849 100755 --- a/include/motionControl/rotationController.hpp +++ b/include/motionControl/rotationController.hpp @@ -9,8 +9,14 @@ #include "pros/rtos.hpp" #include "hardware/hardware.hpp" -namespace Pronounce -{ +namespace Pronounce { + enum RotationOptimizer { + none, + closest, + clockwise, + counterclockwise + }; + class RotationController : public Behavior { private: PID rotationPID; @@ -22,8 +28,10 @@ namespace Pronounce double idleSpeed = 0.0; + RotationOptimizer rotationOptimizer; + public: - RotationController(std::string name, TankDrivetrain& drivetrain, std::function angleFunction, PID rotationPID, Angle target, double idleSpeed = 0.0) : drivetrain(drivetrain), rotationPID(rotationPID), angleFunction(std::move(angleFunction)), Behavior(name) { + RotationController(std::string name, TankDrivetrain& drivetrain, std::function angleFunction, PID rotationPID, Angle target, double idleSpeed = 0.0, RotationOptimizer rotationOptimizer = RotationOptimizer::none) : drivetrain(drivetrain), rotationPID(rotationPID), angleFunction(std::move(angleFunction)), Behavior(name), rotationOptimizer(rotationOptimizer) { rotationPID.setTarget(target.Convert(radian)); this->idleSpeed = idleSpeed; this->target = target; @@ -31,6 +39,31 @@ namespace Pronounce void initialize() { + Angle currentPosition = angleFunction(); + + switch (rotationOptimizer) { + case closest: + target = currentPosition + angleDifference(target.getValue(), currentPosition.getValue()) * 1_rad; + break; + case clockwise: + target = angleDifference(target.getValue(), currentPosition.getValue()) * 1_rad; + if (target.Convert(radian) < 0.0) { + target += 360_deg; + } + target += currentPosition; + break; + case counterclockwise: + target = angleDifference(target.getValue(), currentPosition.getValue()) * 1_rad; + if (target.Convert(radian) > 0.0) { + target -= 360_deg; + } + target += currentPosition; + break; + case none: + default: + break; + } + rotationPID.reset(); rotationPID.setTarget(target.Convert(radian)); diff --git a/include/stateMachine/behaviors/catapult/initCatapult.hpp b/include/stateMachine/behaviors/catapult/initCatapult.hpp index 4e06b489..a481ce19 100644 --- a/include/stateMachine/behaviors/catapult/initCatapult.hpp +++ b/include/stateMachine/behaviors/catapult/initCatapult.hpp @@ -8,6 +8,7 @@ namespace Pronounce { PID cataPID(9.0, 0.0, 12.0, 0.0); auto catapultFire = std::make_shared("CatapultFire", catapultMotors, 1.0); auto catapultHold = std::make_shared("CatapultHold", catapultMotors, 1.15714285714, &cataPID); + auto catapultHoldHigh = std::make_shared("CatapultHold", catapultMotors, 0.7, &cataPID); auto catapultStateController = std::make_shared("CatapultStateController", catapultHold); diff --git a/include/stateMachine/state/competition/teleop.hpp b/include/stateMachine/state/competition/teleop.hpp index d051d775..ef98a66b 100644 --- a/include/stateMachine/state/competition/teleop.hpp +++ b/include/stateMachine/state/competition/teleop.hpp @@ -64,6 +64,17 @@ namespace Pronounce { })); }); + controller1.onPressed(E_CONTROLLER_DIGITAL_Y, [&]() -> void { + if (controller1.get_digital(E_CONTROLLER_DIGITAL_RIGHT)) { + winchStateController->sb(winchUp); + } + }); + + controller1.onPressed(E_CONTROLLER_DIGITAL_RIGHT, [&]() -> void { + if (controller1.get_digital(E_CONTROLLER_DIGITAL_Y)) { + winchStateController->sb(winchUp); + } + }); Enabled::initialize(); } diff --git a/src/main.cpp b/src/main.cpp index 20a1e23d..42e7b0a4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,10 +23,10 @@ SMOOTH_SPLINE_PATH_ASSET(skills_10); SMOOTH_SPLINE_PATH_ASSET(close_mid_rush_elim); SMOOTH_SPLINE_PATH_ASSET(close_rush_mid_2); -void turnTo(Angle angle, QTime waitTimeMS, double idleSpeed = 0.0) { +void turnTo(Angle angle, QTime waitTimeMS, RotationOptimizer rotationOptimizer = none, double idleSpeed = 0.0) { auto angleRotation = std::make_shared("AngleTurn", drivetrain, [&]() -> Angle { return imuOrientation.getAngle(); }, - turningPid, angle, idleSpeed); + turningPid, angle, idleSpeed, rotationOptimizer); drivetrainStateController->sb(angleRotation); @@ -94,6 +94,10 @@ void far6Ball(void* args) { drivetrainStateController->sb(pathFollower)->wait(); } +void far6BallAWP(void* args) { + far6Ball(args); +} + void skills(void *args) { imuOrientation.setRotation(135_deg); @@ -123,9 +127,9 @@ void skills(void *args) { frontRightWingStateController->sb(frontRightWingIn); frontLeftWingStateController->sb(frontLeftWingIn); - turnTo(90_deg, 300_ms, 6000); + turnTo(-15.0_deg, 300_ms); - turnTo(-15.0_deg, 600_ms); + catapultStateController->sb(catapultHoldHigh); pathFollower->setMotionProfile(skills_3); drivetrainStateController->sb(pathFollower)->wait(); @@ -172,7 +176,7 @@ void skills(void *args) { drivetrainStateController->sb(pathFollower)->wait(); drivetrainStateController->sb( - std::make_shared("MatchloadRotationController", drivetrain, [&]() -> auto { return imuOrientation.getAngle(); }, turningPid, 0_deg)); + std::make_shared("MatchloadRotationController", drivetrain, [&]() -> auto { return imuOrientation.getAngle(); }, turningPid, 0_deg, 0.0, closest)); QLength wallDistance = getDistanceSensorMedian(wallDistanceSensor, 3, (70_in).Convert(millimetre)) * 1_mm; @@ -193,7 +197,7 @@ void skills(void *args) { turningPid.setTurnPid(true); movingTurnPid.setTurnPid(true); - turnTo(135_deg, 0.4_s); + turnTo(135_deg, 0.4_s, closest); pathFollower->setMotionProfile(skills_8); drivetrainStateController->sb(pathFollower)->wait(); @@ -387,7 +391,7 @@ void initialize() { #if AUTON == 0 auton->setAuton(far6Ball); #elif AUTON == 1 - auton->setAuton(far5BallAWP); + auton->setAuton(far6BallAWP); #elif AUTON == 2 auton->setAuton(safeCloseAWP); #elif AUTON == 3 diff --git a/static/close_mid_rush.json b/static/close_mid_rush.json index 316894da..96562e19 100644 --- a/static/close_mid_rush.json +++ b/static/close_mid_rush.json @@ -1,106 +1,6 @@ { "startSpeed": 0.0, "endSpeed": 0.0, - "segments": [ - { - "inverted": false, - "stopEnd": false, - "paths": [ - { - "x": 2.646, - "y": 1.199 - }, - { - "x": 2.492, - "y": 1.434 - }, - { - "x": 2.231, - "y": 1.574 - }, - { - "x": 1.929, - "y": 1.661 - } - ], - "constraints": { - "velocity": 76, - "accel": 120 - } - }, - { - "inverted": true, - "stopEnd": false, - "paths": [ - { - "x": 1.929, - "y": 1.661 - }, - { - "x": 2.378, - "y": 1.429 - }, - { - "x": 2.244, - "y": 0.491 - }, - { - "x": 2.144, - "y": 0.044 - } - ], - "constraints": { - "velocity": 76, - "accel": 120 - } - }, - { - "inverted": false, - "stopEnd": false, - "paths": [ - { - "x": 2.782, - "y": 0.318 - }, - { - "x": 3.031, - "y": 0.313 - }, - { - "x": 3.401, - "y": 0.422 - }, - { - "x": 3.309, - "y": 0.7 - } - ], - "constraints": { - "velocity": 76, - "accel": 120 - } - } - ], - "commands": [ - { - "t": 0.0, - "name": "intake" - }, - { - "t": 2.0, - "name": "rightWingOut" - }, - { - "t": 2.0, - "name": "awpOut" - }, - { - "t": 2.06, - "name": "rightWingIn" - }, - { - "t": 2.94, - "name": "awpIn" - } - ] + "segments": [], + "commands": [] } \ No newline at end of file diff --git a/static/close_rush_mid_2.json b/static/close_rush_mid_2.json index 76a12118..d1462d73 100644 --- a/static/close_rush_mid_2.json +++ b/static/close_rush_mid_2.json @@ -1 +1,42 @@ -{"segments":[{"inverted":false,"stopEnd":false,"paths":[{"x":3.016,"y":0.45},{"x":3.247,"y":0.555},{"x":3.429,"y":1.221},{"x":3.43,"y":1.485}],"constraints":{"velocity":35,"accel":120}}],"commands":[{"t":0.0,"name":"leftWingOut"},{"t":0.77,"name":"outtake"}]} \ No newline at end of file +{ + "startSpeed": 0.0, + "endSpeed": 0.0, + "segments": [ + { + "inverted": false, + "stopEnd": false, + "paths": [ + { + "x": 3.016, + "y": 0.45 + }, + { + "x": 3.247, + "y": 0.555 + }, + { + "x": 3.429, + "y": 1.221 + }, + { + "x": 3.43, + "y": 1.485 + } + ], + "constraints": { + "velocity": 35, + "accel": 120 + } + } + ], + "commands": [ + { + "t": 0.0, + "name": "leftWingOut" + }, + { + "t": 0.77, + "name": "outtake" + } + ] +} \ No newline at end of file diff --git a/static/skills_3.json b/static/skills_3.json index f1ff0ff9..37e6fd77 100644 --- a/static/skills_3.json +++ b/static/skills_3.json @@ -67,12 +67,12 @@ "y": 3.356 }, { - "x": 3.161, - "y": 3.633 + "x": 3.182, + "y": 3.641 }, { - "x": 2.286, - "y": 3.468 + "x": 2.282, + "y": 3.539 } ], "constraints": { diff --git a/static/skills_5.json b/static/skills_5.json index 9445cfd2..2fc3d1b4 100644 --- a/static/skills_5.json +++ b/static/skills_5.json @@ -15,12 +15,12 @@ "y": 3.331 }, { - "x": 2.835, - "y": 2.338 + "x": 2.846, + "y": 2.239 }, { - "x": 2.448, - "y": 2.29 + "x": 2.459, + "y": 2.191 } ], "constraints": { @@ -33,12 +33,12 @@ "stopEnd": false, "paths": [ { - "x": 2.448, - "y": 2.293 + "x": 2.459, + "y": 2.191 }, { - "x": 2.155, - "y": 2.238 + "x": 2.166, + "y": 2.136 }, { "x": 1.992, @@ -65,7 +65,7 @@ "name": "frontLeftWingOut" }, { - "t": 1.15, + "t": 1.13, "name": "frontLeftWingIn" } ] diff --git a/static/skills_7.json b/static/skills_7.json index ac3104c2..594039b0 100644 --- a/static/skills_7.json +++ b/static/skills_7.json @@ -63,12 +63,12 @@ "y": 2.29 }, { - "x": 2.508, - "y": 2.109 + "x": 2.493, + "y": 2.06 }, { - "x": 1.664, - "y": 2.108 + "x": 1.666, + "y": 2.067 }, { "x": 1.712, diff --git a/static/skills_7_5.json b/static/skills_7_5.json index 0d4e071d..69fd793c 100644 --- a/static/skills_7_5.json +++ b/static/skills_7_5.json @@ -15,12 +15,12 @@ "y": 2.656 }, { - "x": 1.167, - "y": 2.279 + "x": 1.192, + "y": 2.403 }, { - "x": 1.193, - "y": 1.842 + "x": 1.196, + "y": 2.027 } ], "constraints": { @@ -33,20 +33,20 @@ "stopEnd": false, "paths": [ { - "x": 1.193, - "y": 1.842 + "x": 1.199, + "y": 2.021 }, { - "x": 1.205, - "y": 1.504 + "x": 1.204, + "y": 1.681 }, { "x": 1.195, - "y": 1.224 + "y": 1.355 }, { "x": 1.207, - "y": 0.921 + "y": 1.052 } ], "constraints": { diff --git a/static/skills_8.json b/static/skills_8.json index babebd36..4aefe9fb 100644 --- a/static/skills_8.json +++ b/static/skills_8.json @@ -11,16 +11,16 @@ "y": 0.195 }, { - "x": 0.518, - "y": 0.359 + "x": 0.567, + "y": 0.459 }, { - "x": 0.391, - "y": 0.554 + "x": 0.475, + "y": 0.587 }, { - "x": 0.286, - "y": 0.936 + "x": 0.37, + "y": 0.969 } ], "constraints": { @@ -37,15 +37,15 @@ "y": 0.936 }, { - "x": 0.169, - "y": 1.399 + "x": 0.178, + "y": 1.404 }, { - "x": 0.176, - "y": 1.754 + "x": 0.14, + "y": 1.75 }, { - "x": 0.199, + "x": 0.116, "y": 2.349 } ], @@ -59,12 +59,12 @@ "stopEnd": false, "paths": [ { - "x": 0.203, + "x": 0.191, "y": 2.353 }, { - "x": 0.233, - "y": 2.843 + "x": 0.173, + "y": 2.886 }, { "x": 0.656, @@ -91,15 +91,15 @@ "name": "backRightWingOut" }, { - "t": 0.56, + "t": 0.81, "name": "backRightWingIn" }, { - "t": 2.1, + "t": 2.01, "name": "backRightWingOut" }, { - "t": 2.31, + "t": 2.4, "name": "backRightWingIn" } ] diff --git a/uploadAllAutons.py b/uploadAllAutons.py index 70d8cab0..09c9029d 100755 --- a/uploadAllAutons.py +++ b/uploadAllAutons.py @@ -17,24 +17,35 @@ "SKILLS"] offset = 1 +def getFilePath(): + return os.path.dirname(__file__) if len(sys.argv) > 1: i = int(sys.argv[1]) - f = open("include/auton.h", "w") + f = open(getFilePath() + "/include/auton.h", "w") f.write("#define AUTON " + str(i) + "\n") f.close() + command = f"pros mu --project {getFilePath().replace(' ', '\\ ')} --slot " + str(i+offset) + " --name \"" + autons[i] + "\"" + print (command) - time.sleep(1) + result = os.system(command) - print ("pros mu --slot " + str(i+offset) + " --name \"" + autons[i] + "\"") + if result != 0: + os._exit(2) - os.system("pros mu --slot " + str(i+offset) + " --name \"" + autons[i] + "\"") else: for i in range(len(autons)): - f = open("include/auton.h", "w") + f = open(getFilePath() + "/include/auton.h", "w") f.write("#define AUTON " + str(i) + "\n") f.close() - print ("pros mu --slot " + str(i+offset) + " --name \"" + autons[i] + "\"") + command = f"pros mu --project {getFilePath().replace(" ", "\\ ")} --slot " + str(i+offset) + " --name \"" + autons[i] + "\"" + print(command) + + result = os.system(command) + + if result != 0: + os._exit(2) + + os._exit(0) - os.system("pros mu --slot " + str(i+offset) + " --name \"" + autons[i] + "\"")