Skip to content

Commit

Permalink
feat: tuning & profiled PID for strafe angle
Browse files Browse the repository at this point in the history
  • Loading branch information
@LouisAsanaka
LouisAsanaka committed Dec 10, 2020
1 parent a5502a7 commit 428f5fc
Show file tree
Hide file tree
Showing 5 changed files with 28 additions and 41 deletions.
15 changes: 7 additions & 8 deletions include/constants.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ constexpr double STRAFING_DIST_TARGET_ERROR = 0.015;
constexpr double STRAFING_DIST_TARGET_DERIV = 5;
constexpr okapi::QTime STRAFING_DIST_TARGET_TIME = 150_ms;

constexpr double STRAFING_ANGLE_TARGET_ERROR = 0.05;
constexpr double STRAFING_ANGLE_TARGET_ERROR = 2;
constexpr double STRAFING_ANGLE_TARGET_DERIV = 5;
constexpr okapi::QTime STRAFING_ANGLE_TARGET_TIME = 150_ms;

Expand All @@ -51,18 +51,17 @@ constexpr double TURN_KP = 0.0045;
constexpr double TURN_KI = 0.001;
constexpr double TURN_KD = 0.00009;

constexpr double STRAFE_DISTANCE_KP = 6;
constexpr double STRAFE_DISTANCE_KP = 5;
constexpr double STRAFE_DISTANCE_KI = 0.0;
constexpr double STRAFE_DISTANCE_KD = 0.005;
constexpr double STRAFE_DISTANCE_KD = 0.003;
constexpr planner::TrapezoidProfile::Constraints
STRAFE_DISTANCE_CONTSTRAINTS {1.1, 1.2}; // m/s, m/s^2
STRAFE_DISTANCE_CONTSTRAINTS {1.0, 1.0}; // m/s, m/s^2

constexpr double STRAFE_ANGLE_KP = 1.0;
constexpr double STRAFE_ANGLE_KP = 0.03;
constexpr double STRAFE_ANGLE_KI = 0.0;
constexpr double STRAFE_ANGLE_KD = 0.001;

constexpr double ANGLE_SLEW_INCREASE_RATE = 1; // units / s
constexpr double ANGLE_SLEW_DECREASE_RATE = 6; // units / s
constexpr planner::TrapezoidProfile::Constraints
STRAFE_ANGLE_CONTSTRAINTS {210, 210}; // deg/s, deg/s^2

