Skip to content

Commit

Permalink
Merge pull request #132 from ut-ras/pid-tuning
Browse files Browse the repository at this point in the history 
Pid Tuning, etc.
  • Loading branch information
@calebchalmers
calebchalmers authored Jun 15, 2024
2 parents 81e91a9 + 1b687e5 commit 92fb94e
Show file tree
Hide file tree
Showing 25 changed files with 257 additions and 224 deletions.
47 changes: 46 additions & 1 deletion .vscode/launch.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,15 +22,60 @@
"address": "0x20000000",
"searchSize": 131072,
"searchId": "modm.rtt.modm",
"polling_interval": 100,
"polling_interval": 10,
"decoders": [
{
"port": 0,
"type": "console",
"logfile": "${workspaceRoot}/ut-robomaster/rtt.log"
},
{
"port": 1,
"type": "graph",
"encoding": "float",
"graphId": "1"
},
{
"port": 2,
"type": "graph",
"encoding": "float",
"graphId": "2"
},
{
"port": 3,
"type": "graph",
"encoding": "float",
"graphId": "3"
}
],
},
"graphConfig": [
{
"label": "RTT Data",
"timespan": 30,
"type": "realtime",
"annotate": false,
"maximum": 1,
"minimum": -1,
"plots": [
{
"graphId": "1",
"label": "Plot 1",
"color": "#ff0000"
},
{
"graphId": "2",
"label": "Plot 2",
"color": "#00ff00"
},
{
"graphId": "3",
"label": "Plot 3",
"color": "#0000ff"
}
]
},
]
},
]
}
36 changes: 33 additions & 3 deletions ut-robomaster/src/communication/rtt.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,13 +3,43 @@

namespace communication
{
using modm::platform::Rtt;

class RttStream : public modm::IOStream
{
public:
RttStream() : IOStream(device) {}
RttStream() : IOStream(text_device) {}

/// @brief Send raw binary data to RTT channel for plotting and analysis
/// @param channel Binary RTT channel to send to (1, 2, or 3)
void plot(uint8_t channel, float32_t val)
{
Rtt* rtt = nullptr;

switch (channel)
{
case 1:
rtt = &rtt_plot1;
break;
case 2:
rtt = &rtt_plot2;
break;
case 3:
rtt = &rtt_plot3;
break;
default:
return;
}

rtt->write(reinterpret_cast<uint8_t const*>(&val), 4);
}

private:
modm::platform::Rtt rtt{0};
modm::IODeviceObjectWrapper<modm::platform::Rtt, modm::IOBuffer::DiscardIfFull> device{rtt};
Rtt rtt_text{0};
modm::IODeviceObjectWrapper<Rtt, modm::IOBuffer::DiscardIfFull> text_device{rtt_text};

Rtt rtt_plot1{1};
Rtt rtt_plot2{2};
Rtt rtt_plot3{3};
};
} // namespace communication
42 changes: 21 additions & 21 deletions ut-robomaster/src/robots/hero/hero_constants.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,32 +51,32 @@ static constexpr float PITCH_REDUCTION = 1.0f;

constexpr PidConstants PID_VELOCITY_DEFAULT = {0.75f, 12.0f, 0.0f};
constexpr PidConstants PID_WHEELS = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_FLYWHEEL = {0.1f, 0.1f, 0.0f};
constexpr PidConstants PID_FLYWHEEL = {0.065f, 0.325f, 0.0f};
constexpr PidConstants PID_AGITATOR = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_FEEDER = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_TURRET_VELOCITY = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_TURRET_POSITION = {0.1f, 0.1f, 0.0f};

static constexpr tap::algorithms::SmoothPidConfig YAW_PID_CONFIG = {
.kp = 100'183.1f,
.kp = 100'000.0f,
.ki = 0.0f,
.kd = 2'500.0f,
.kd = 150.0f,
.maxICumulative = 0.0f,
.maxOutput = 32'000.0f,
.maxOutput = M3508.maxOutput,
.tQDerivativeKalman = 1.0f,
.tRDerivativeKalman = 30.0f,
.tRDerivativeKalman = 0.0f,
.tQProportionalKalman = 1.0f,
.tRProportionalKalman = 0.0f,
.errDeadzone = 0.0f,
.errorDerivativeFloor = 10.0f,
.errorDerivativeFloor = 0.0f,
};

static constexpr tap::algorithms::SmoothPidConfig PITCH_PID_CONFIG = {
.kp = 100'183.1f,
.ki = 0.0f,
.kd = 1'000.0f,
.kp = 100'000.0f,
.ki = 100'000.0f,
.kd = 3'500.0f,
.maxICumulative = 0.0f,
.maxOutput = 32'000.0f,
.maxOutput = GM6020.maxOutput,
.tQDerivativeKalman = 1.0f,
.tRDerivativeKalman = 30.0f,
.tQProportionalKalman = 1.0f,
Expand Down Expand Up @@ -105,25 +105,25 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;

// chassis
const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS};
const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS};
const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS};
const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS};
const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}};
const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}};
const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}};
const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}};

