Skip to content

Commit

Permalink
Skills and auton testing
Browse files Browse the repository at this point in the history
  • Loading branch information
@alexDickhans
alexDickhans committed Apr 14, 2024
1 parent a355ac2 commit 2321e64
Show file tree
Hide file tree
Showing 13 changed files with 269 additions and 63 deletions.
2 changes: 1 addition & 1 deletion include/auton.h
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
#define AUTON 5
#define AUTON 0
2 changes: 1 addition & 1 deletion include/pathPlanner/pathFollower.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)), -8000.0, 8000.0);
double distanceOutput = std::clamp(distancePID.update((drivetrain.getDistanceSinceReset() - startDistance).Convert(metre)), -12000.0, 12000.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));
Expand Down
2 changes: 1 addition & 1 deletion include/stateMachine/behaviors/catapult/initCatapult.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ namespace Pronounce {
PID cataPID(9.0, 0.0, 12.0, 0.0);
auto catapultFire = std::make_shared<Catapult>("CatapultFire", catapultMotors, 1.0);
auto catapultHold = std::make_shared<CatapultHold>("CatapultHold", catapultMotors, 1.15714285714, &cataPID);
auto catapultHoldHigh = std::make_shared<CatapultHold>("CatapultHold", catapultMotors, 0.7, &cataPID);
auto catapultHoldHigh = std::make_shared<CatapultHold>("CatapultHold", catapultMotors, 0.5, &cataPID);

auto catapultStateController = std::make_shared<StateController>("CatapultStateController", catapultHold);

Expand Down
24 changes: 18 additions & 6 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,9 +59,11 @@ void move(QLength distance, ProfileConstraints profileConstraints, QCurvature cu
void far6Ball(void* args) {
imuOrientation.setRotation(-120.5_deg);

intakeStateController->sb(intakeIntaking);
intakeExtensionStateController->sb(deploySequence);

move(51_in, speedProfileConstraints, 0.0, -120.5_deg);

move(53_in, speedProfileConstraints, 0.0, -120.5_deg);
intakeExtensionStateController->ud();

pathFollower->setMotionProfile(far_6_1);
drivetrainStateController->sb(pathFollower)->wait();
Expand Down Expand Up @@ -176,15 +178,15 @@ void skills(void *args) {

QLength wallDistance = getDistanceSensorMedian(wallDistanceSensor, 3, (70_in).Convert(millimetre)) * 1_mm;

turnTo(45_deg, 0.2_s, closest);
turnTo(55_deg, 0.4_s, closest);

pathFollower->setMotionProfile(
PathPlanner::SmoothSplineProfile::build(
{PathPlanner::BezierSegment(PathPlanner::Point(
wallDistance, 76_in),
PathPlanner::Point(
wallDistance - 10_in,
60_in),
wallDistance - 14_in,
66_in),
PathPlanner::Point(
15_in, 52_in),
PathPlanner::Point(
Expand Down Expand Up @@ -226,6 +228,16 @@ void safeCloseAWP(void *args) {
pros::Task::delay(15000);
}

void safeCloseAWPDelay(void *args) {
imuOrientation.setRotation(-135_deg);

pros::Task::delay(11000);

pathFollower->setMotionProfile(safe_close_awp);
drivetrainStateController->sb(pathFollower)->wait();

}

void closeRushMid(void *args) {
imuOrientation.setRotation(-75.7_deg);

Expand Down Expand Up @@ -389,7 +401,7 @@ void initialize() {
#elif AUTON == 1
auton->setAuton(far6BallAWP);
#elif AUTON == 2
auton->setAuton(safeCloseAWP);
auton->setAuton(safeCloseAWPDelay);
#elif AUTON == 3
auton->setAuton(closeRushMidElim);
#elif AUTON == 4
Expand Down
223 changes: 222 additions & 1 deletion static/far_6_1.json
View file
Original file line number Diff line number Diff line change
@@ -1 +1,222 @@
{"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"}]}
{
"startSpeed": 0.0,
"endSpeed": 0.0,
"segments": [
{
"inverted": true,
"stopEnd": false,
"paths": [
{
"x": 1.943,
"y": 1.994
},
{
"x": 2.071,
"y": 2.043
},
{
"x": 2.271,
"y": 2.036
},
{
"x": 2.271,
"y": 1.926
}
],
"constraints": {
"velocity": 74,
"accel": 140
}
},
{
"inverted": false,
"stopEnd": false,
"paths": [
{
"x": 2.159,
"y": 2.048
},
{
"x": 2.156,
"y": 2.26
},
{
"x": 2.054,
"y": 2.326
},
{
"x": 1.833,
"y": 2.324
}
],
"constraints": {
"velocity": 74,
"accel": 140
}
},
{
"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": 140
}
},
{
"inverted": false,
"stopEnd": false,
"paths": [
{
"x": 2.108,
"y": 2.186
},
{
"x": 2.199,
"y": 2.354
},
{
"x": 2.378,
"y": 2.147
},
{
"x": 2.334,
"y": 1.915
}
],
"constraints": {
"velocity": 74,
"accel": 140
}
},
{
"inverted": true,
"stopEnd": false,
"paths": [
{
"x": 2.428,
"y": 2.029
},
{
"x": 2.417,
"y": 2.214
},
{
"x": 2.161,
"y": 2.365
},
{
"x": 2.12,
"y": 2.142
}
],
"constraints": {
"velocity": 74,
"accel": 140
}
},
{
"inverted": false,
"stopEnd": false,
"paths": [
{
"x": 2.12,
"y": 2.142
},
{
"x": 2.037,
"y": 2.5
},
{
"x": 1.976,
"y": 2.764
},
{
"x": 1.945,
"y": 3.001
}
],
"constraints": {
"velocity": 74,
"accel": 140
}
},
{
"inverted": true,
"stopEnd": false,
"paths": [
{
"x": 1.625,
"y": 2.998
},
{
"x": 1.638,
"y": 2.671
},
{
"x": 2.678,
"y": 2.389
},
{
"x": 3.567,
"y": 2.764
}
],
"constraints": {
"velocity": 55,
"accel": 140
}
}
],
"commands": [
{
"t": 0.88,
"name": "outtake"
},
{
"t": 1.65,
"name": "intake"
},
{
"t": 2.24,
"name": "outtake"
},
{
"t": 3.77,
"name": "intake"
},
{
"t": 4.84,
"name": "outtake"
},
{
"t": 5.01,
"name": "frontRightWingOut"
},
{
"t": 6.02,
"name": "frontRightWingIn"
},
{
"t": 6.71,
"name": "intakeStop"
}
]
}
1 change: 0 additions & 1 deletion static/safe_close_awp_2.json
View file

This file was deleted.

6 changes: 3 additions & 3 deletions static/skills_3.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@
"y": 0.458
},
{
"x": 3.368,
"y": 0.971
"x": 3.341,
"y": 0.98
},
{
"x": 3.391,
Expand Down Expand Up @@ -103,7 +103,7 @@
"name": "backLeftWingOut"
},
{
"t": 2.56,
"t": 2.5,
"name": "backLeftWingIn"
}
]
Expand Down
8 changes: 0 additions & 8 deletions static/skills_5.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,14 +59,6 @@
{
"t": 0.0,
"name": "frontRightWingOut"
},
{
"t": 0.63,
"name": "frontLeftWingOut"
},
{
"t": 1.12,
"name": "frontLeftWingIn"
}
]
}
7 changes: 1 addition & 6 deletions static/skills_6.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,10 +107,5 @@
}
}
],
"commands": [
{
"t": 1.86,
"name": "frontLeftWingIn"
}
]
"commands": []
}
9 changes: 2 additions & 7 deletions static/skills_6_5.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@
},
{
"x": 1.591,
"y": 2.014
"y": 2.013
},
{
"x": 1.607,
Expand Down Expand Up @@ -107,10 +107,5 @@
}
}
],
"commands": [
{
"t": 1.03,
"name": "frontLeftWingIn"
}
]
"commands": []
}
4 changes: 0 additions & 4 deletions static/skills_7.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,10 +134,6 @@
}
],
"commands": [
{
"t": 1.03,
"name": "frontLeftWingIn"
},
{
"t": 2.52,
"name": "frontLeftWingOut"
Expand Down
Loading

0 comments on commit 2321e64

Please sign in to comment.