// Controller
constexpr double CONTROLLER_DEADBAND = 0.1;
Expand Down
10 changes: 4 additions & 6 deletions include/libraidzero/chassis/controller/xOdomController.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,8 @@ class XOdomController : public TaskWrapper {
* @param iturnPid The PID controller that controls chassis angle for turning.
* @param istrafeDistancePid The trapezoid-profiled PID controller that controls
chassis distance while strafing.
* @param istrafeAnglePid The PID controller that controls chassis angle while strafing.
* @param iangleSlewRate The slew rate limiter for angle.
* @param istrafeAnglePid The trapezoid-profiled PID controller that controls
chassis angle while strafing.
* @param igearset The internal gearset and external ratio used on the drive motors.
* @param iscales The ChassisScales.
* @param idistanceThreshold minimum length movement (smaller movements will be skipped)
Expand All @@ -67,8 +67,7 @@ class XOdomController : public TaskWrapper {
std::unique_ptr<IterativePosPIDController> ianglePid,
std::unique_ptr<IterativePosPIDController> iturnPid,
std::unique_ptr<ProfiledPIDController> istrafeDistancePid,
std::unique_ptr<IterativePosPIDController> istrafeAnglePid,
std::unique_ptr<SlewRateLimiter> iangleSlewRate,
std::unique_ptr<ProfiledPIDController> istrafeAnglePid,
const AbstractMotor::GearsetRatioPair& igearset,
const ChassisScales& iscales,
QLength idistanceThreshold = 0_mm,
Expand Down Expand Up @@ -217,8 +216,7 @@ class XOdomController : public TaskWrapper {
std::unique_ptr<IterativePosPIDController> anglePid {nullptr};
std::unique_ptr<IterativePosPIDController> turnPid {nullptr};
std::unique_ptr<ProfiledPIDController> strafeDistancePid {nullptr};
std::unique_ptr<IterativePosPIDController> strafeAnglePid {nullptr};
std::unique_ptr<SlewRateLimiter> angleSlewRate {nullptr};
std::unique_ptr<ProfiledPIDController> strafeAnglePid {nullptr};

AbstractMotor::GearsetRatioPair gearsetRatioPair;
ChassisScales scales;
Expand Down
8 changes: 1 addition & 7 deletions src/autonomous.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,8 @@ void autonomous() {
rightSide3(true);
} else {
reset(0_in, 0_in, 0_deg, false);
robot::drive->controller->strafeToPose({-0.4_m, 0.4_m, 0_deg}, 4000);
robot::drive->controller->strafeToPose({0.0_m, 0.0_m, 0_deg}, 4000);
// pros::delay(500);
// robot::drive->controller->strafeToPose({0_in, 0_in, 0_deg});
robot::drive->controller->strafeToPose({0.314_m, 0.0_m, 0_deg}, 4000);
std::cout << Pose2d::fromOdomState(robot::drive->controller->getState()).toString() << std::endl;
// robot::drive->controller->strafeToPose({16_in, 16_in, 90_deg});
// pros::delay(1000);
// robot::drive->controller->strafeToPose({0_in, 16_in, -45_deg});
}

std::stringstream ss;
Expand Down
28 changes: 13 additions & 15 deletions src/libraidzero/chassis/controller/xOdomController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,7 @@ XOdomController::XOdomController(
std::unique_ptr<IterativePosPIDController> ianglePid,
std::unique_ptr<IterativePosPIDController> iturnPid,
std::unique_ptr<ProfiledPIDController> istrafeDistancePid,
std::unique_ptr<IterativePosPIDController> istrafeAnglePid,
std::unique_ptr<SlewRateLimiter> iangleSlewRate,
std::unique_ptr<ProfiledPIDController> istrafeAnglePid,
const AbstractMotor::GearsetRatioPair& igearset,
const ChassisScales& iscales,
QLength idistanceThreshold,
Expand All @@ -38,7 +37,6 @@ XOdomController::XOdomController(
turnPid{std::move(iturnPid)},
strafeDistancePid{std::move(istrafeDistancePid)},
strafeAnglePid{std::move(istrafeAnglePid)},
angleSlewRate{std::move(iangleSlewRate)},
gearsetRatioPair{igearset},
scales{iscales},
distanceThreshold{idistanceThreshold},
Expand Down Expand Up @@ -132,13 +130,13 @@ void XOdomController::driveToPoint(const Point& ipoint, bool ibackwards,
if (angle.abs() > turnThreshold) {
LOG_INFO("XOdomController: Turning " + std::to_string(angle.convert(degree)) +
" degrees");
turnAngle(angle, TurnType::PointTurn, itimeout, iactions);
turnAngle(angle, TurnType::PointTurn, itimeout, std::move(iactions));
}

if (length.abs() > distanceThreshold) {
LOG_INFO("XOdomController: Driving " +
std::to_string((length).convert(meter)) + " meters");
driveForDistance(length, itimeout, iactions);
driveForDistance(length, itimeout, std::move(iactions));
}
}

Expand Down Expand Up @@ -209,7 +207,7 @@ void XOdomController::turnToAngle(QAngle iangle, TurnType iturnType, int itimeou
if (angle.abs() > turnThreshold) {
LOG_INFO("XOdomController: Turning " + std::to_string(angle.convert(degree)) +
" degrees");
turnAngle(angle, iturnType, itimeout, iactions);
turnAngle(angle, iturnType, itimeout, std::move(iactions));
}
}

Expand All @@ -227,7 +225,7 @@ void XOdomController::turnToPoint(const Point& ipoint, int itimeout,
if (angle.abs() > turnThreshold) {
LOG_INFO("XOdomController: Turning " + std::to_string(angle.convert(degree)) +
" degrees");
turnAngle(angle, TurnType::PointTurn, itimeout, iactions);
turnAngle(angle, TurnType::PointTurn, itimeout, std::move(iactions));
}
}

Expand All @@ -240,7 +238,7 @@ void XOdomController::strafeToPoint(const Point& ipoint, int itimeout,
Rotation2d{-getState().theta}
},
itimeout,
iactions
std::move(iactions)
);
}

Expand Down Expand Up @@ -277,9 +275,8 @@ void XOdomController::updateStrafeToPose(
directionVector *= distanceOutput;
directionVector = directionVector.rotateBy(Rotation2d{-gyroRotation});

double preSlewAngleOutput = strafeAnglePid->step(gyroRotation.convert(radian));
double angleOutput = angleSlewRate->calculate(preSlewAngleOutput);
//std::cout << "Angle Pre-slew: " << preSlewAngleOutput << " | Slewed: " << angleOutput << std::endl;
double angleOutput = strafeAnglePid->step(gyroRotation.convert(degree));
std::cout << "Angle Output: " << angleOutput << std::endl;

// std::cout << "Distance PID: " << distanceOutput << " | Angle PID: " << angleOutput << std::endl;

Expand All @@ -300,17 +297,18 @@ void XOdomController::strafeToPose(const Pose2d& ipose, int itimeout,

LOG_INFO("XOdomController: strafing to " + ipose.toString());

auto currentPose = Pose2d::fromOdomState(getState());
strafeDistancePid->reset(
Pose2d::fromOdomState(getState()).translation().distance(
currentPose.translation().distance(
ipose.translation()
).convert(meter),
0.0
);
strafeDistancePid->flipDisable(false);
strafeDistancePid->setGoal(0.0);
strafeAnglePid->reset();
strafeAnglePid->reset(currentPose.angle().convert(degree));
strafeAnglePid->flipDisable(false);
strafeAnglePid->setTarget(ipose.angle().convert(radian));
strafeAnglePid->setGoal(ipose.angle().convert(degree));
distancePid->flipDisable(true);
anglePid->flipDisable(true);
turnPid->flipDisable(true);
Expand Down Expand Up @@ -401,7 +399,7 @@ bool XOdomController::isAngleSettled() {
}

bool XOdomController::isStrafeSettled() {
return strafeDistancePid->atGoal() && strafeAnglePid->isSettled();
return strafeDistancePid->atGoal() && strafeAnglePid->atGoal();
}

void XOdomController::stopAfterSettled() {
Expand Down
8 changes: 3 additions & 5 deletions src/robot/drive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,11 +78,9 @@ Drive::Drive() {
std::make_unique<ProfiledPIDController>(STRAFE_DISTANCE_GAINS,
strafingDistTimeFactory.create(),
STRAFE_DISTANCE_CONTSTRAINTS),
std::make_unique<IterativePosPIDController>(STRAFE_ANGLE_GAINS,
strafingAngleTimeFactory.create(),
std::make_unique<PassthroughFilter>(),
controllerLogger),
std::make_unique<SlewRateLimiter>(ANGLE_SLEW_INCREASE_RATE, ANGLE_SLEW_DECREASE_RATE),
std::make_unique<ProfiledPIDController>(STRAFE_ANGLE_GAINS,
strafingAngleTimeFactory.create(),
STRAFE_ANGLE_CONTSTRAINTS),
gearing, odomScales, DISTANCE_BEFORE_MOVE, ANGLE_BEFORE_TURN, controllerLogger
);
controller->startTask();
Expand Down

0 comments on commit 428f5fc

Please sign in to comment.