1000 matchloads a day keeps the head ref away

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 = std::clamp(distancePID.update((drivetrain.getDistanceSinceReset() - startDistance).Convert(metre)), -12000.0, 12000.0);
+			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));

src/main.cpp

@@ -105,7 +105,7 @@ void skills(void *args) {
 
 	drivetrainStateController->sb(
 			std::make_shared<RotationController>("MatchloadRotationController", drivetrain, [&]() -> auto { return imuOrientation.getAngle(); }, turningPid,
-			                                     21.8_deg, -800.0));
+			                                     20.7_deg, -800.0));
 	auton->resetTriballs();
 	pros::Task::delay(100);
 	backLeftWingStateController->sb(backLeftWingOut);
@@ -127,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();
-
-	move(15_in, speedProfileConstraints, 0.0, 110_deg);
+	drivetrainStateController->sb(std::make_shared<Wait>(std::make_shared<VoltageDrivetrain>(-12000, -12000, drivetrain), 1.5_s))->wait();
 
-	pathFollower->setMotionProfile(skills_4);
+	move(20_in, speedProfileConstraints, 0.0, 110_deg);
 
-	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);
 	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();
@@ -181,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.66,
-							                                  68_in),
+							                                  wallDistance.getValue() * 0.60,
+							                                  70_in),
 					                                  PathPlanner::Point(
 							                                  19_in, 55_in),
 					                                  PathPlanner::Point(
@@ -196,15 +194,17 @@ 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, 250_deg);
+	move(15_in, speedProfileConstraints, 0.0, 610_deg);
 
 	pathFollower->setMotionProfile(skills_9);
 	drivetrainStateController->sb(pathFollower)->wait();
 
-	move(15_in, speedProfileConstraints, 0.0, 260_deg);
+	move(15_in, speedProfileConstraints, 0.0, 610_deg);
 
 	pathFollower->setMotionProfile(skills_9);
 	drivetrainStateController->sb(pathFollower)->wait();

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.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.925,"y":1.592},{"x":2.374,"y":1.513},{"x":2.951,"y":1.462}],"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.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.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.835,"y":2.338},{"x":2.448,"y":2.29}],"constraints":{"velocity":30,"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":20,"accel":161}}],"commands":[{"t":0.63,"name":"frontLeftWingOut"},{"t":0.63,"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"}]}

static/skills_6.json

