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

Jump to bottom

Cleanup constants & motor struct #117

Merged
merged 6 commits into from
May 16, 2024
Merged

Cleanup constants & motor struct #117

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
1a95ffd
created demo constants
Redfalcon5-ai Apr 5, 2024
3e16f31
Reorganized constants & added motor struct
JasonKlm Apr 26, 2024
d57405f
Add turret motor struct constants
calebchalmers May 15, 2024
87e8bcb
Finish converting motor constants to structs
calebchalmers May 16, 2024
7545cdb
Adjust standard demo mode turret speeds
calebchalmers May 16, 2024
6d274e4
Reorder a few constants
calebchalmers May 16, 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
128 changes: 82 additions & 46 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 @@ -6,74 +6,56 @@

#include "modm/container/pair.hpp"
#include "subsystems/sound/sounds/sound_smb_powerup.hpp"
#include "utils/motor_controller/motor_constants.hpp"
#include "utils/motor_controller/pid.hpp"

using motor_controller::PidConstants;
using tap::can::CanBus;
using namespace tap::motor;
using namespace motor_controller;

constexpr CanBus CAN_WHEELS = CanBus::CAN_BUS1;
constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;
// General constants ------------------------------------------------

constexpr bool USE_BALLISTICS = false;
constexpr int BALLISTIC_ITERATIONS = 2;

const Sound SOUND_STARTUP = SOUND_SMB_POWERUP;

constexpr MotorId ID_WHEEL_LF = MOTOR2;
constexpr MotorId ID_WHEEL_RF = MOTOR1;
constexpr MotorId ID_WHEEL_LB = MOTOR3;
constexpr MotorId ID_WHEEL_RB = MOTOR4;
// Kinematics ------------------------------------------------------

// chassis
static constexpr int WHEELS = 4;
static constexpr float WHEEL_DISTANCE_X = 0.525f; // meters
static constexpr float WHEEL_DISTANCE_Y = 0.400f; // meters
static constexpr float WHEEL_RADIUS = 0.1524f; // meters
static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f;
static constexpr float WHEEL_MAX_VEL = 120.0f; // rad/s
static constexpr float MAX_LINEAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS; // m/s
static constexpr float MAX_ANGULAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS / WHEEL_LXY; // rad/s

constexpr PidConstants PID_VELOCITY_DEFAULT = {0.75f, 12.0f, 0.0f};
constexpr PidConstants PID_WHEELS = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_TURRET_VELOCITY = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_TURRET_POSITION = {0.1f, 0.1f, 0.0f};

constexpr MotorId ID_FLYWHEEL_L = MOTOR3;
constexpr MotorId ID_FLYWHEEL_R = MOTOR4;
constexpr MotorId ID_AGITATOR = MOTOR1;
constexpr MotorId ID_FEEDER = MOTOR2;

constexpr MotorId ID_YAW = MOTOR5;
constexpr MotorId ID_PITCH = MOTOR6;

constexpr PidConstants PID_FLYWHEEL = {0.1f, 0.1f, 0.0f};
constexpr PidConstants PID_AGITATOR = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_FEEDER = PID_VELOCITY_DEFAULT;
// flywheel

constexpr bool USE_BALLISTICS = false;
constexpr int BALLISTIC_ITERATIONS = 2;
static constexpr int FLYWHEELS = 2;

constexpr float PITCH_MIN = -0.3349f; // rad
constexpr float PITCH_MAX = 0.3534f; // rad
// turret

// NEED TO MEASURE (MAYBE ADD NEW CONSTANTS)
constexpr float CAMERA_TO_PITCH = 0.0f; // distance from main camera lens to pitch axis (m)
constexpr float NOZZLE_TO_PITCH = 0.0f; // distance from barrel nozzle to pitch axis (m)
constexpr float CAMERA_TO_BARRELS = 0.0f; // vertical ctc offset from camera lens to barrel (m)
constexpr float CAMERA_X_OFFSET = 0.0f; // horizontal offset of main camera lens (m)
constexpr float PITCH_MIN = -0.3349f; // rad
constexpr float PITCH_MAX = 0.3534f; // rad

static constexpr modm::Pair<uint16_t, float> FLYWHEEL_RPS_MAPPING[] = {{10, 105.0f}, {16, 190.0f}};

const float BALLS_PER_SEC = 4.0f;
const float BALLS_PER_REV = 6.0f;
const float FEEDER_RATIO = 1.0f; // feeder speed / agitator speed

const uint16_t BARREL_HEAT_BUFFER = 100.0f;
static constexpr float YAW_REDUCTION = 2.0f;
static constexpr float PITCH_REDUCTION = 1.0f;

const float JAM_TRIGGER_RATIO = 0.5; // 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 = 12.0f; // rev/s
// Tuning Constants -----------------------------------

