Skip to content

Commit 0cdc798

Browse files
calebchalmerscalebchalmers
committed
Merge remote-tracking branch 'origin/develop' into remote-debug
2 parents 579364a + 94ba858 commit 0cdc798

9 files changed

+151
-132
lines changed

‎ut-robomaster/src/robots/hero/hero_constants.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ constexpr float CAMERA_X_OFFSET = 0.0f; // horizontal offset of main camera l
4444
constexpr float PITCH_MIN = -0.3349f; // rad
4545
constexpr float PITCH_MAX = 0.3534f; // rad
4646

47-
static constexpr float YAW_REDUCTION = 2.0f;
47+
static constexpr float YAW_REDUCTION = 1.0f;
4848
static constexpr float PITCH_REDUCTION = 1.0f;
4949

5050
// Tuning Constants -----------------------------------

‎ut-robomaster/src/robots/hero/hero_control.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -142,7 +142,7 @@ HoldCommandMapping leftSwitchUp(
142142
RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP));
143143

144144
// Register subsystems here -----------------------------------------------
145-
void registerStandardSubsystems(src::Drivers *drivers)
145+
void registerSubsystems(src::Drivers *drivers)
146146
{
147147
drivers->commandScheduler.registerSubsystem(&chassis);
148148
drivers->commandScheduler.registerSubsystem(&agitator);
@@ -199,7 +199,7 @@ namespace control
199199
void initSubsystemCommands(src::Drivers *drivers)
200200
{
201201
hero_control::initializeSubsystems();
202-
hero_control::registerStandardSubsystems(drivers);
202+
hero_control::registerSubsystems(drivers);
203203
hero_control::setDefaultCommands(drivers);
204204
hero_control::runStartupCommands(drivers);
205205
hero_control::registerMappings(drivers);

‎ut-robomaster/src/robots/sentry/sentry_control.cpp

+110-9
Original file line numberDiff line numberDiff line change
@@ -8,23 +8,33 @@
88
#include "tap/control/toggle_command_mapping.hpp"
99

1010
#include "robots/robot_constants.hpp"
11+
#include "utils/power_limiter/barrel_cooldown.hpp"
1112

1213
#include "drivers.hpp"
1314
#include "drivers_singleton.hpp"
1415

1516
// Chassis includes ----------------------------------------
1617
#include "subsystems/chassis/chassis_subsystem.hpp"
17-
#include "subsystems/chassis/command_sentry_position.hpp"
18+
#include "subsystems/chassis/command_beyblade_chassis_keyboard.hpp"
19+
#include "subsystems/chassis/command_move_chassis_joystick.hpp"
20+
#include "subsystems/chassis/command_move_chassis_keyboard.hpp"
21+
#include "subsystems/chassis/command_move_chassis_turret_relative_joystick.hpp"
1822

1923
// Agitator includes ----------------------------------------
2024
#include "subsystems/agitator/agitator_subsystem.hpp"
25+
#include "subsystems/agitator/command_agitator_continuous.hpp"
26+
#include "subsystems/agitator/command_unjam_agitator.hpp"
2127

2228
// Flywheel includes ----------------------------------------
29+
#include "subsystems/flywheel/command_flywheel_off.hpp"
30+
#include "subsystems/flywheel/command_rotate_flywheel.hpp"
2331
#include "subsystems/flywheel/flywheel_subsystem.hpp"
2432

2533
// Turret includes ------------------------------------------
2634
#include "subsystems/odometry/odometry_subsystem.hpp"
27-
#include "subsystems/turret/command_sentry_aim.hpp"
35+
#include "subsystems/turret/command_move_turret_aimbot.hpp"
36+
#include "subsystems/turret/command_move_turret_joystick.hpp"
37+
#include "subsystems/turret/command_move_turret_mouse.hpp"
2838
#include "subsystems/turret/turret_subsystem.hpp"
2939

3040
// Sound includes -----------------------------------------
@@ -43,6 +53,8 @@ using namespace subsystems::sound;
4353

4454
using namespace commands;
4555

56+
using power_limiter::BarrelId;
57+
4658
/*
4759
* NOTE: We are using the DoNotUse_getDrivers() function here
4860
* because this file defines all subsystems and command
@@ -64,13 +76,86 @@ OdometrySubsystem odometry(drivers(), &chassis, &turret);
6476
SoundSubsystem sound(drivers());
6577

6678
// Command definitions -----------------------------------------------------------
67-
CommandSentryPosition sentryPositionCommand(drivers(), &chassis);
68-
CommandSentryAim sentryAimCommand(drivers(), &turret);
79+
CommandMoveChassisJoystick moveChassisCommandJoystick(drivers(), &chassis, &turret);
80+
CommandMoveChassisTurretRelativeJoystick moveChassisTurretRelativeCommandJoystick(
81+
drivers(),
82+
&chassis,
83+
&turret);
84+
CommandMoveChassisKeyboard moveChassisCommandKeyboard(drivers(), &chassis, &turret);
85+
CommandBeybladeChassisKeyboard beybladeChassisCommandKeyboard(drivers(), &chassis, &turret);
86+
87+
CommandAgitatorContinuous agitator1ContinuousCommand(drivers(), &agitator1, BarrelId::STANDARD1);
88+
CommandAgitatorContinuous agitator2ContinuousCommand(drivers(), &agitator2, BarrelId::STANDARD2);
89+
CommandUnjamAgitator unjamAgitator1Command(drivers(), &agitator1);
90+
CommandUnjamAgitator unjamAgitator2Command(drivers(), &agitator2);
91+
92+
CommandRotateFlywheel rotateFlywheelKeyboardCommand(drivers(), &flywheel);
93+
CommandRotateFlywheel rotateFlywheelNoAgitatorCommand(drivers(), &flywheel);
94+
CommandRotateFlywheel rotateFlywheelWithAgitatorCommand(drivers(), &flywheel);
95+
CommandFlywheelOff flywheelOffCommand(drivers(), &flywheel);
96+
97+
CommandMoveTurretJoystick moveTurretCommandJoystick(drivers(), &turret);
98+
CommandMoveTurretJoystick moveTurretWhenChassisIsTurretRelativeCommandJoystick(drivers(), &turret);
99+
CommandMoveTurretMouse moveTurretCommandMouse(drivers(), &turret);
100+
CommandMoveTurretAimbot moveTurretCommandAimbot(drivers(), &turret);
69101

70102
CommandPlaySound playStartupSoundCommand(drivers(), &sound, SOUND_STARTUP);
71103

104+
// Keyboard mappings ------------------------------------------------------------
105+
ToggleCommandMapping keyRToggled(
106+
drivers(),
107+
{&beybladeChassisCommandKeyboard},
108+
RemoteMapState({Remote::Key::R}));
109+
110+
ToggleCommandMapping keyGToggled(
111+
drivers(),
112+
{&rotateFlywheelKeyboardCommand},
113+
RemoteMapState({Remote::Key::G}));
114+
115+
HoldCommandMapping leftMouseDown(
116+
drivers(),
117+
{&agitator1ContinuousCommand, &agitator2ContinuousCommand},
118+
RemoteMapState(RemoteMapState::MouseButton::LEFT));
119+
120+
HoldCommandMapping keyXHeld(
121+
drivers(),
122+
{&unjamAgitator1Command, &unjamAgitator2Command},
123+
RemoteMapState({Remote::Key::X}));
124+
125+
HoldCommandMapping rightMouseDown(
126+
drivers(),
127+
{&moveTurretCommandAimbot},
128+
RemoteMapState(RemoteMapState::MouseButton::RIGHT));
129+
130+
// Joystick mappings ------------------------------------------------------------
131+
HoldCommandMapping rightSwitchUp(
132+
drivers(),
133+
{&moveChassisTurretRelativeCommandJoystick,
134+
&moveTurretWhenChassisIsTurretRelativeCommandJoystick},
135+
RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::UP));
136+
137+
HoldCommandMapping rightSwitchMid(
138+
drivers(),
139+
{&moveChassisCommandJoystick, &moveTurretCommandJoystick},
140+
RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::MID));
141+
142+
HoldCommandMapping rightSwitchDown(
143+
drivers(),
144+
{&flywheelOffCommand},
145+
RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::DOWN));
146+
147+
HoldCommandMapping leftSwitchMid(
148+
drivers(),
149+
{&rotateFlywheelNoAgitatorCommand},
150+
RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::MID));
151+
152+
HoldCommandMapping leftSwitchUp(
153+
drivers(),
154+
{&agitator1ContinuousCommand, &agitator2ContinuousCommand, &rotateFlywheelWithAgitatorCommand},
155+
RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP));
156+
72157
// Register subsystems here -----------------------------------------------
73-
void registerStandardSubsystems(src::Drivers *drivers)
158+
void registerSubsystems(src::Drivers *drivers)
74159
{
75160
drivers->commandScheduler.registerSubsystem(&chassis);
76161
drivers->commandScheduler.registerSubsystem(&agitator1);
@@ -96,8 +181,9 @@ void initializeSubsystems()
96181
// Set default commands here -----------------------------------------------
97182
void setDefaultCommands(src::Drivers *)
98183
{
99-
chassis.setDefaultCommand(&sentryPositionCommand);
100-
turret.setDefaultCommand(&sentryAimCommand);
184+
chassis.setDefaultCommand(&moveChassisCommandKeyboard);
185+
flywheel.setDefaultCommand(&flywheelOffCommand);
186+
turret.setDefaultCommand(&moveTurretCommandMouse);
101187
}
102188

103189
void runStartupCommands(src::Drivers *drivers)
@@ -106,15 +192,30 @@ void runStartupCommands(src::Drivers *drivers)
106192
}
107193

108194
// Register IO mappings here -----------------------------------------------
109-
void registerMappings(src::Drivers *drivers) {}
195+
void registerMappings(src::Drivers *drivers)
196+
{
197+
// Keyboard mappings ------------------------------------------------------------
198+
drivers->commandMapper.addMap(&keyRToggled);
199+
drivers->commandMapper.addMap(&leftMouseDown);
200+
drivers->commandMapper.addMap(&keyXHeld);
201+
drivers->commandMapper.addMap(&rightMouseDown);
202+
drivers->commandMapper.addMap(&keyGToggled);
203+
204+
// Joystick mappings ------------------------------------------------------------
205+
drivers->commandMapper.addMap(&rightSwitchUp);
206+
drivers->commandMapper.addMap(&rightSwitchMid);
207+
drivers->commandMapper.addMap(&rightSwitchDown);
208+
drivers->commandMapper.addMap(&leftSwitchMid);
209+
drivers->commandMapper.addMap(&leftSwitchUp);
210+
}
110211
} // namespace sentry_control
111212

112213
namespace control
113214
{
114215
void initSubsystemCommands(src::Drivers *drivers)
115216
{
116217
sentry_control::initializeSubsystems();
117-
sentry_control::registerStandardSubsystems(drivers);
218+
sentry_control::registerSubsystems(drivers);
118219
sentry_control::setDefaultCommands(drivers);
119220
sentry_control::runStartupCommands(drivers);
120221
sentry_control::registerMappings(drivers);

‎ut-robomaster/src/robots/standard/standard_constants.hpp

+6-11
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ static constexpr float WHEEL_RADIUS = 0.1524f; // meters
3131
static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f;
3232

3333
// flywheels -----------
34-
static constexpr int FLYWHEELS = 4;
34+
static constexpr int FLYWHEELS = 2;
3535

3636
// turret ------------
3737
constexpr float PITCH_MIN = -0.2185f; // rad
@@ -41,8 +41,8 @@ constexpr float NOZZLE_TO_PITCH = 0.18151f; // distance from barrel nozzle to
4141
constexpr float CAMERA_TO_BARRELS = 0.0427f; // vertical ctc offset from camera lens to barrel (m)
4242
constexpr float CAMERA_X_OFFSET = -0.0335f; // horizontal offset of main camera lens (m)
4343

44-
static constexpr float YAW_REDUCTION = 2.0f;
45-
static constexpr float PITCH_REDUCTION = 4.0f;
44+
static constexpr float YAW_REDUCTION = 1.0f;
45+
static constexpr float PITCH_REDUCTION = 1.0f;
4646

4747
// Tuning constants -----------------------------------------------
4848

@@ -108,17 +108,12 @@ const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel",
108108

109109
// flywheels
110110
const MotorConfig
111-
FLYWHEEL_TL{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel top left", PID_FLYWHEEL};
111+
FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel left", PID_FLYWHEEL};
112112
const MotorConfig
113-
FLYWHEEL_TR{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel top right", PID_FLYWHEEL};
114-
const MotorConfig
115-
FLYWHEEL_BL{M3508_NOGEARBOX, MOTOR5, CAN_SHOOTER, false, "flywheel bottom left", PID_FLYWHEEL};
116-
const MotorConfig
117-
FLYWHEEL_BR{M3508_NOGEARBOX, MOTOR6, CAN_SHOOTER, true, "flywheel bottom right", PID_FLYWHEEL};
113+
FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel right", PID_FLYWHEEL};
118114

119115
// agitator
120-
const MotorConfig AGITATOR_L{M2006, MOTOR1, CAN_SHOOTER, false, "agitator left", PID_AGITATOR};
121-
const MotorConfig AGITATOR_R{M2006, MOTOR2, CAN_SHOOTER, true, "agitator right", PID_AGITATOR};
116+
const MotorConfig AGITATOR{M2006, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR};
122117

123118
// turret
124119
const MotorConfig YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", PID_VELOCITY_DEFAULT};

‎ut-robomaster/src/robots/standard/standard_control.cpp

+10-18
Original file line numberDiff line numberDiff line change
@@ -68,8 +68,7 @@ namespace standard_control
6868

6969
// Subsystem definitions ---------------------------------------------------------
7070
ChassisSubsystem chassis(drivers());
71-
AgitatorSubsystem agitator1(drivers(), AGITATOR_L);
72-
AgitatorSubsystem agitator2(drivers(), AGITATOR_R);
71+
AgitatorSubsystem agitator(drivers(), AGITATOR);
7372
FlywheelSubsystem flywheel(drivers());
7473
TurretSubsystem turret(drivers());
7574
OdometrySubsystem odometry(drivers(), &chassis, &turret);
@@ -84,10 +83,8 @@ CommandMoveChassisTurretRelativeJoystick moveChassisTurretRelativeCommandJoystic
8483
CommandMoveChassisKeyboard moveChassisCommandKeyboard(drivers(), &chassis, &turret);
8584
CommandBeybladeChassisKeyboard beybladeChassisCommandKeyboard(drivers(), &chassis, &turret);
8685

87-
CommandAgitatorContinuous agitator1ContinuousCommand(drivers(), &agitator1, BarrelId::STANDARD1);
88-
CommandAgitatorContinuous agitator2ContinuousCommand(drivers(), &agitator2, BarrelId::STANDARD2);
89-
CommandUnjamAgitator unjamAgitator1Command(drivers(), &agitator1);
90-
CommandUnjamAgitator unjamAgitator2Command(drivers(), &agitator2);
86+
CommandAgitatorContinuous agitatorContinuousCommand(drivers(), &agitator, BarrelId::STANDARD1);
87+
CommandUnjamAgitator unjamAgitatorCommand(drivers(), &agitator);
9188

9289
CommandRotateFlywheel rotateFlywheelKeyboardCommand(drivers(), &flywheel);
9390
CommandRotateFlywheel rotateFlywheelNoAgitatorCommand(drivers(), &flywheel);
@@ -114,13 +111,10 @@ ToggleCommandMapping keyGToggled(
114111

115112
HoldCommandMapping leftMouseDown(
116113
drivers(),
117-
{&agitator1ContinuousCommand, &agitator2ContinuousCommand},
114+
{&agitatorContinuousCommand},
118115
RemoteMapState(RemoteMapState::MouseButton::LEFT));
119116

120-
HoldCommandMapping keyXHeld(
121-
drivers(),
122-
{&unjamAgitator1Command, &unjamAgitator2Command},
123-
RemoteMapState({Remote::Key::X}));
117+
HoldCommandMapping keyXHeld(drivers(), {&unjamAgitatorCommand}, RemoteMapState({Remote::Key::X}));
124118

125119
HoldCommandMapping rightMouseDown(
126120
drivers(),
@@ -151,15 +145,14 @@ HoldCommandMapping leftSwitchMid(
151145

152146
HoldCommandMapping leftSwitchUp(
153147
drivers(),
154-
{&agitator1ContinuousCommand, &agitator2ContinuousCommand, &rotateFlywheelWithAgitatorCommand},
148+
{&agitatorContinuousCommand, &rotateFlywheelWithAgitatorCommand},
155149
RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP));
156150

157151
// Register subsystems here -----------------------------------------------
158-
void registerStandardSubsystems(src::Drivers *drivers)
152+
void registerSubsystems(src::Drivers *drivers)
159153
{
160154
drivers->commandScheduler.registerSubsystem(&chassis);
161-
drivers->commandScheduler.registerSubsystem(&agitator1);
162-
drivers->commandScheduler.registerSubsystem(&agitator2);
155+
drivers->commandScheduler.registerSubsystem(&agitator);
163156
drivers->commandScheduler.registerSubsystem(&flywheel);
164157
drivers->commandScheduler.registerSubsystem(&turret);
165158
drivers->commandScheduler.registerSubsystem(&odometry);
@@ -170,8 +163,7 @@ void registerStandardSubsystems(src::Drivers *drivers)
170163
void initializeSubsystems()
171164
{
172165
chassis.initialize();
173-
agitator1.initialize();
174-
agitator2.initialize();
166+
agitator.initialize();
175167
flywheel.initialize();
176168
turret.initialize();
177169
odometry.initialize();
@@ -215,7 +207,7 @@ namespace control
215207
void initSubsystemCommands(src::Drivers *drivers)
216208
{
217209
standard_control::initializeSubsystems();
218-
standard_control::registerStandardSubsystems(drivers);
210+
standard_control::registerSubsystems(drivers);
219211
standard_control::setDefaultCommands(drivers);
220212
standard_control::runStartupCommands(drivers);
221213
standard_control::registerMappings(drivers);

‎ut-robomaster/src/subsystems/flywheel/command_rotate_flywheel.cpp

+10-35
Original file line numberDiff line numberDiff line change
@@ -9,52 +9,27 @@ void CommandRotateFlywheel::execute()
99
{
1010
if (drivers->refSerial.getRefSerialReceivingData())
1111
{
12-
#if defined(TARGET_STANDARD) || defined(TARGET_SENTRY)
13-
uint16_t refSystemLaunchSpeedLeft =
14-
drivers->refSerial.getRobotData().turret.barrelSpeedLimit17ID1;
15-
uint16_t refSystemLaunchSpeedRight =
16-
drivers->refSerial.getRobotData().turret.barrelSpeedLimit17ID2;
17-
18-
float launchSpeedLeft = 0.0f;
19-
float launchSpeedRight = 0.0f;
20-
21-
for (int i = 0; i < 3; i++)
22-
{
23-
if (FLYWHEEL_RPS_MAPPING[i].getFirst() == refSystemLaunchSpeedLeft)
24-
{
25-
launchSpeedLeft = FLYWHEEL_RPS_MAPPING[i].getSecond();
26-
}
27-
28-
if (FLYWHEEL_RPS_MAPPING[i].getFirst() == refSystemLaunchSpeedRight)
29-
{
30-
launchSpeedRight = FLYWHEEL_RPS_MAPPING[i].getSecond();
31-
}
32-
33-
if (launchSpeedLeft != 0.0f && launchSpeedRight != 0.0f)
34-
{
35-
break;
36-
}
37-
}
38-
39-
flywheel->setLaunchSpeed(launchSpeedLeft, launchSpeedRight);
12+
auto turretData = drivers->refSerial.getRobotData().turret;
4013

14+
#if defined(TARGET_STANDARD) || defined(TARGET_SENTRY)
15+
uint16_t speedLimit = turretData.barrelSpeedLimit17ID1;
4116
#elif defined(TARGET_HERO)
42-
uint16_t refSystemLaunchSpeed = drivers->refSerial.getRobotData().turret.barrelSpeedLimit42;
43-
float launchSpeedHero = 0.0f;
17+
uint16_t speedLimit = turretData.barrelSpeedLimit42;
18+
#endif
19+
20+
float launchSpeed = 0.0f;
4421

4522
for (int i = 0; i < 3; i++)
4623
{
47-
if (FLYWHEEL_RPS_MAPPING[i].getFirst() == refSystemLaunchSpeed)
24+
if (FLYWHEEL_RPS_MAPPING[i].getFirst() == speedLimit)
4825
{
49-
launchSpeedHero = FLYWHEEL_RPS_MAPPING[i].getSecond();
26+
launchSpeed = FLYWHEEL_RPS_MAPPING[i].getSecond();
5027
break;
5128
}
5229
}
5330

54-
flywheel->setLaunchSpeed(launchSpeedHero);
55-
#endif
31+
flywheel->setLaunchSpeed(launchSpeed);
5632
}
57-
5833
else
5934
{
6035
// NO REF SYSTEM DATA

0 commit comments

Comments
 (0)