Torque Control

control-board/src/bin/hwtest-motor/main.rs

@@ -123,16 +123,27 @@ async fn main(main_spawner: embassy_executor::Spawner) {
         p
     );
 
+    let mut vel: f32 = 0.0;
+    let mut ctr: usize = 0;
     loop {
         Timer::after_millis(100).await;
 
+        ctr += 1;
+        if ctr > 50 {
+            ctr = 0;
+        } else if ctr > 25 {
+            vel = 2.0;
+        } else {
+            vel = 0.0;
+        }
+
         test_command_publisher
             .publish(DataPacket::BasicControl(BasicControl {
                 _bitfield_1: Default::default(),
                 _bitfield_align_1: Default::default(),
-                vel_x_linear: 1.0,
+                vel_x_linear: 0.0,
                 vel_y_linear: 0.0,
-                vel_z_angular: 0.0,
+                vel_z_angular: vel * 10.0,
                 kick_vel: 0.0,
                 dribbler_speed: 10.0,
                 kick_request: KickRequest::KR_DISABLE,

control-board/src/stspin_motor.rs

@@ -33,10 +33,9 @@ pub struct WheelMotor<
     stm32_uart_interface:
         Stm32Interface<'a, LEN_RX, LEN_TX, DEPTH_RX, DEPTH_TX, DEBUG_MOTOR_UART_QUEUES>,
     firmware_image: &'a [u8],
-
+    current_timestamp_ms: u32,
     current_state: MotorTelemetry,
     current_params_state: ParameterMotorResponse,
-
     version_major: u8,
     version_minor: u8,
     version_patch: u16,
@@ -50,6 +49,8 @@ pub struct WheelMotor<
     motion_type: MotionCommandType::Type,
     reset_flagged: bool,
     telemetry_enabled: bool,
+    motion_enabled: bool,
+    calibrate_current: bool,
 }
 
 impl<
@@ -81,6 +82,7 @@ impl<
             version_major: 0,
             version_minor: 0,
             version_patch: 0,
+            current_timestamp_ms: 0,
             current_state: start_state,
             current_params_state: start_params_state,
             vel_pid_constants: Vector3::new(0.0, 0.0, 0.0),
@@ -93,6 +95,8 @@ impl<
             motion_type: OPEN_LOOP,
             reset_flagged: false,
             telemetry_enabled: false,
+            motion_enabled: false,
+            calibrate_current: false,
         }
     }
 
@@ -125,6 +129,7 @@ impl<
             version_major: 0,
             version_minor: 0,
             version_patch: 0,
+            current_timestamp_ms: 0,
             current_state: start_state,
             current_params_state: start_params_state,
             vel_pid_constants: Vector3::new(0.0, 0.0, 0.0),
@@ -137,6 +142,8 @@ impl<
             motion_type: OPEN_LOOP,
             reset_flagged: false,
             telemetry_enabled: false,
+            motion_enabled: false,
+            calibrate_current: false,
         }
     }
 
@@ -216,6 +223,7 @@ impl<
                 res = Err(());
             }
         }
+
         // Make sure that the uart queue is empty of any possible parameter
         // response packets, which may cause side effects for the flashing
         // process
@@ -274,6 +282,16 @@ impl<
         return self.init_firmware_image(flash, self.firmware_image).await;
     }
 
