grSim Control Layer Abstraction (#2004)

* Add wheels to controls

* Add draft stop function

* Add rough interface for firmware_robot and force/velocity wheels

* Add basic per wheel and local velociy (still needs implementation)

* Further implement firmware_robot and integrate controls

* Move robot_constants to its own file and change signatures to applyAccel / trackRobotInFrame accordingly

* Add position trajectory to firmware_robot

* Add robot constants and most of trajectory follower

* MOre fixes

* Get firmware_robot compiling

* Delete files gone from merge confs

* Replace some app_wheel_create instances

* Temporarily reintroduce wheel getters, corner_kick_play_test works

* Fix some unit tests

* Minor fix to main.c

* Fix simulator_robot_singleton test and formatting

* Add app_firmware_robot_destroy for force wheels

* Fix velocity wheel robot destroy

* Fix free error whoops

* Move move_plan_rotation away and fix main.c

* Add wheel destroyers as separate functions

* Clean up firmware robot

* Clean up javadocs

* Address PR comments

* Address wheel array comments

* Fix  force wheels setLocalVelocity

Author: kelvinkoon

src/firmware/app/control/BUILD

@@ -75,7 +75,8 @@ cc_library(
     srcs = ["control.c"],
     hdrs = ["control.h"],
     deps = [
-        "//firmware/app/world:firmware_robot",
+        "//firmware/app/world:firmware_robot_constants",
+        "//firmware/app/world:force_wheel",
         "//firmware/shared:physics",
         "//firmware/shared:util",
     ],
@@ -87,7 +88,6 @@ cc_library(
     hdrs = ["physbot.h"],
     deps = [
         ":bangbang",
-        "//firmware/app/world:firmware_robot",
         "//firmware/shared:physics",
     ],
 )

src/firmware/app/control/control.c

@@ -10,14 +10,14 @@
  * Note that this could scale the forces *down* if they exceed the physical capabilities
  * of the robot (ex. wheel slip).
  *
- * @param wheels Wheels to compute the maximum torque scaling from
+ * @param force_wheels Force wheels to compute the maximum torque scaling from
  * @param wheel_forces Forces to apply to each wheel
  * @param battery_voltage The current battery voltage
  *
  * @return The amount by which to scale the force on each wheel to get the maximum
  *         torque possible while maintaining the same torque ratio between wheels
  */
-float app_control_getMaximalTorqueScaling(const Wheel_t* wheels[4],
+float app_control_getMaximalTorqueScaling(const ForceWheel_t* force_wheels[4],
                                           const float wheel_forces[4],
                                           float battery_voltage)
 {
@@ -26,12 +26,12 @@ float app_control_getMaximalTorqueScaling(const Wheel_t* wheels[4],
 
     for (long i = 0; i < 4; i++)
     {
-        const Wheel_t* wheel             = wheels[i];
-        const WheelConstants_t constants = app_wheel_getWheelConstants(wheel);
-        float force                      = wheel_forces[i];
+        const ForceWheel_t* wheel             = force_wheels[i];
+        const ForceWheelConstants_t constants = app_force_wheel_getWheelConstants(wheel);
+        float force                           = wheel_forces[i];
         float motor_torque =
             force * constants.wheel_radius * constants.wheel_rotations_per_motor_rotation;
-        float curr_motor_rpm = app_wheel_getMotorSpeedRPM(wheel);
+        float curr_motor_rpm = app_force_wheel_getMotorSpeedRPM(wheel);
 
         float resistive_voltage_loss = motor_torque *
                                        constants.motor_current_per_unit_torque *
@@ -62,20 +62,22 @@ float app_control_getMaximalTorqueScaling(const Wheel_t* wheels[4],
  * Note that this could scale the accelerations *down* if the given forces exceed the
  * physical capabilities of the robot (ex. wheel slip).
  *
- * @param robot The robot to compute the acceleration scaling constant for
+ * @param robot_constants The robot constants representing the robot
+ * @param battery_voltage The robot's battery voltage
+ * @param force_wheels The robot's force wheels
  * @param linear_accel_x [m/s^2] The linear x acceleration to scale
  * @param linear_accel_y [m/s^2] The linear y acceleration to scale
  * @param angular_accel [rad/s^2] The angular acceleration to scale
  *
  * @return The scaling constant to multiply the all acceleration values by in order to
  *         accelerate the robot as fast as physically possible
  */
-float app_control_getMaximalAccelScaling(const FirmwareRobot_t* robot,
+float app_control_getMaximalAccelScaling(const RobotConstants_t robot_constants,
+                                         float battery_voltage,
+                                         const ForceWheel_t* force_wheels[4],
                                          const float linear_accel_x,
                                          const float linear_accel_y, float angular_accel)
 {
-    const RobotConstants_t robot_constants = app_firmware_robot_getRobotConstants(robot);
-
     // first convert accelerations into consistent units
     // choose units of Force (N)
     float normed_force[3];
@@ -87,25 +89,25 @@ float app_control_getMaximalAccelScaling(const FirmwareRobot_t* robot,
     float wheel_forces[4];
     force3_to_force4(normed_force, wheel_forces);
 
-    const Wheel_t* wheels[4];
-    wheels[0] = app_firmware_robot_getFrontLeftWheel(robot);
-    wheels[1] = app_firmware_robot_getBackLeftWheel(robot);
-    wheels[2] = app_firmware_robot_getBackRightWheel(robot);
-    wheels[3] = app_firmware_robot_getFrontRightWheel(robot);
-
-    float battery_voltage = app_firmware_robot_getBatteryVoltage(robot);
-
-    return app_control_getMaximalTorqueScaling(wheels, wheel_forces, battery_voltage);
+    return app_control_getMaximalTorqueScaling(force_wheels, wheel_forces,
+                                               battery_voltage);
 }
 
-void app_control_applyAccel(const FirmwareRobot_t* robot, float linear_accel_x,
+void app_control_applyAccel(RobotConstants_t robot_constants,
+                            ControllerState_t* controller_state, float battery_voltage,
+                            ForceWheel_t* force_wheels[4], float linear_accel_x,
                             float linear_accel_y, float angular_accel)
 {
-    const RobotConstants_t robot_constants = app_firmware_robot_getRobotConstants(robot);
+    const ForceWheel_t* wheels[4];
+    wheels[0] = force_wheels[0];
+    wheels[1] = force_wheels[1];
+    wheels[2] = force_wheels[2];
+    wheels[3] = force_wheels[3];
 
     // check for max acceleration in direction of the vel difference
-    float scaling = app_control_getMaximalAccelScaling(robot, linear_accel_x,
-                                                       linear_accel_y, angular_accel);
+    float scaling =
+        app_control_getMaximalAccelScaling(robot_constants, battery_voltage, wheels,
+                                           linear_accel_x, linear_accel_y, angular_accel);
 
     // if the (very naive) 1 tick acceleration violates the physical limits of the robot
     // scale it to maximum
@@ -117,10 +119,9 @@ void app_control_applyAccel(const FirmwareRobot_t* robot, float linear_accel_x,
         angular_accel *= scaling;
     }
 
-    ControllerState_t* controller_state = app_firmware_robot_getControllerState(robot);
-    float prev_linear_accel_x           = controller_state->last_applied_acceleration_x;
-    float prev_linear_accel_y           = controller_state->last_applied_acceleration_y;
-    float prev_angular_accel = controller_state->last_applied_acceleration_angular;
+    float prev_linear_accel_x = controller_state->last_applied_acceleration_x;
+    float prev_linear_accel_y = controller_state->last_applied_acceleration_y;
+    float prev_angular_accel  = controller_state->last_applied_acceleration_angular;
 
     float linear_diff_x = linear_accel_x - prev_linear_accel_x;
     float linear_diff_y = linear_accel_y - prev_linear_accel_y;
@@ -151,41 +152,8 @@ void app_control_applyAccel(const FirmwareRobot_t* robot, float linear_accel_x,
     float wheel_force[4];
     speed3_to_speed4(robot_force, wheel_force);  // Convert to wheel coordinate system
 
-    app_wheel_applyForce(app_firmware_robot_getFrontLeftWheel(robot), wheel_force[0]);
-    app_wheel_applyForce(app_firmware_robot_getFrontRightWheel(robot), wheel_force[3]);
-    app_wheel_applyForce(app_firmware_robot_getBackLeftWheel(robot), wheel_force[1]);
-    app_wheel_applyForce(app_firmware_robot_getBackRightWheel(robot), wheel_force[2]);
-}
-
-void app_control_trackVelocityInRobotFrame(FirmwareRobot_t* robot,
-                                           float linear_velocity_x,
-                                           float linear_velocity_y,
-                                           float angular_velocity)
-{
-    float current_vx               = app_firmware_robot_getVelocityX(robot);
-    float current_vy               = app_firmware_robot_getVelocityY(robot);
-    float current_angular_velocity = app_firmware_robot_getVelocityAngular(robot);
-    float current_orientation      = app_firmware_robot_getOrientation(robot);
-
-    // Rotate the current_velocity vector from the world frame to the robot frame
-    float current_velocity[2];
-    current_velocity[0] = current_vx;
-    current_velocity[1] = current_vy;
-    rotate(current_velocity, -current_orientation);
-
-    // This is the "P" term in a PID controller. We essentially do proportional
-    // control of our acceleration based on velocity error
-    static const float VELOCITY_ERROR_GAIN = 10.0f;
-
-    float desired_acceleration[2];
-    desired_acceleration[0] =
-        (linear_velocity_x - current_velocity[0]) * VELOCITY_ERROR_GAIN;
-    desired_acceleration[1] =
-        (linear_velocity_y - current_velocity[1]) * VELOCITY_ERROR_GAIN;
-
-    float angular_acceleration =
-        (angular_velocity - current_angular_velocity) * VELOCITY_ERROR_GAIN;
-
-    app_control_applyAccel(robot, desired_acceleration[0], desired_acceleration[1],
-                           angular_acceleration);
+    app_force_wheel_applyForce(force_wheels[0], wheel_force[0]);
+    app_force_wheel_applyForce(force_wheels[3], wheel_force[3]);
+    app_force_wheel_applyForce(force_wheels[1], wheel_force[1]);
+    app_force_wheel_applyForce(force_wheels[2], wheel_force[2]);
 }

src/firmware/app/control/control.h

@@ -1,31 +1,23 @@
 #pragma once
 
-#include "firmware/app/world/firmware_robot.h"
+#include "firmware/app/world/firmware_robot_constants.h"
+#include "firmware/app/world/force_wheel.h"
 
 /**
  * Apply the given acceleration (in robot coordinates) to the given robot
  *
- * @param robot The robot to apply acceleration to
+ * @param robot_constants The robot constants representing the robot
+ * @param controller_state The controller state representing the robot
+ * @param battery_voltage The robot's battery voltage
+ * @param force_wheels The robot's force wheels (front_left, back_left, back_right,
+ * front_right in order)
  * @param linear_accel_x The linear acceleration, in robot coordinates, in the x
  *                       (forward/backwards) direction (m/s^2)
  * @param linear_accel_y The linear acceleration, in robot coordinates, in the y
  *                       (side-to-side) direction (m/s^2)
  * @param angular_accel The angular acceleration to apply to the robot (rad/s^2)
  */
-void app_control_applyAccel(const FirmwareRobot_t* robot, float linear_accel_x,
+void app_control_applyAccel(const RobotConstants_t robot_constants,
+                            ControllerState_t* controller_state, float battery_voltage,
+                            ForceWheel_t* force_wheels[4], float linear_accel_x,
                             float linear_accel_y, float angular_accel);
-
-/**
- * Drive the given robot at the given velocity in the robot coordinate frame
- *
- * @param robot The robot to move at the given velocity
- * @param linear_velocity_x The velocity to move at in the x-direction in the robot
- *                          coordinate frame (m/s)
- * @param linear_velocity_y The velocity to move at in the y-direction in the robot
- *                          coordinate frame (m/s)
- * @param angular_velocity The angular velocity to move at (rad/s)
- */
-void app_control_trackVelocityInRobotFrame(FirmwareRobot_t* robot,
-                                           float linear_velocity_x,
-                                           float linear_velocity_y,
-                                           float angular_velocity);

src/firmware/app/control/physbot.c

@@ -5,13 +5,12 @@
 
 #define TIME_HORIZON 0.05f  // s
 
-PhysBot app_physbot_create(const FirmwareRobot_t *robot, float *destination,
+PhysBot app_physbot_create(float velocity_x, float velocity_y, float position_x,
+                           float position_y, float orientation, float *destination,
                            float *major_vec, float *minor_vec)
 {
-    float v[2]            = {app_firmware_robot_getVelocityX(robot),
-                  app_firmware_robot_getVelocityY(robot)};
-    float dr[2]           = {destination[0] - app_firmware_robot_getPositionX(robot),
-                   destination[1] - app_firmware_robot_getPositionY(robot)};
+    float v[2]            = {velocity_x, velocity_y};
+    float dr[2]           = {destination[0] - position_x, destination[1] - position_y};
     const Component major = {
         .disp = dot2D(major_vec, dr), .vel = dot2D(major_vec, v), .accel = 0, .time = 0};
     const Component minor = {
@@ -20,8 +19,7 @@ PhysBot app_physbot_create(const FirmwareRobot_t *robot, float *destination,
     // current orientation is directly subtracted from destination because we do not
     // assume that the user wants the rotational displacement to be the min angle between
     // the destination and current orientation
-    const Component rot = {.disp =
-                               destination[2] - app_firmware_robot_getOrientation(robot)};
+    const Component rot = {.disp = destination[2] - orientation};
     PhysBot pb          = {.rot = rot, .maj = major, .min = minor};
     for (unsigned i = 0; i < 2; i++)
     {

src/firmware/app/control/physbot.h

@@ -1,7 +1,5 @@
 #pragma once
 
-#include "firmware/app/world/firmware_robot.h"
-
 /**
  * component to build information along major axis, minor axis, or rotation.
  * disp is the displacement along that axis.
@@ -47,14 +45,19 @@ typedef struct
  * as well as data about the bot on the global axis. Details about what
  * can go in the PhysBot are in the physbot.h file.
  *
- * @param robot The current state of the robot
+ * @param velocity_x the robot's x component of velocity
+ * @param velocity_y the robot's y component of velocity
+ * @param position_x the robot's x component of position
+ * @param position_y the robot's y component of position
+ * @param orientation the robot's orientation
  * @param destination a 3 length array of {x, y, rotation} destination values
  * on the global axis
  * @param major_vec the major vector components on the global axis
  * @param minor_vec the minor vector components on the global axis
  * @return a new PhysBot data container
  */
-PhysBot app_physbot_create(const FirmwareRobot_t *robot, float *destination,
+PhysBot app_physbot_create(float velocity_x, float velocity_y, float position_x,
+                           float position_y, float orientation, float *destination,
                            float *major_vec, float *minor_vec);
 
 /**

src/firmware/app/primitives/direct_control_primitive.c

@@ -31,14 +31,7 @@ void app_direct_control_primitive_start(TbotsProto_DirectControlPrimitive prim_m
                 prim_msg.wheel_control.direct_per_wheel_control;
             state->direct_velocity = false;
             // TODO (#1649): Fix passing rpm into an applyForce function
-            app_wheel_applyForce(app_firmware_robot_getFrontLeftWheel(robot),
-                                 control_msg.front_left_wheel_rpm);
-            app_wheel_applyForce(app_firmware_robot_getBackLeftWheel(robot),
-                                 control_msg.back_left_wheel_rpm);
-            app_wheel_applyForce(app_firmware_robot_getBackRightWheel(robot),
-                                 control_msg.back_right_wheel_rpm);
-            app_wheel_applyForce(app_firmware_robot_getFrontRightWheel(robot),
-                                 control_msg.front_right_wheel_rpm);
+            app_firmware_robot_applyDirectPerWheelPower(robot, control_msg);
             break;
         }
         case TbotsProto_DirectControlPrimitive_direct_velocity_control_tag:
@@ -122,9 +115,9 @@ static void app_direct_control_primitive_tick(void* void_state_ptr,
     if (state->direct_velocity)
     {
         FirmwareRobot_t* robot = app_firmware_world_getRobot(world);
-        app_control_trackVelocityInRobotFrame(robot, state->direct_target_velocity_x,
-                                              state->direct_target_velocity_y,
-                                              state->direct_target_velocity_angular);
+        app_firmware_robot_trackVelocityInRobotFrame(
+            robot, state->direct_target_velocity_x, state->direct_target_velocity_y,
+            state->direct_target_velocity_angular);
     }
 }
 

src/firmware/app/primitives/move_primitive.c

@@ -35,28 +35,6 @@ typedef struct MoveState
 } MoveState_t;
 DEFINE_PRIMITIVE_STATE_CREATE_AND_DESTROY_FUNCTIONS(MoveState_t);
 
-/**
- * Determines the rotation acceleration after setup_bot has been used and
- * plan_move has been done along the minor axis. The minor time from bangbang
- * is used to determine the rotation time, and thus the rotation velocity and
- * acceleration. The rotational acceleration is clamped under the MAX_T_A.
- *
- * @param pb [in/out] The PhysBot data container that should have minor axis time and
- * will store the rotational information
- * @param avel The rotational velocity of the bot
- */
-void plan_move_rotation(PhysBot* pb, float avel);
-
-void plan_move_rotation(PhysBot* pb, float avel)
-{
-    pb->rot.time = (pb->min.time > TIME_HORIZON) ? pb->min.time : TIME_HORIZON;
-    // 1.4f is a magic constant to force the robot to rotate faster to its final
-    // orientation.
-    pb->rot.vel   = 1.4f * pb->rot.disp / pb->rot.time;
-    pb->rot.accel = (pb->rot.vel - avel) / TIME_HORIZON;
-    limit(&pb->rot.accel, MAX_T_A);
-}
-
 void app_move_primitive_start(TbotsProto_MovePrimitive prim_msg, void* void_state_ptr,
                               FirmwareWorld_t* world)
 {
@@ -159,50 +137,8 @@ static void app_move_primitive_tick(void* void_state_ptr, FirmwareWorld_t* world
         trajectory_index++;
     }
 
-    const float dest_x = state->position_trajectory.x_position[trajectory_index];
-    const float dest_y = state->position_trajectory.y_position[trajectory_index];
-    const float dest_orientation =
-        state->position_trajectory.orientation[trajectory_index];
-    float dest[3] = {dest_x, dest_y, dest_orientation};
-
-    const float curr_x = app_firmware_robot_getPositionX(robot);
-    const float curr_y = app_firmware_robot_getPositionY(robot);
-
-    const float dx = dest_x - curr_x;
-    const float dy = dest_y - curr_y;
-
-    float total_disp = sqrtf(dx * dx + dy * dy);
-    // Add a small number to avoid division by zero
-    float major_vec[2] = {dx / (total_disp + 1e-6f), dy / (total_disp + 1e-6f)};
-    float minor_vec[2] = {major_vec[0], major_vec[1]};
-    rotate(minor_vec, P_PI / 2);
-
-    PhysBot pb = app_physbot_create(robot, dest, major_vec, minor_vec);
-
-    const float dest_speed = state->position_trajectory.linear_speed[trajectory_index];
-
-    // plan major axis movement
-    float max_major_a     = (float)ROBOT_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED;
-    float max_major_v     = state->max_speed_m_per_s;
-    float major_params[3] = {dest_speed, max_major_a, max_major_v};
-    app_physbot_planMove(&pb.maj, major_params);
-
-    // plan minor axis movement
-    float max_minor_a = (float)ROBOT_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED / 2.0f;
-    float max_minor_v = state->max_speed_m_per_s / 2.0f;
-    float minor_params[3] = {0, max_minor_a, max_minor_v};
-    app_physbot_planMove(&pb.min, minor_params);
-
-    // plan rotation movement
-    plan_move_rotation(&pb, app_firmware_robot_getVelocityAngular(robot));
-
-    float accel[3] = {0, 0, pb.rot.accel};
-
-    // rotate the accel and apply it
-    app_physbot_computeAccelInLocalCoordinates(
-        accel, pb, app_firmware_robot_getOrientation(robot), major_vec, minor_vec);
-
-    app_control_applyAccel(robot, accel[0], accel[1], accel[2]);
+    app_firmware_robot_followPosTrajectory(robot, state->position_trajectory,
+                                           trajectory_index, state->max_speed_m_per_s);
 }
 
 /**