Merge pull request #246 from alexDickhans/skills4-6

Skills + 6++ ball

include/auton.h

@@ -1 +1 @@
-#define AUTON 5
+#define AUTON 5

include/pathPlanner/pathFollower.hpp

@@ -87,7 +87,7 @@ namespace PathPlanner {
 			std::pair<double, double> driveVoltages = {feedforward(driveSpeeds.first, driveAccel.first), feedforward(driveSpeeds.second, driveAccel.second)};
 			distancePID.setTarget(target.targetDistance.getValue());
 
-			double distanceOutput = distancePID.update((drivetrain.getDistanceSinceReset() - startDistance).Convert(metre));
+			double distanceOutput = std::clamp(distancePID.update((drivetrain.getDistanceSinceReset() - startDistance).Convert(metre)), -8000.0, 8000.0);
 			driveVoltages = {driveVoltages.first + distanceOutput, driveVoltages.second + distanceOutput};
 
 			turnPID.setTarget((target.targetPose.getAngle() + ((driveSpeeds.first + driveSpeeds.second).getValue() > 0.0 ? 0.0 : 180_deg)).Convert(radian));

include/stateMachine/state/competition/auton.hpp

@@ -11,11 +11,7 @@ namespace Pronounce {
 			intakeStateController->sb(intakeIntaking);
 		});
 
-		pathFollower->addCommandMapping("intakeHold", [&]() -> void {
-			intakeStateController->sb(intakeHold);
-		});
-
-		pathFollower->addCommandMapping("intakeStopped", [&]() -> void {
+		pathFollower->addCommandMapping("intakeStop", [&]() -> void {
 			intakeStateController->ud();
 		});
 

src/main.cpp

@@ -5,10 +5,11 @@ std::shared_ptr<lv_obj_t> tabview;
 
 // SECTION Auton
 SMOOTH_SPLINE_PATH_ASSET(close_mid_rush);
-SMOOTH_SPLINE_PATH_ASSET(mid_6_ball_1);
-SMOOTH_SPLINE_PATH_ASSET(mid_6_ball_2);
-SMOOTH_SPLINE_PATH_ASSET(mid_6_ball_awp);
 SMOOTH_SPLINE_PATH_ASSET(safe_close_awp);
+SMOOTH_SPLINE_PATH_ASSET(far_6_1);
+SMOOTH_SPLINE_PATH_ASSET(far_6_2);
+SMOOTH_SPLINE_PATH_ASSET(far_6_3);
+SMOOTH_SPLINE_PATH_ASSET(far_6_4);
 SMOOTH_SPLINE_PATH_ASSET(skills_1);
 SMOOTH_SPLINE_PATH_ASSET(skills_2);
 SMOOTH_SPLINE_PATH_ASSET(skills_3);
@@ -57,89 +58,42 @@ void move(QLength distance, ProfileConstraints profileConstraints, QCurvature cu
 			                                      endSpeed))->wait();
 }
 
