Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Stabilize beyblade (fix wobbling) #133

Merged
merged 2 commits into from
Jun 17, 2024
Merged

Stabilize beyblade (fix wobbling) #133

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ecef84e
Swap to cascading yaw control and motor encoder
calebchalmers Jun 17, 2024
a86c148
Copy standard yaw PIDs to hero
calebchalmers Jun 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions ut-robomaster/src/robots/hero/hero_constants.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -121,8 +121,9 @@ const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AG
const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "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
YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}};
const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}};
const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};

// Velocities ----------------------------
Expand Down
7 changes: 5 additions & 2 deletions ut-robomaster/src/robots/standard/standard_constants.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,8 +117,11 @@ const MotorConfig
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, {}};
// {30.0f, 0.0f, 0.0f}
// {60.0f, 0.0f, 1.0f}
const MotorConfig
YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}};
const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}};
const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};

// Velocities -------------------------------------
Expand Down
59 changes: 42 additions & 17 deletions ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,15 +9,15 @@ DoubleYawMotor::DoubleYawMotor(
src::Drivers *drivers,
MotorConfig motor1,
MotorConfig motor2,
Encoder *encoder,
const SmoothPidConfig &pidConfig)
Encoder *encoder)
: drivers(drivers),
motor1(drivers, motor1.id, motor1.canBus, motor1.inverted, motor1.name),
motor2(drivers, motor2.id, motor2.canBus, motor2.inverted, motor2.name),
encoder(encoder),
pid(pidConfig),
setpoint(0.0f, 0.0f, M_TWOPI),
currentAngle(0.0f, 0.0f, M_TWOPI)
velocityPid(motor1.velocityPidConstants),
positionPid(motor1.positionPidConstants),
setpoint(0.0f, 0.0f, 1.0f),
currentAngle(0.0f, 0.0f, 1.0f)
{
}

Expand All @@ -31,31 +31,56 @@ void DoubleYawMotor::reset()
{
motor2.setDesiredOutput(0);
motor1.setDesiredOutput(0);
pid.reset();
velocityPid.reset();
positionPid.reset();
}

void DoubleYawMotor::updateMotorAngle()
{
float encoderAngle = encoder->getAngle() * M_TWOPI;
// float encoderAngle = encoder->getAngle();
float encoderAngle = static_cast<float>(motor1.getEncoderUnwrapped()) /
DjiMotor::ENC_RESOLUTION / M3508.gearRatio / 2.0f;
currentAngle.setValue(encoderAngle);
}

void DoubleYawMotor::setAngle(float desiredAngle, float dt)
{
setpoint.setValue(desiredAngle);
setpoint.setValue(desiredAngle / M_TWOPI);

float positionControllerError =
ContiguousFloat(currentAngle.getValue(), 0, M_TWOPI).difference(setpoint.getValue());
float rpm1 = motor1.getShaftRPM(); // rev / m
float rpm2 = motor2.getShaftRPM(); // rev / m
float avgRps = (rpm1 + rpm2) / 2.0f * (M_TWOPI / 60.0f); // rad / s
float output = pid.runController(positionControllerError, avgRps, dt);
float positionError =
ContiguousFloat(currentAngle.getValue(), 0, 1.0f).difference(setpoint.getValue());

motor1.setDesiredOutput(output);
motor2.setDesiredOutput(output);
// account for chassis rotation
float disturbance = drivers->bmi088.getGz() / 360.0f; // rev / s
disturbance *= 2.0f; // seems to help?

float targetVelocity = positionPid.update(positionError, dt, false) - disturbance;
setVelocity(targetVelocity, dt);
}

float DoubleYawMotor::getAngle() { return currentAngle.getValue() * M_TWOPI; }

void DoubleYawMotor::setVelocity(float velocity, float dt)
{
float output = velocityPid.update(velocity - getCurrentVelocity(), dt, true);
setOutput(output);
}

float DoubleYawMotor::getCurrentVelocity()
{
float rpm1 = motor1.getShaftRPM(); // rev / m
float rpm2 = motor2.getShaftRPM(); // rev / m
float currentVelocity = (rpm1 + rpm2) / 2.0f / 60.0f / M3508.gearRatio; // rev / s
return currentVelocity;
}

void DoubleYawMotor::setOutput(float output)
{
motor1.setDesiredOutput(output * M3508.maxOutput);
motor2.setDesiredOutput(output * M3508.maxOutput);
}

float DoubleYawMotor::getAngle() { return currentAngle.getValue(); }
float DoubleYawMotor::getOutput() { return motor1.getOutputDesired() / M3508.maxOutput; }

float DoubleYawMotor::getSetpoint() { return setpoint.getValue(); }
bool DoubleYawMotor::isOnline() { return motor1.isMotorOnline() && motor2.isMotorOnline(); }
Expand Down
15 changes: 8 additions & 7 deletions ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

#include "drivers/encoder.hpp"
#include "utils/motors/motor_constants.hpp"
#include "utils/motors/pid.hpp"

#include "drivers.hpp"

Expand All @@ -19,18 +20,17 @@ using driver::Encoder;
class DoubleYawMotor
{
public:
DoubleYawMotor(
src::Drivers *drivers,
MotorConfig motor1,
MotorConfig motor2,
Encoder *encoder,
const SmoothPidConfig &pidConfig);
DoubleYawMotor(src::Drivers *drivers, MotorConfig motor1, MotorConfig motor2, Encoder *encoder);

void initialize();
void reset();
void updateMotorAngle();
void setAngle(float desiredAngle, float dt);
float getAngle();
void setVelocity(float velocity, float dt);
float getCurrentVelocity();
void setOutput(float output);
float getOutput();
float getSetpoint();
bool isOnline();

Expand All @@ -39,7 +39,8 @@ class DoubleYawMotor
DjiMotor motor1;
DjiMotor motor2;
Encoder *encoder;
SmoothPid pid;
motor_controller::Pid velocityPid;
motor_controller::Pid positionPid;
ContiguousFloat setpoint;
ContiguousFloat currentAngle;
};
Expand Down
2 changes: 1 addition & 1 deletion ut-robomaster/src/subsystems/turret/turret_subsystem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ TurretSubsystem::TurretSubsystem(src::Drivers* drivers)
drivers(drivers),
#if defined(TARGET_STANDARD) || defined(TARGET_HERO)
yawEncoder(),
yaw(drivers, YAW_L, YAW_R, &yawEncoder, YAW_PID_CONFIG),
yaw(drivers, YAW_L, YAW_R, &yawEncoder),
#else
yaw(drivers, YAW, YAW_PID_CONFIG),
#endif
Expand Down