// flywheels
const MotorConfig
FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel left", PID_FLYWHEEL};
FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel left", PID_FLYWHEEL, {}};
const MotorConfig
FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel right", PID_FLYWHEEL};
FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel right", PID_FLYWHEEL, {}};

// agitator
const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR};
const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER};
const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}};
const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, true, "feeder", PID_FEEDER, {}};

// turret
const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", PID_VELOCITY_DEFAULT};
const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", PID_VELOCITY_DEFAULT};
const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT};
const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", PID_VELOCITY_DEFAULT, {}};
const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", PID_VELOCITY_DEFAULT, {}};
const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};

// Velocities ----------------------------

Expand Down
2 changes: 1 addition & 1 deletion ut-robomaster/src/robots/robot_constants.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ const float ANALOG_DEAD_ZONE = 0.1f;
const float KEYBOARD_ACCEL = 5.0f;
const float KEYBOARD_DECEL = 5.0f;
const float CHASSIS_AUTOALIGN_FACTOR = 0.5f;
const float CHASSIS_AUTOALIGN_ANGLE = M_PI / 4.0f;
const float CHASSIS_AUTOALIGN_ANGLE = M_PI / 2.0f;

#if defined(TARGET_STANDARD)
#include "robots/standard/standard_constants.hpp"
Expand Down
48 changes: 33 additions & 15 deletions ut-robomaster/src/robots/sentry/sentry_constants.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,28 +101,46 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;

// chassis
const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS};
const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS};
const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS};
const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS};
const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}};
const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}};
const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}};
const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}};

// flywheels
const MotorConfig
FLYWHEEL_TL{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel top left", PID_FLYWHEEL};
const MotorConfig
FLYWHEEL_TR{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel top right", PID_FLYWHEEL};
const MotorConfig
FLYWHEEL_BL{M3508_NOGEARBOX, MOTOR5, CAN_SHOOTER, false, "flywheel bottom left", PID_FLYWHEEL};
const MotorConfig
FLYWHEEL_BR{M3508_NOGEARBOX, MOTOR6, CAN_SHOOTER, true, "flywheel bottom right", PID_FLYWHEEL};
FLYWHEEL_TL{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel top left", PID_FLYWHEEL, {}};
const MotorConfig FLYWHEEL_TR{
M3508_NOGEARBOX,
MOTOR4,
CAN_SHOOTER,
false,
"flywheel top right",
PID_FLYWHEEL,
{}};
const MotorConfig FLYWHEEL_BL{
M3508_NOGEARBOX,
MOTOR5,
CAN_SHOOTER,
false,
"flywheel bottom left",
PID_FLYWHEEL,
{}};
const MotorConfig FLYWHEEL_BR{
M3508_NOGEARBOX,
MOTOR6,
CAN_SHOOTER,
true,
"flywheel bottom right",
PID_FLYWHEEL,
{}};

// agitator
const MotorConfig AGITATOR_L{M2006, MOTOR1, CAN_SHOOTER, false, "agitator left", PID_AGITATOR};
const MotorConfig AGITATOR_R{M2006, MOTOR2, CAN_SHOOTER, true, "agitator right", PID_AGITATOR};
const MotorConfig AGITATOR_L{M2006, MOTOR1, CAN_SHOOTER, false, "agitator left", PID_AGITATOR, {}};
const MotorConfig AGITATOR_R{M2006, MOTOR2, CAN_SHOOTER, true, "agitator right", PID_AGITATOR, {}};

// turret
const MotorConfig YAW{GM6020, MOTOR6, CAN_TURRET, false, "yaw", PID_VELOCITY_DEFAULT};
const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT};
const MotorConfig YAW{GM6020, MOTOR6, CAN_TURRET, false, "yaw", PID_VELOCITY_DEFAULT, {}};
const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};

// Velocities -------------------------------------

Expand Down
53 changes: 27 additions & 26 deletions ut-robomaster/src/robots/standard/standard_constants.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,8 @@ static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f;
static constexpr int FLYWHEELS = 2;

// turret ------------
constexpr float PITCH_MIN = -0.2185f; // rad
constexpr float PITCH_MAX = 0.2299f; // rad
constexpr float PITCH_MIN = 0.10f; // rad
constexpr float PITCH_MAX = 0.79f; // rad
constexpr float CAMERA_TO_PITCH = 0.13555f; // distance from main camera lens to pitch axis (m)
constexpr float NOZZLE_TO_PITCH = 0.18151f; // distance from barrel nozzle to pitch axis (m)
constexpr float CAMERA_TO_BARRELS = 0.0427f; // vertical ctc offset from camera lens to barrel (m)
Expand All @@ -47,32 +47,33 @@ static constexpr float PITCH_REDUCTION = 1.0f;
// Tuning constants -----------------------------------------------

constexpr PidConstants PID_VELOCITY_DEFAULT = {0.75f, 12.0f, 0.0f};
constexpr PidConstants PID_POSITION_DEFAULT = {10.0f, 0.0f, 0.0f};
constexpr PidConstants PID_WHEELS = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_FLYWHEEL = {0.1f, 0.1f, 0.0f};
constexpr PidConstants PID_FLYWHEEL = {0.065f, 0.325f, 0.0f};
constexpr PidConstants PID_AGITATOR = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_TURRET_VELOCITY = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_TURRET_POSITION = {0.1f, 0.1f, 0.0f};

static constexpr tap::algorithms::SmoothPidConfig YAW_PID_CONFIG = {
.kp = 65'000.0f,
.kp = 100'000.0f,
.ki = 0.0f,
.kd = 3'000.0f,
.kd = 150.0f,
.maxICumulative = 0.0f,
.maxOutput = 32'000.0f,
.maxOutput = M3508.maxOutput,
.tQDerivativeKalman = 1.0f,
.tRDerivativeKalman = 30.0f,
.tRDerivativeKalman = 0.0f,
.tQProportionalKalman = 1.0f,
.tRProportionalKalman = 0.0f,
.errDeadzone = 0.0f,
.errorDerivativeFloor = 10.0f,
.errorDerivativeFloor = 0.0f,
};

static constexpr tap::algorithms::SmoothPidConfig PITCH_PID_CONFIG = {
.kp = 100'183.1f,
.ki = 0.0f,
.kd = 1'000.0f,
.kp = 100'000.0f,
.ki = 100'000.0f,
.kd = 3'500.0f,
.maxICumulative = 0.0f,
.maxOutput = 32'000.0f,
.maxOutput = GM6020.maxOutput,
.tQDerivativeKalman = 1.0f,
.tRDerivativeKalman = 30.0f,
.tQProportionalKalman = 1.0f,
Expand Down Expand Up @@ -101,24 +102,24 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;

// chassis
const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS};
const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS};
const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS};
const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS};
const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}};
const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}};
const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}};
const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}};