static constexpr int FLYWHEELS = 2;
static constexpr float DEFAULT_SPEED = 60.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_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,
Expand Down Expand Up @@ -103,13 +85,67 @@ static constexpr tap::algorithms::SmoothPidConfig PITCH_PID_CONFIG = {
.errorDerivativeFloor = 0.0f,
};

static constexpr float YAW_REDUCTION = 2.0f;
static constexpr float PITCH_REDUCTION = 1.0f;
// Input Constants ---------------------------------------

#ifdef DEMO_MDOE
static constexpr float YAW_INPUT_SCALE = 1.0f;
static constexpr float PITCH_INPUT_SCALE = 1.0f;
#else
static constexpr float YAW_INPUT_SCALE = 4.0f;
static constexpr float PITCH_INPUT_SCALE = 5.0f;
#endif

static constexpr float MOUSE_SENS_YAW = 0.0045f;
static constexpr float MOUSE_SENS_PITCH = 0.002f;

const Sound SOUND_STARTUP = SOUND_SMB_POWERUP;
// Motor Constants -------------------------------------

constexpr CanBus CAN_WHEELS = CanBus::CAN_BUS1;
constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;

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

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

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

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

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

#ifdef MODE_DEMO
static constexpr float WHEEL_MAX_VEL = 10.0f; // rad/s
#else
static constexpr float WHEEL_MAX_VEL = 120.0f; // rad/s
#endif

static constexpr float MAX_LINEAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS; // m/s
static constexpr float MAX_ANGULAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS / WHEEL_LXY; // rad/s

static constexpr float DEFAULT_SPEED = 60.0f; // default speed for the flywheels

static constexpr modm::Pair<uint16_t, float> FLYWHEEL_RPS_MAPPING[] = {{10, 105.0f}, {16, 190.0f}};

const float BALLS_PER_SEC = 4.0f;
const float BALLS_PER_REV = 6.0f;
const float FEEDER_RATIO = 1.0f; // feeder speed / agitator speed

const float JAM_TRIGGER_RATIO = 0.5; // 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 = 12.0f; // rev/s

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

const uint16_t BARREL_HEAT_BUFFER = 100.0f;
2 changes: 1 addition & 1 deletion ut-robomaster/src/robots/hero/hero_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ namespace hero_control

// Subsystem definitions ---------------------------------------------------------
ChassisSubsystem chassis(drivers());
AgitatorSubsystem agitator(drivers(), ID_AGITATOR, false);
AgitatorSubsystem agitator(drivers(), AGITATOR);
FlywheelSubsystem flywheel(drivers());
TurretSubsystem turret(drivers());
OdometrySubsystem odometry(drivers(), &chassis, &turret);
Expand Down
143 changes: 86 additions & 57 deletions ut-robomaster/src/robots/sentry/sentry_constants.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,93 +6,52 @@

#include "modm/container/pair.hpp"
#include "subsystems/sound/sounds/sound_smb_powerup.hpp"
#include "utils/motor_controller/motor_constants.hpp"
#include "utils/motor_controller/pid.hpp"

using motor_controller::PidConstants;
using tap::can::CanBus;
using namespace tap::motor;
using namespace motor_controller;

// General constants ------------------------------------------------
constexpr CanBus CAN_WHEELS = CanBus::CAN_BUS1;
constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;

constexpr PidConstants PID_VELOCITY_DEFAULT = {0.75f, 12.0f, 0.0f};

constexpr bool USE_BALLISTICS = false;
constexpr int BALLISTIC_ITERATIONS = 2;

// Chassis constants ------------------------------------------------
constexpr MotorId ID_WHEEL_LF = MOTOR2;
constexpr MotorId ID_WHEEL_RF = MOTOR1;
constexpr MotorId ID_WHEEL_LB = MOTOR3;
constexpr MotorId ID_WHEEL_RB = MOTOR4;
const Sound SOUND_STARTUP = SOUND_SMB_POWERUP;

// Kinematic constants -----------------------------------------------

// chassis ------------
static constexpr int WHEELS = 4;
static constexpr float WHEEL_DISTANCE_X = 0.391f; // meters
static constexpr float WHEEL_DISTANCE_Y = 0.315f; // meters
static constexpr float WHEEL_RADIUS = 0.1524f; // meters
static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f;
static constexpr float WHEEL_MAX_VEL = 50.0f; // rad/s
static constexpr float MAX_LINEAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS; // m/s
static constexpr float MAX_ANGULAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS / WHEEL_LXY; // rad/s

constexpr PidConstants PID_WHEELS = PID_VELOCITY_DEFAULT;