+    pub async fn save_motor_current_constants(&mut self, current_constant: f32) -> Result<(), ()> {
+        defmt::debug!(
+            "Drive Motor - Saving motor current constant: {:?}",
+            current_constant
+        );
+        self.stm32_uart_interface
+            .write_current_calibration_constants(current_constant)
+            .await
+    }
+
     pub fn process_packets(&mut self) {
         while let Ok(res) = self.stm32_uart_interface.try_read_data() {
             let buf = res.data();
@@ -424,6 +442,18 @@ impl<
         self.telemetry_enabled = telemetry_enabled;
     }
 
+    pub fn set_motion_enabled(&mut self, enabled: bool) {
+        self.motion_enabled = enabled;
+    }
+
+    pub fn set_calibrate_current(&mut self, calibrate_current: bool) {
+        self.calibrate_current = calibrate_current;
+    }
+
+    pub fn read_current_timestamp_ms(&self) -> u32 {
+        return self.current_timestamp_ms;
+    }
+
     pub fn read_is_error(&self) -> bool {
         return self.current_state.master_error() != 0;
     }
@@ -434,10 +464,6 @@ impl<
             || self.current_state.hall_enc_vel_disagreement_error() != 0;
     }
 
-    pub fn read_current(&self) -> f32 {
-        return self.current_state.current_estimate;
-    }
-
     pub fn read_encoder_delta(&self) -> i32 {
         return self.current_state.encoder_delta;
     }
@@ -454,7 +480,35 @@ impl<
         return self.current_state.vel_setpoint;
     }
 
-    pub fn read_vel_computed_setpoint(&self) -> f32 {
-        return self.current_state.vel_computed_setpoint;
+    pub fn read_vel_computed_duty(&self) -> f32 {
+        return self.current_state.vel_computed_duty;
+    }
+
+    pub fn read_current(&self) -> f32 {
+        return self.current_state.current_estimate;
+    }
+
+    pub fn read_torque_setpoint(&self) -> f32 {
+        return self.current_state.torque_setpoint;
+    }
+
+    pub fn read_torque_estimate(&self) -> f32 {
+        return self.current_state.torque_estimate;
+    }
+
+    pub fn read_torque_computed_error(&self) -> f32 {
+        return self.current_state.torque_computed_error;
+    }
+
+    pub fn read_torque_computed_nm(&self) -> f32 {
+        return self.current_state.torque_computed_nm;
+    }
+
+    pub fn read_torque_computed_duty(&self) -> f32 {
+        return self.current_state.torque_computed_duty;
+    }
+
+    pub fn read_vbus_voltage(&self) -> f32 {
+        return self.current_state.vbus_voltage;
     }
 }

control-board/src/tasks/control_task.rs