// flywheels
const MotorConfig
FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel left", PID_FLYWHEEL};
FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel left", PID_FLYWHEEL, {}};
const MotorConfig
FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel right", PID_FLYWHEEL};
FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel right", PID_FLYWHEEL, {}};

// agitator
const MotorConfig AGITATOR{M2006, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR};
const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}};

// turret
const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", PID_VELOCITY_DEFAULT};
const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", PID_VELOCITY_DEFAULT};
const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT};
const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", PID_VELOCITY_DEFAULT, {}};
const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", PID_VELOCITY_DEFAULT, {}};
const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};

// Velocities -------------------------------------

Expand All @@ -132,15 +133,15 @@ static constexpr float MAX_LINEAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS;
static constexpr float MAX_ANGULAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS / WHEEL_LXY; // rad/s

const float TARGET_PROJECTILE_VELOCITY = 30; // m/s
const float FLYWHEEL_SPEED = 122.0f;
const float FLYWHEEL_SPEED = 160.0f;

const float BALLS_PER_SEC = 10.0f;
const float BALLS_PER_REV = 8.0f;
const float BALLS_PER_REV = 18.0f;

const float JAM_TRIGGER_RATIO = 0.5; // measured speed to driven speed ratio
const float JAM_TRIGGER_RATIO = 0.2; // measured speed to driven speed ratio
const float JAM_TRIGGER_DURATION = 0.1f; // s
const float UNJAM_DURATION = 0.1f; // s
const float UNJAM_SPEED = 15.0f; // rev/s
const float UNJAM_SPEED = 30.0f; // rev/s

// Heat Buffers -------------------------------------

Expand Down
4 changes: 2 additions & 2 deletions ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,10 +41,10 @@ void AgitatorSubsystem::refresh()
bool killSwitch = drivers->isKillSwitched();

agitator.setActive(!killSwitch);
agitator.update(velocity);
agitator.updateVelocity(velocity);
#ifdef TARGET_HERO
feeder.setActive(!killSwitch);
feeder.update(velocity * FEEDER_RATIO);
feeder.updateVelocity(velocity * FEEDER_RATIO);
#endif
}

Expand Down
Loading

0 comments on commit 92fb94e

Please sign in to comment.