-void far5BallRushMid(void *args) {
+void far6Ball(void* args) {
+	imuOrientation.setRotation(-120.5_deg);
 
-	Log("5 ball start");
-
-	imuOrientation.setRotation(80.7_deg);
-
-	intakeExtensionStateController->sb(deploySequence);
-	frontRightWingStateController->sb(std::make_shared<Wait>(frontRightWingOut, 200_ms));
-
-	move(50_in, speedProfileConstraints, 0.0, 80.7_deg);
-
-	intakeExtensionStateController->ud();
 	intakeStateController->sb(intakeIntaking);
 
-	pathFollower->setMotionProfile(PathPlanner::SmoothSplineProfile::build(mid_6_ball_1_json));
-	drivetrainStateController->sb(pathFollower)->wait();
+	move(53_in, speedProfileConstraints, 0.0, -120.5_deg);
 
-	turnTo(-2_deg, 550_ms);
+	pathFollower->setMotionProfile(far_6_1);
+	drivetrainStateController->sb(pathFollower)->wait();
 
+	turnTo(-180_deg, 0.4_s);
 	intakeStateController->sb(intakeIntaking);
+	move(5_in, speedProfileConstraints, 0.0, -180_deg);
 
-	move(19_in, speedProfileConstraints, 0.0, 2_deg, 0.0, 0.0);
-	move(-16_in, speedProfileConstraints, 0.0, 2_deg, 0.0, 0.0);
-
-	intakeStateController->sb(intakeHold);
-//	move(-4_in, speedProfileConstraints, 0.0, 2_deg);
-	turnTo(170_deg, 700_ms);
-
-	pathFollower->setMotionProfile(PathPlanner::SmoothSplineProfile::build(mid_6_ball_2_json));
+	pathFollower->setMotionProfile(far_6_2);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(-12_in, speedProfileConstraints, 0.0, 110_deg);
+	backRightWingStateController->ud();
+	move(15_in, speedProfileConstraints, 0.0, -250_deg);
+	turnTo(-430_deg, 0.6_s);
 
-	frontLeftWingStateController->ud();
-	turnTo(120_deg, 300_ms);
 	frontLeftWingStateController->sb(frontLeftWingOut);
-	drivetrain.tankSteerVoltage(12000, 12000);
-	pros::Task::delay(800);
-	drivetrain.tankSteerVoltage(0.0, 0.0);
-	frontLeftWingStateController->ud();
-	move(-9_in, speedProfileConstraints, 0.0, 90_deg);
-	turnTo(25_deg, 200_ms);
-	intakeStateController->sb(intakeIntaking);
-	move(48_in, speedProfileConstraints, 0.0, 25_deg);
-
-	turnTo(150_deg, 550_ms);
-	intakeExtensionStateController->sb(outtakeSequence);
-	move(38_in, speedProfileConstraints, 0.0, 150_deg);
-}
-
-void far6BallRushMid(void *args) {
-	Log("6 ball");
-
-	far5BallRushMid(args);
-
-	turnTo(3_deg, 550_ms);
 
-	intakeStateController->sb(intakeIntaking);
-
-	move(23_in, defaultProfileConstraints, 0.0, 3_deg);
-
-	turnTo(180_deg, 550_ms);
-	intakeExtensionStateController->ud();
 	intakeStateController->sb(intakeEject);
-	frontLeftWingStateController->sb(frontLeftWingOut);
-	frontRightWingStateController->sb(frontRightWingOut);
-	move(35_in, speedProfileConstraints, 0.0, 180_deg);
-	move(-10_in, speedProfileConstraints, 0.0, 180_deg);
-	turnTo(0_deg, 3_s);
-}
 
-void far5BallAWP(void *args) {
-	far5BallRushMid(args);
+	pathFollower->setMotionProfile(far_6_3);
+	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(-5_in, speedProfileConstraints, 0.0, 0_deg);
+	move(-15_in, defaultProfileConstraints, 0.0, -430_deg);
 
-	turnTo(-90_deg, 600_ms);
+	frontLeftWingStateController->ud();
 
-	pathFollower->setMotionProfile(PathPlanner::SmoothSplineProfile::build(mid_6_ball_awp_json));
+	pathFollower->setMotionProfile(far_6_3);
+	drivetrainStateController->sb(pathFollower)->wait();
+	pathFollower->setMotionProfile(far_6_4);
 	drivetrainStateController->sb(pathFollower)->wait();
-
-	drivetrain.tankSteerVoltage(3000, 2000);
-	pros::Task::delay(5000);
 }
 
 void skills(void *args) {
@@ -151,10 +105,11 @@ void skills(void *args) {
 
 	drivetrainStateController->sb(
 			std::make_shared<RotationController>("MatchloadRotationController", drivetrain, [&]() -> auto { return imuOrientation.getAngle(); }, turningPid,
-			                                     21.1_deg, -800.0));
+			                                     20.7_deg, -800.0));
 	auton->resetTriballs();
-	pros::Task::delay(1000);
+	pros::Task::delay(100);
 	backLeftWingStateController->sb(backLeftWingOut);
+	pros::Task::delay(900);
 
 	// Wait until the catapult triballs shot has increased to 44 triballs
 	while (auton->getTriballCount() < 44 && catapultStateController->getDuration() < 2.0_s) {
@@ -172,51 +127,47 @@ void skills(void *args) {
 
 	turnTo(90_deg, 300_ms, 6000);
 
-	turnTo(0.0, 500_ms);
+	turnTo(0.0, 600_ms);
 
-	pathFollower->setMotionProfile(skills_3);
-	drivetrainStateController->sb(pathFollower)->wait();
+	drivetrainStateController->sb(std::make_shared<Wait>(std::make_shared<VoltageDrivetrain>(-12000, -12000, drivetrain), 1.5_s))->wait();
 
-	move(15_in, speedProfileConstraints, 0.0, 110_deg);
+	move(20_in, speedProfileConstraints, 0.0, 110_deg);
 
-	pathFollower->setMotionProfile(skills_4);
+	drivetrainStateController->sb(std::make_shared<Wait>(std::make_shared<VoltageDrivetrain>(-12000, -12000, drivetrain), 1.5_s))->wait();
 
-	drivetrainStateController->sb(pathFollower)->wait();
-
-	move(15_in, speedProfileConstraints, 0.0, 110_deg);
+	move(20_in, speedProfileConstraints, 0.0, 110_deg);
 	backRightWingStateController->ud();
 	backLeftWingStateController->ud();
 
-	pathFollower->setMotionProfile(skills_4);
-	drivetrainStateController->sb(pathFollower)->wait();
+	drivetrainStateController->sb(std::make_shared<Wait>(std::make_shared<VoltageDrivetrain>(-12000, -12000, drivetrain), 1.5_s))->wait();
 
 	pathFollower->setMotionProfile(skills_5);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(-5_in, speedProfileConstraints, 0.0);
+	move(-8_in, speedProfileConstraints, 0.0);
 
-	move(8_in, speedProfileConstraints, 0.0, 0.0_deg);
+	move(12_in, speedProfileConstraints, 0.0, 0.0_deg);
 
 	pathFollower->setMotionProfile(skills_6);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(-3_in, speedProfileConstraints, 0.0);
+	move(-8_in, speedProfileConstraints, 0.0);
 
-	move(8_in, speedProfileConstraints, 0.0, 0.0_deg);
+	move(12_in, speedProfileConstraints, 0.0, 0.0_deg);
 
 	pathFollower->setMotionProfile(skills_6_5);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(-3_in, speedProfileConstraints, 0.0);
+	move(-8_in, speedProfileConstraints, 0.0);
 
-	move(8_in, speedProfileConstraints, 0.0, 0.0_deg);
+	move(12_in, speedProfileConstraints, 0.0, 0.0_deg);
 
 	pathFollower->setMotionProfile(skills_7);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(-3_in, speedProfileConstraints, 0.0);
+	move(-8_in, speedProfileConstraints, 0.0);
 
-	move(8_in, speedProfileConstraints, 0.0, 0.0_deg);
+	move(12_in, speedProfileConstraints, 0.0, 0.0_deg);
 
 	pathFollower->setMotionProfile(skills_7_5);
 	drivetrainStateController->sb(pathFollower)->wait();
@@ -226,13 +177,15 @@ void skills(void *args) {
 
 	QLength wallDistance = getDistanceSensorMedian(wallDistanceSensor, 3) * 1_mm;
 
+	turnTo(385_deg, 300_ms);
+
 	pathFollower->setMotionProfile(
 			PathPlanner::SmoothSplineProfile::build(
 					{PathPlanner::BezierSegment(PathPlanner::Point(
 							                                  wallDistance, 76_in),
 					                                  PathPlanner::Point(
-							                                  wallDistance.getValue() * 0.74,
-							                                  68_in),
+							                                  wallDistance.getValue() * 0.60,
+							                                  70_in),
 					                                  PathPlanner::Point(
 							                                  19_in, 55_in),
 					                                  PathPlanner::Point(
@@ -241,22 +194,22 @@ void skills(void *args) {
 
 	drivetrainStateController->sb(pathFollower)->wait();
 
+	turnTo(490_deg, 0.4_s);
+
 	pathFollower->setMotionProfile(skills_8);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(-15_in, speedProfileConstraints, 0.0, 70_deg);
+	move(15_in, speedProfileConstraints, 0.0, 610_deg);
 
 	pathFollower->setMotionProfile(skills_9);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(-15_in, speedProfileConstraints, 0.0, 80_deg);
+	move(15_in, speedProfileConstraints, 0.0, 610_deg);
 
 	pathFollower->setMotionProfile(skills_9);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	frontRightWingStateController->ud();
-
-	move(-15_in, speedProfileConstraints, 0.0, 80_deg);
+	backLeftWingStateController->ud();
 
 	pathFollower->setMotionProfile(skills_10);
 	drivetrainStateController->sb(pathFollower)->wait();
@@ -439,7 +392,7 @@ void initialize() {
 	pros::Task display(updateDisplay, TASK_PRIORITY_MIN + 1, TASK_STACK_DEPTH_DEFAULT, "updateDisplay");
 
 #if AUTON == 0
-	auton->setAuton(far6BallRushMid);
+	auton->setAuton(far6Ball);
 #elif AUTON == 1
 	auton->setAuton(far5BallAWP);
 #elif AUTON == 2

static/far_6_1.json

@@ -0,0 +1 @@
+{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":1.958,"y":2.003},{"x":2.086,"y":2.052},{"x":2.181,"y":2.019},{"x":2.181,"y":1.908}],"constraints":{"velocity":74,"accel":125}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.159,"y":2.048},{"x":2.156,"y":2.26},{"x":2.041,"y":2.34},{"x":1.82,"y":2.338}],"constraints":{"velocity":74,"accel":133}},{"inverted":true,"stopEnd":false,"paths":[{"x":1.881,"y":2.418},{"x":1.972,"y":2.371},{"x":1.989,"y":2.303},{"x":1.991,"y":2.192}],"constraints":{"velocity":74,"accel":163}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.108,"y":2.186},{"x":2.199,"y":2.354},{"x":2.331,"y":2.15},{"x":2.257,"y":1.918}],"constraints":{"velocity":74,"accel":168}},{"inverted":true,"stopEnd":false,"paths":[{"x":2.403,"y":2.036},{"x":2.392,"y":2.22},{"x":2.161,"y":2.365},{"x":2.12,"y":2.142}],"constraints":{"velocity":74,"accel":168}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.12,"y":2.142},{"x":2.083,"y":2.503},{"x":2.051,"y":2.766},{"x":2.033,"y":3.003}],"constraints":{"velocity":74,"accel":164}},{"inverted":true,"stopEnd":false,"paths":[{"x":1.625,"y":2.998},{"x":1.638,"y":2.671},{"x":2.678,"y":2.289},{"x":3.567,"y":2.664}],"constraints":{"velocity":55,"accel":132}}],"commands":[{"t":0.75,"name":"outtake"},{"t":1.65,"name":"intake"},{"t":2.24,"name":"outtake"},{"t":3.6,"name":"intake"},{"t":4.84,"name":"outtake"},{"t":5.01,"name":"frontRightWingOut"},{"t":6.02,"name":"frontRightWingIn"},{"t":6.71,"name":"intakeStop"}]}

static/far_6_2.json

@@ -0,0 +1 @@
+{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":3.349,"y":2.118},{"x":3.534,"y":2.985},{"x":3.202,"y":3.455},{"x":2.197,"y":3.214}],"constraints":{"velocity":76,"accel":200}}],"commands":[{"t":0.18,"name":"backRightWingOut"},{"t":0.36,"name":"backLeftWingOut"},{"t":0.6,"name":"backLeftWingIn"}]}

static/far_6_3.json

@@ -0,0 +1 @@
+{"segments":[{"inverted":false,"stopEnd":false,"paths":[{"x":2.917,"y":3.215},{"x":2.8,"y":3.395},{"x":2.673,"y":3.469},{"x":2.429,"y":3.473}],"constraints":{"velocity":76,"accel":200}}],"commands":[]}

static/far_6_4.json

@@ -0,0 +1 @@
+{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":2.466,"y":3.321},{"x":2.681,"y":3.325},{"x":2.967,"y":2.942},{"x":3.245,"y":3.235}],"constraints":{"velocity":76,"accel":200}}],"commands":[]}

static/skills_10.json

@@ -1 +1 @@
-{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":1.06,"y":3.316},{"x":0.986,"y":3.319},{"x":0.934,"y":3.351},{"x":0.936,"y":3.463}],"constraints":{"velocity":71,"accel":165}},{"inverted":false,"stopEnd":false,"paths":[{"x":0.936,"y":3.463},{"x":0.828,"y":3.173},{"x":0.374,"y":3.147},{"x":0.359,"y":2.41}],"constraints":{"velocity":71,"accel":165}},{"inverted":false,"stopEnd":false,"paths":[{"x":0.359,"y":2.41},{"x":0.362,"y":2.215},{"x":0.36,"y":1.933},{"x":0.366,"y":1.736}],"constraints":{"velocity":48,"accel":165}}],"commands":[{"t":0.0,"name":"hangOut"},{"t":2.96,"name":"hangIn"}]}
+{"segments":[{"inverted":false,"stopEnd":false,"paths":[{"x":1.182,"y":3.349},{"x":0.797,"y":3.351},{"x":0.374,"y":3.147},{"x":0.359,"y":2.41}],"constraints":{"velocity":71,"accel":165}},{"inverted":false,"stopEnd":false,"paths":[{"x":0.359,"y":2.41},{"x":0.362,"y":2.215},{"x":0.36,"y":1.933},{"x":0.366,"y":1.736}],"constraints":{"velocity":48,"accel":165}}],"commands":[{"t":0.0,"name":"hangOut"},{"t":1.91,"name":"hangIn"}]}

static/skills_2.json

@@ -1 +1 @@
-{"segments":[{"inverted":false,"stopEnd":false,"paths":[{"x":0.589,"y":0.607},{"x":0.68,"y":0.881},{"x":0.712,"y":1.006},{"x":0.862,"y":1.269}],"constraints":{"velocity":71,"accel":166}},{"inverted":false,"stopEnd":false,"paths":[{"x":0.861,"y":1.271},{"x":1.039,"y":1.677},{"x":2.537,"y":1.482},{"x":2.929,"y":1.388}],"constraints":{"velocity":71,"accel":166}}],"commands":[{"t":0.0,"name":"backLeftWingIn"},{"t":0.69,"name":"frontRightWingOut"},{"t":0.69,"name":"frontLeftWingOut"}]}
+{"segments":[{"inverted":false,"stopEnd":false,"paths":[{"x":0.589,"y":0.607},{"x":0.639,"y":0.85},{"x":0.667,"y":0.952},{"x":0.763,"y":1.182}],"constraints":{"velocity":71,"accel":166}},{"inverted":false,"stopEnd":false,"paths":[{"x":0.767,"y":1.185},{"x":0.982,"y":1.674},{"x":2.363,"y":1.449},{"x":2.945,"y":1.34}],"constraints":{"velocity":71,"accel":166}}],"commands":[{"t":0.0,"name":"backLeftWingIn"},{"t":0.69,"name":"frontRightWingOut"},{"t":0.69,"name":"frontLeftWingOut"},{"t":1.75,"name":"frontLeftWingIn"}]}

static/skills_3.json

@@ -1 +1 @@
-{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":2.692,"y":1.441},{"x":2.765,"y":0.992},{"x":2.782,"y":0.363},{"x":3.005,"y":0.346}],"constraints":{"velocity":71,"accel":140}},{"inverted":true,"stopEnd":false,"paths":[{"x":3.006,"y":0.347},{"x":3.357,"y":0.311},{"x":3.418,"y":1.068},{"x":3.467,"y":1.546}],"constraints":{"velocity":60,"accel":148}},{"inverted":true,"stopEnd":false,"paths":[{"x":3.467,"y":1.546},{"x":3.656,"y":3.289},{"x":3.139,"y":3.614},{"x":2.142,"y":3.422}],"constraints":{"velocity":47,"accel":185}}],"commands":[{"t":0.0,"name":"backRightWingOut"},{"t":0.0,"name":"frontWingsIn"},{"t":0.91,"name":"backLeftWingOut"},{"t":1.38,"name":"backLeftWingIn"},{"t":2.27,"name":"backLeftWingOut"},{"t":2.5,"name":"backLeftWingIn"}]}
+{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":2.68,"y":1.441},{"x":2.753,"y":0.992},{"x":2.778,"y":0.492},{"x":3.001,"y":0.475}],"constraints":{"velocity":71,"accel":140}},{"inverted":true,"stopEnd":false,"paths":[{"x":2.999,"y":0.476},{"x":3.35,"y":0.44},{"x":3.383,"y":0.986},{"x":3.408,"y":1.548}],"constraints":{"velocity":60,"accel":148}},{"inverted":true,"stopEnd":false,"paths":[{"x":3.41,"y":1.548},{"x":3.502,"y":3.356},{"x":3.137,"y":3.663},{"x":2.14,"y":3.471}],"constraints":{"velocity":47,"accel":185}}],"commands":[{"t":0.0,"name":"backRightWingOut"},{"t":0.0,"name":"frontWingsIn"},{"t":0.91,"name":"backLeftWingOut"},{"t":1.33,"name":"backLeftWingIn"},{"t":2.13,"name":"backLeftWingOut"},{"t":2.56,"name":"backLeftWingIn"}]}

static/skills_4.json

@@ -1 +1 @@
-{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":3.133,"y":3.135},{"x":2.961,"y":3.313},{"x":2.814,"y":3.39},{"x":2.526,"y":3.395}],"constraints":{"velocity":71,"accel":203}}],"commands":[{"t":0.0,"name":"outtake"}]}
+{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":3.185,"y":3.097},{"x":3.013,"y":3.275},{"x":2.814,"y":3.39},{"x":2.526,"y":3.395}],"constraints":{"velocity":71,"accel":203}}],"commands":[{"t":0.0,"name":"outtake"}]}

static/skills_5.json

@@ -1 +1 @@
-{"segments":[{"inverted":false,"stopEnd":false,"paths":[{"x":2.628,"y":3.332},{"x":2.854,"y":3.177},{"x":2.715,"y":2.28},{"x":2.466,"y":2.449}],"constraints":{"velocity":38,"accel":141}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.466,"y":2.449},{"x":2.334,"y":2.568},{"x":2.379,"y":2.89},{"x":2.371,"y":3.3}],"constraints":{"velocity":25,"accel":141}}],"commands":[{"t":0.64,"name":"frontLeftWingOut"},{"t":0.75,"name":"frontRightWingOut"}]}
+{"segments":[{"inverted":false,"stopEnd":false,"paths":[{"x":2.628,"y":3.332},{"x":2.914,"y":3.331},{"x":2.835,"y":2.338},{"x":2.448,"y":2.29}],"constraints":{"velocity":41,"accel":161}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.448,"y":2.293},{"x":2.155,"y":2.238},{"x":1.962,"y":2.768},{"x":1.952,"y":3.274}],"constraints":{"velocity":33,"accel":161}}],"commands":[{"t":0.5,"name":"frontRightWingOut"},{"t":0.63,"name":"frontLeftWingOut"},{"t":1.04,"name":"frontLeftWingIn"}]}