Initial current sensing reworking

control-board/.cargo/config.toml

@@ -8,4 +8,4 @@ runner = 'probe-rs run --preverify --chip STM32H723ZGTx'
 
 [env]
 # DEFMT_LOG = "error,ateam-control-board::tasks::control_task=trace"
-DEFMT_LOG="info"
+DEFMT_LOG="trace"

control-board/Cargo.toml

@@ -26,7 +26,7 @@ embassy-time = { version = "0.4.0", features = [
 embassy-stm32 = { version = "0.3.0", features = [
     "defmt",
     "stm32h723zg",
-    "unstable-pac", 
+    "unstable-pac",
     "time-driver-tim1",
     "exti",
     "chrono"
@@ -124,6 +124,11 @@ name = "hwtest-radio"
 test = false
 harness = false
 
+[[bin]]
+name = "calibrate-motor"
+test = false
+harness = false
+
 [[test]]
 name = "basic-test"
 harness = false
@@ -140,4 +145,3 @@ embassy-sync = { git = "https://github.com/embassy-rs/embassy"}
 embassy-time = { git = "https://github.com/embassy-rs/embassy"}
 embassy-stm32 = { git = "https://github.com/embassy-rs/embassy"}
 embassy-futures = { git = "https://github.com/embassy-rs/embassy"}
-

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

@@ -0,0 +1,127 @@
+#![no_std]
+#![no_main]
+
+use embassy_executor::InterruptExecutor;
+use embassy_stm32::{
+    interrupt, pac::Interrupt
+};
+use embassy_sync::pubsub::PubSubChannel;
+
+use defmt_rtt as _;
+
+use ateam_control_board::{create_motor_calibrate_task, create_imu_task, create_io_task, create_kicker_task, create_power_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, KickerTelemetryPubSub, LedCommandPubSub, PowerTelemetryPubSub, TelemetryPubSub}, robot_state::SharedRobotState};
+
+use embassy_time::Timer;
+// provide embedded panic probe
+use panic_probe as _;
+use static_cell::ConstStaticCell;
+
+static ROBOT_STATE: ConstStaticCell<SharedRobotState> = ConstStaticCell::new(SharedRobotState::new());
+
+static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new();
+static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new();
+static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new();
+static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new();
+static POWER_DATA_CHANNEL: PowerTelemetryPubSub = PubSubChannel::new();
+static KICKER_DATA_CHANNEL: KickerTelemetryPubSub = PubSubChannel::new();
+static LED_COMMAND_PUBSUB: LedCommandPubSub = PubSubChannel::new();
+
+static RADIO_UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new();
+static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new();
+
+#[allow(non_snake_case)]
+#[interrupt]
+unsafe fn CEC() {
+    UART_QUEUE_EXECUTOR.on_interrupt();
+}
+
+#[allow(non_snake_case)]
+#[interrupt]
+unsafe fn CORDIC() {
+    RADIO_UART_QUEUE_EXECUTOR.on_interrupt();
+}
+
+#[embassy_executor::main]
+async fn main(main_spawner: embassy_executor::Spawner) {
+    // init system
+    let sys_config = get_system_config();
+    let p = embassy_stm32::init(sys_config);
+
+    defmt::info!("embassy HAL configured.");
+
+    let robot_state = ROBOT_STATE.take();
+
+    ////////////////////////
+    //  setup task pools  //
+    ////////////////////////
+
+    // uart queue executor should be highest priority
+    // NOTE: maybe this should be all DMA tasks? No computation tasks here
+    interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P7);
+    let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC);
+
+    //////////////////////////////////////
+    //  setup inter-task coms channels  //
+    //////////////////////////////////////
+
+    // commands channel
+    let control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap();
+    let kicker_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap();
+
+    // telemetry channel
+    let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap();
+
+    // IMU channels
+    let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap();
+    let control_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap();
+    let imu_led_cmd_publisher = LED_COMMAND_PUBSUB.publisher().unwrap();
+
+    let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap();
+    let control_accel_data_subscriber = ACCEL_DATA_CHANNEL.subscriber().unwrap();
+
+    // power channel
+    let power_board_telemetry_publisher = POWER_DATA_CHANNEL.publisher().unwrap();
+    let control_task_power_telemetry_subscriber = POWER_DATA_CHANNEL.subscriber().unwrap();
+
+    // kicker channel
+    let kicker_board_telemetry_publisher = KICKER_DATA_CHANNEL.publisher().unwrap();
+    let control_task_kicker_telemetry_subscriber = KICKER_DATA_CHANNEL.subscriber().unwrap();
+
+    // power board
+    let power_led_cmd_publisher = LED_COMMAND_PUBSUB.publisher().unwrap();
+
+    ///////////////////
+    //  start tasks  //
+    ///////////////////
+
+    create_io_task!(main_spawner,
+        robot_state,
+        p);
+
+    create_power_task!(main_spawner, uart_queue_spawner,
+        robot_state, power_board_telemetry_publisher, power_led_cmd_publisher,
+        p);
+
+    create_imu_task!(main_spawner,
+        robot_state,
+        imu_gyro_data_publisher, imu_accel_data_publisher, imu_led_cmd_publisher,
+        p);
+
+    create_motor_calibrate_task!(main_spawner, uart_queue_spawner,
+        robot_state,
+        control_command_subscriber, control_telemetry_publisher,
+        control_task_power_telemetry_subscriber, control_task_kicker_telemetry_subscriber,
+        control_gyro_data_subscriber, control_accel_data_subscriber,
+        p);
+
+    create_kicker_task!(
+        main_spawner, uart_queue_spawner,
+        robot_state,
+        kicker_command_subscriber,
+        kicker_board_telemetry_publisher,
+        p);
+
+    loop {
+        Timer::after_millis(10).await;
+    }
+}

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

@@ -124,19 +124,17 @@ async fn main(main_spawner: embassy_executor::Spawner) {
     );
 
     loop {
-        Timer::after_millis(100).await;
-
-        test_command_publisher
-            .publish(DataPacket::BasicControl(BasicControl {
-                _bitfield_1: Default::default(),
-                _bitfield_align_1: Default::default(),
-                vel_x_linear: 1.0,
-                vel_y_linear: 0.0,
-                vel_z_angular: 0.0,
-                kick_vel: 0.0,
-                dribbler_speed: 10.0,
-                kick_request: KickRequest::KR_DISABLE,
-            }))
-            .await;
+        Timer::after_millis(10).await;
+
+        test_command_publisher.publish(DataPacket::BasicControl(BasicControl {
+            _bitfield_1: Default::default(),
+            _bitfield_align_1: Default::default(),
+            vel_x_linear: 1.0,
+            vel_y_linear: 0.0,
+            vel_z_angular: 5.0,
+            kick_vel: 0.0,
+            dribbler_speed: 10.0,
+            kick_request: KickRequest::KR_DISABLE,
+        })).await;
     }
 }

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
@@ -273,6 +281,13 @@ 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() {
@@ -424,6 +439,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 +461,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 +477,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;
     }
 }