@@ -1 +1 @@
-{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":2.166,"y":2.895},{"x":2.167,"y":2.825},{"x":2.111,"y":2.767},{"x":2.059,"y":2.753}],"constraints":{"velocity":71,"accel":167}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.059,"y":2.753},{"x":2.19,"y":2.72},{"x":2.259,"y":2.656},{"x":2.22,"y":2.519}],"constraints":{"velocity":45,"accel":167}},{"inverted":false,"stopEnd":true,"paths":[{"x":2.206,"y":2.32},{"x":2.148,"y":2.16},{"x":1.944,"y":2.15},{"x":1.762,"y":2.185}],"constraints":{"velocity":35,"accel":167}},{"inverted":false,"stopEnd":false,"paths":[{"x":1.765,"y":2.247},{"x":1.778,"y":2.443},{"x":1.787,"y":2.934},{"x":1.789,"y":3.242}],"constraints":{"velocity":35,"accel":167}}],"commands":[{"t":1.86,"name":"frontLeftWingIn"}]}
+{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":2.166,"y":2.895},{"x":2.167,"y":2.825},{"x":2.111,"y":2.767},{"x":2.059,"y":2.753}],"constraints":{"velocity":71,"accel":167}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.059,"y":2.753},{"x":2.19,"y":2.72},{"x":2.249,"y":2.473},{"x":2.21,"y":2.336}],"constraints":{"velocity":45,"accel":167}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.206,"y":2.32},{"x":2.148,"y":2.16},{"x":1.791,"y":2.091},{"x":1.744,"y":2.184}],"constraints":{"velocity":35,"accel":167}},{"inverted":false,"stopEnd":false,"paths":[{"x":1.765,"y":2.247},{"x":1.726,"y":2.426},{"x":1.766,"y":2.934},{"x":1.768,"y":3.242}],"constraints":{"velocity":35,"accel":167}}],"commands":[{"t":1.86,"name":"frontLeftWingIn"}]}

static/skills_6_5.json

@@ -1 +1 @@
-{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":2.166,"y":2.895},{"x":2.167,"y":2.825},{"x":2.111,"y":2.767},{"x":2.059,"y":2.753}],"constraints":{"velocity":71,"accel":165}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.059,"y":2.753},{"x":2.19,"y":2.72},{"x":2.217,"y":2.469},{"x":2.178,"y":2.332}],"constraints":{"velocity":45,"accel":165}},{"inverted":false,"stopEnd":true,"paths":[{"x":2.18,"y":2.33},{"x":2.122,"y":2.17},{"x":1.937,"y":2.209},{"x":1.757,"y":2.304}],"constraints":{"velocity":35,"accel":165}},{"inverted":false,"stopEnd":false,"paths":[{"x":1.76,"y":2.386},{"x":1.81,"y":2.61},{"x":1.861,"y":2.887},{"x":1.921,"y":3.184}],"constraints":{"velocity":35,"accel":165}}],"commands":[{"t":1.03,"name":"frontLeftWingIn"}]}
+{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":2.166,"y":2.895},{"x":2.167,"y":2.825},{"x":2.111,"y":2.767},{"x":2.059,"y":2.753}],"constraints":{"velocity":71,"accel":165}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.059,"y":2.753},{"x":2.19,"y":2.72},{"x":2.217,"y":2.469},{"x":2.178,"y":2.332}],"constraints":{"velocity":45,"accel":165}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.18,"y":2.33},{"x":2.122,"y":2.17},{"x":1.687,"y":2.131},{"x":1.647,"y":2.306}],"constraints":{"velocity":35,"accel":165}},{"inverted":false,"stopEnd":false,"paths":[{"x":1.76,"y":2.386},{"x":1.738,"y":2.624},{"x":1.826,"y":2.89},{"x":1.868,"y":3.187}],"constraints":{"velocity":35,"accel":165}}],"commands":[{"t":1.03,"name":"frontLeftWingIn"}]}

static/skills_7.json

@@ -1 +1 @@
-{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":2.166,"y":2.895},{"x":2.167,"y":2.825},{"x":2.111,"y":2.767},{"x":2.059,"y":2.753}],"constraints":{"velocity":61,"accel":163}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.059,"y":2.753},{"x":2.207,"y":2.707},{"x":2.255,"y":2.566},{"x":2.206,"y":2.423}],"constraints":{"velocity":44,"accel":163}},{"inverted":false,"stopEnd":true,"paths":[{"x":2.473,"y":2.28},{"x":2.41,"y":2.099},{"x":1.843,"y":2.163},{"x":1.714,"y":2.463}],"constraints":{"velocity":35,"accel":163}},{"inverted":false,"stopEnd":false,"paths":[{"x":1.718,"y":2.477},{"x":1.899,"y":2.702},{"x":2.694,"y":2.481},{"x":2.715,"y":2.949}],"constraints":{"velocity":35,"accel":163}}],"commands":[{"t":1.03,"name":"frontLeftWingIn"},{"t":2.52,"name":"frontLeftWingOut"}]}
+{"segments":[{"inverted":true,"stopEnd":false,"paths":[{"x":2.166,"y":2.895},{"x":2.167,"y":2.825},{"x":2.111,"y":2.767},{"x":2.059,"y":2.753}],"constraints":{"velocity":61,"accel":163}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.059,"y":2.753},{"x":2.207,"y":2.707},{"x":2.255,"y":2.566},{"x":2.206,"y":2.423}],"constraints":{"velocity":44,"accel":163}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.571,"y":2.29},{"x":2.508,"y":2.109},{"x":1.687,"y":2.153},{"x":1.712,"y":2.425}],"constraints":{"velocity":35,"accel":163}},{"inverted":false,"stopEnd":false,"paths":[{"x":1.713,"y":2.429},{"x":1.759,"y":2.636},{"x":1.943,"y":2.541},{"x":2.111,"y":2.594}],"constraints":{"velocity":27,"accel":163}},{"inverted":false,"stopEnd":false,"paths":[{"x":2.111,"y":2.594},{"x":2.24,"y":2.659},{"x":2.33,"y":2.731},{"x":2.314,"y":2.852}],"constraints":{"velocity":74,"accel":200}}],"commands":[{"t":1.03,"name":"frontLeftWingIn"},{"t":2.52,"name":"frontLeftWingOut"}]}