// Flywheel constants ------------------------------------------------
constexpr MotorId ID_FLYWHEEL_TL = MOTOR3;
constexpr MotorId ID_FLYWHEEL_TR = MOTOR4;
constexpr MotorId ID_FLYWHEEL_BL = MOTOR5;
constexpr MotorId ID_FLYWHEEL_BR = MOTOR6;

constexpr PidConstants PID_FLYWHEEL = {0.1f, 0.1f, 0.0f};

static constexpr modm::Pair<uint16_t, float> FLYWHEEL_RPS_MAPPING[] = {
{15, 67.0f},
{18, 75.0f},
{30, 122.0f}};

// flywheels -----------
static constexpr int FLYWHEELS = 4;
static constexpr float DEFAULT_SPEED = 70.0f;

// Agitator constants ------------------------------------------------
constexpr MotorId ID_AGITATOR_L = MOTOR1;
constexpr MotorId ID_AGITATOR_R = MOTOR2;

constexpr PidConstants PID_AGITATOR = PID_VELOCITY_DEFAULT;

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

const uint16_t BARREL_HEAT_BUFFER = 20.0f;

const float JAM_TRIGGER_RATIO = 0.5; // 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

// Turret constants ------------------------------------------------
constexpr MotorId ID_YAW = MOTOR6;
constexpr MotorId ID_PITCH = MOTOR7;

// turret ------------
constexpr float PITCH_MIN = -0.2185f; // rad
constexpr float PITCH_MAX = 0.2299f; // 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)
constexpr float CAMERA_X_OFFSET = -0.0335f; // horizontal offset of main camera lens (m)

constexpr PidConstants PID_TURRET_VELOCITY = PID_VELOCITY_DEFAULT;
constexpr PidConstants PID_TURRET_POSITION = {0.1f, 0.1f, 0.0f};

static constexpr float YAW_REDUCTION = 2.0f;
static constexpr float PITCH_REDUCTION = 4.0f;

static constexpr float YAW_INPUT_SCALE = 10.0f;
static constexpr float PITCH_INPUT_SCALE = 5.0f;
// Tuning constants -----------------------------------------------

static constexpr float MOUSE_SENS_YAW = 0.1f;
static constexpr float MOUSE_SENS_PITCH = 0.1f;
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_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,
Expand Down Expand Up @@ -122,4 +81,74 @@ static constexpr tap::algorithms::SmoothPidConfig PITCH_PID_CONFIG = {
.errorDerivativeFloor = 0.0f,
};

const Sound SOUND_STARTUP = SOUND_SMB_POWERUP;
// Input constants ----------------------------

#ifdef DEMO_MODE
static constexpr float YAW_INPUT_SCALE = 4.0f;
static constexpr float PITCH_INPUT_SCALE = 2.0f;
#else
static constexpr float YAW_INPUT_SCALE = 10.0f;
static constexpr float PITCH_INPUT_SCALE = 5.0f;
#endif

static constexpr float MOUSE_SENS_YAW = 0.1f;
static constexpr float MOUSE_SENS_PITCH = 0.1f;

// Motor constants --------------------------------

constexpr CanBus CAN_WHEELS = CanBus::CAN_BUS1;
constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;

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

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

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

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

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

#ifdef MODE_DEMO
static constexpr float WHEEL_MAX_VEL = 10.0f; // rad/s
#else
static constexpr float WHEEL_MAX_VEL = 50.0f; // rad/s
#endif

static constexpr float MAX_LINEAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS; // m/s
static constexpr float MAX_ANGULAR_VEL = WHEEL_MAX_VEL * WHEEL_RADIUS / WHEEL_LXY; // rad/s

static constexpr modm::Pair<uint16_t, float> FLYWHEEL_RPS_MAPPING[] = {
{15, 67.0f},
{18, 75.0f},
{30, 122.0f}};
static constexpr float DEFAULT_SPEED = 70.0f;

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

const float JAM_TRIGGER_RATIO = 0.5; // 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

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

const uint16_t BARREL_HEAT_BUFFER = 20.0f;
4 changes: 2 additions & 2 deletions ut-robomaster/src/robots/sentry/sentry_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,8 @@ namespace sentry_control

// Subsystem definitions ---------------------------------------------------------
ChassisSubsystem chassis(drivers());
AgitatorSubsystem agitator1(drivers(), ID_AGITATOR_L, false);
AgitatorSubsystem agitator2(drivers(), ID_AGITATOR_R, true);
AgitatorSubsystem agitator1(drivers(), AGITATOR_L);
AgitatorSubsystem agitator2(drivers(), AGITATOR_R);
FlywheelSubsystem flywheel(drivers());
TurretSubsystem turret(drivers());
OdometrySubsystem odometry(drivers(), &chassis, &turret);
Expand Down
Loading