@@ -29,9 +29,9 @@ use crate::{
 
 include_external_cpp_bin! {WHEEL_FW_IMG, "wheel.bin"}
 
-const MAX_TX_PACKET_SIZE: usize = 60;
+const MAX_TX_PACKET_SIZE: usize = 80;
 const TX_BUF_DEPTH: usize = 3;
-const MAX_RX_PACKET_SIZE: usize = 60;
+const MAX_RX_PACKET_SIZE: usize = 80;
 const RX_BUF_DEPTH: usize = 20;
 
 type ControlWheelMotor =
@@ -378,7 +378,7 @@ impl<
                             self.last_command.wheel_vel_control_enabled() != 0,
                             self.last_command.wheel_torque_control_enabled() != 0,
                         ) {
-                            (true, true) => MotionCommandType::BOTH,
+                            (true, true) => MotionCommandType::VELOCITY_W_TORQUE,
                             (true, false) => MotionCommandType::VELOCITY,
                             (false, true) => MotionCommandType::TORQUE,
                             (false, false) => MotionCommandType::OPEN_LOOP,
@@ -459,8 +459,20 @@ impl<
             self.motor_br.set_setpoint(wheel_vels[2]);
             self.motor_fr.set_setpoint(wheel_vels[3]);
 
-            // defmt::info!("wheel vels: {} {} {} {}", self.motor_fl.read_encoder_delta(), self.motor_bl.read_encoder_delta(), self.motor_br.read_encoder_delta(), self.motor_fr.read_encoder_delta());
-            // defmt::info!("wheel curr: {} {} {} {}", self.motor_fl.read_current(), self.motor_bl.read_current(), self.motor_br.read_current(), self.motor_fr.read_current());
+            defmt::info!(
+                "wheel vels: {} {} {} {}",
+                self.motor_fl.read_encoder_delta(),
+                self.motor_bl.read_encoder_delta(),
+                self.motor_br.read_encoder_delta(),
+                self.motor_fr.read_encoder_delta()
+            );
+            defmt::info!(
+                "wheel curr: {} {} {} {}",
+                self.motor_fl.read_current(),
+                self.motor_bl.read_current(),
+                self.motor_br.read_current(),
+                self.motor_fr.read_current()
+            );
 
             ///////////////////////////////////
             //  send commands and telemetry  //

control-board/src/tasks/imu_task.rs

@@ -189,8 +189,7 @@ async fn imu_task_entry(
                         } else {
                             // After the first tipped is seen, then wait if it has been tipped for long enough.
                             let cur_time = Instant::now();
-                            if Instant::checked_duration_since(&cur_time, first_tipped_check_time)
-                                .unwrap()
+                            if Instant::duration_since(&cur_time, first_tipped_check_time)
                                 .as_millis()
                                 > TIPPED_MIN_DURATION_MS
                             {

control-board/src/tasks/kicker_task.rs

@@ -143,9 +143,7 @@ impl<
             } else if self.kicker_task_state >= KickerTaskState::ConnectUart {
                 // Check if connection timeout occurred
                 let time_elapsed =
-                    Instant::checked_duration_since(&Instant::now(), connection_timeout_start)
-                        .unwrap()
-                        .as_millis();
+                    Instant::duration_since(&Instant::now(), connection_timeout_start).as_millis();
                 if time_elapsed > TELEMETRY_TIMEOUT_MS {
                     defmt::error!("Kicker Interface - Kicker telemetry timed out, current state is '{}', rolling state back to 'Reset'", self.kicker_task_state);
                     self.kicker_task_state = KickerTaskState::Reset;
@@ -185,6 +183,8 @@ impl<
                     );
                     self.kicker_driver.reset().await;
                     self.kicker_task_state = KickerTaskState::InitFirmware;
+
+                    main_loop_ticker.reset();
                 }
                 KickerTaskState::InitFirmware => {
                     // Ensure firmware image is up to date
@@ -222,6 +222,8 @@ impl<
                     connection_timeout_start = Instant::now();
                     // Move on to ConnectUart state
                     self.kicker_task_state = KickerTaskState::Connected;
+
+                    main_loop_ticker.reset();
                 }
                 KickerTaskState::ConnectUart => {
                     if telemetry_received {

control-board/src/tasks/radio_task.rs

@@ -392,9 +392,7 @@ impl<
                     // reboot the robot (unless we had a shutdown request).
                     let cur_time = Instant::now();
                     if !cur_robot_state.shutdown_requested
-                        && Instant::checked_duration_since(&cur_time, self.last_software_packet)
-                            .unwrap()
-                            .as_millis()
+                        && Instant::duration_since(&cur_time, self.last_software_packet).as_millis()
                             > Self::RESPONSE_FROM_PC_TIMEOUT_MS
                     {
                         defmt::warn!("software timeout - rebooting...");
@@ -425,8 +423,10 @@ impl<
 
             let loop_end_time = Instant::now();
             let loop_execution_time = loop_end_time - loop_start_time;
-            if loop_execution_time > Duration::from_millis(2) {
-                defmt::warn!("radio loop is taking >2ms to complete (it may be interrupted by higher priority tasks). This is >20% of an execution frame. Loop execution time {:?}", loop_execution_time);
+            if loop_execution_time > Duration::from_millis(2)
+                && self.connection_state == RadioConnectionState::Connected
+            {
+                defmt::warn!("radio loop is connected and taking >2ms to complete (it may be interrupted by higher priority tasks). This is >20% of an execution frame. Loop execution time {:?}", loop_execution_time);
             }
 
             last_loop_term_time = Instant::now();