From d28ad181bda11adada9fdee4cf4097ba3f8e650f Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 6 Sep 2025 18:47:01 -0400 Subject: [PATCH 01/18] always send telemetry --- kicker-board/src/bin/hwtest-coms/main.rs | 84 +++++++++++++---------- kicker-board/src/bin/kicker/main.rs | 87 +++++++++++++----------- motor-controller/bin/dribbler/main.c | 25 ++++--- motor-controller/bin/wheel-test/main.c | 4 +- motor-controller/bin/wheel/main.c | 26 ++++--- 5 files changed, 132 insertions(+), 94 deletions(-) diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index 18e3e121..556f67fd 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -59,6 +59,7 @@ async fn high_pri_kick_task( kick_pin: Peri<'static, KickPin>, chip_pin: Peri<'static, ChipPin>, mut rail_pin: Peri<'static, PowerRail200vReadPin>, + grn_led_pin: Peri<'static, GreenStatusLedPin>, err_led_pin: Peri<'static, RedStatusLedPin>, ball_detected_led_pin: Peri<'static, BlueStatusLedPin>, ) -> ! { @@ -69,8 +70,10 @@ async fn high_pri_kick_task( let mut kick_manager = KickManager::new(charge_pin, kick_pin, chip_pin); // debug LEDs + let mut status_led = Output::new(grn_led_pin, Level::Low, Speed::Low); let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); let mut ball_detected_led = Output::new(ball_detected_led_pin, Level::Low, Speed::Low); + let mut green_led_ctr: u16 = 0; // TODO dotstars @@ -124,6 +127,17 @@ async fn high_pri_kick_task( // update telemetry requests telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; + if !telemetry_enabled { + status_led.set_high(); + } else { + if green_led_ctr > 200 { + green_led_ctr = 0; + } else if green_led_ctr > 100 { + status_led.set_low(); + } else { + status_led.set_high(); + } + } // no charge/kick in coms test let res = kick_manager @@ -131,47 +145,46 @@ async fn high_pri_kick_task( .await; // send telemetry packet - if telemetry_enabled { - let cur_time = Instant::now(); - if Instant::checked_duration_since(&cur_time, last_packet_sent_time) - .unwrap() - .as_millis() - > 20 - { - kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( - res.is_err() as u16, - 0, - 0, - 0, - ball_detected as u16, - 0, - Default::default(), + let cur_time = Instant::now(); + if Instant::checked_duration_since(&cur_time, last_packet_sent_time) + .unwrap() + .as_millis() + > 20 + { + kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( + res.is_err() as u16, + 0, + 0, + 0, + ball_detected as u16, + 0, + Default::default(), + ); + kicker_telemetry_packet.rail_voltage = rail_voltage; + kicker_telemetry_packet.battery_voltage = 22.5; + + // raw interpretaion of a struct for wire transmission is unsafe + unsafe { + // get a slice to packet for transmission + let struct_bytes = core::slice::from_raw_parts( + (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, + core::mem::size_of::(), ); - kicker_telemetry_packet.rail_voltage = rail_voltage; - kicker_telemetry_packet.battery_voltage = 22.5; - - // raw interpretaion of a struct for wire transmission is unsafe - unsafe { - // get a slice to packet for transmission - let struct_bytes = core::slice::from_raw_parts( - (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, - core::mem::size_of::(), - ); - - // send the packet - let _res = coms_writer.enqueue_copy(struct_bytes); - } - if ball_detected_led.is_set_high() { - ball_detected_led.set_low(); - } else { - ball_detected_led.set_high(); - } + // send the packet + let _res = coms_writer.enqueue_copy(struct_bytes); + } - last_packet_sent_time = cur_time; + if ball_detected_led.is_set_high() { + ball_detected_led.set_low(); + } else { + ball_detected_led.set_high(); } + + last_packet_sent_time = cur_time; } + // LEDs if res.is_err() { err_led.set_high(); @@ -254,6 +267,7 @@ async fn main(spawner: Spawner) -> ! { p.PD9, p.PD8, p.PC3, + p.PB9, p.PE0, p.PE1, )) diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index fac2771f..6866232b 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -130,13 +130,14 @@ async fn high_pri_kick_task( let mut status_led = Output::new(grn_led_pin, Level::Low, Speed::Low); let mut err_led = Output::new(err_led_pin, Level::Low, Speed::Low); let mut ball_detected_led1 = Output::new(ball_detected_led1_pin, Level::Low, Speed::Low); + let mut green_led_ctr: u16 = 0; // TODO dotstars let mut breakbeam = Breakbeam::new(breakbeam_tx.into(), breakbeam_rx.into()); // coms buffers - let mut telemetry_enabled: bool; // = false; + let mut telemetry_enabled: bool; let mut kicker_control_packet: KickerControl = Default::default(); let mut kicker_telemetry_packet: KickerTelemetry = Default::default(); let mut dribbler_motor_telemetry: MotorTelemetry = Default::default(); @@ -238,10 +239,16 @@ async fn high_pri_kick_task( // update telemetry requests telemetry_enabled = kicker_control_packet.telemetry_enabled() != 0; - if telemetry_enabled { + if !telemetry_enabled { status_led.set_high(); } else { - status_led.set_low(); + if green_led_ctr > 200 { + green_led_ctr = 0; + } else if green_led_ctr > 100 { + status_led.set_low(); + } else { + status_led.set_high(); + } } // for now shutdown requests will be latched and a reboot is required to re-power @@ -396,47 +403,45 @@ async fn high_pri_kick_task( } // send telemetry packet - if telemetry_enabled { - let cur_time = Instant::now(); - if Instant::checked_duration_since(&cur_time, last_packet_sent_time) - .unwrap() - .as_millis() - > 20 - { - kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( - res.is_err() as u16, - dribbler_motor_telemetry.master_error() as u16, - shutdown_requested as u16, - shutdown_completed as u16, - ball_detected as u16, - (rail_voltage_ave > CHARGED_THRESH_VOLTAGE) as u16, - Default::default(), + let cur_time = Instant::now(); + if Instant::checked_duration_since(&cur_time, last_packet_sent_time) + .unwrap() + .as_millis() + > 20 + { + kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( + res.is_err() as u16, + dribbler_motor_telemetry.master_error() as u16, + shutdown_requested as u16, + shutdown_completed as u16, + ball_detected as u16, + (rail_voltage_ave > CHARGED_THRESH_VOLTAGE) as u16, + Default::default(), + ); + + let charge_pct = rail_voltage_ave / CHARGE_TARGET_VOLTAGE; + kicker_telemetry_packet.charge_pct = (charge_pct * 100.0) as u16; + kicker_telemetry_packet.rail_voltage = rail_voltage_ave; + kicker_telemetry_packet.battery_voltage = battery_voltage; + + kicker_telemetry_packet.dribbler_motor = dribbler_motor_telemetry; + kicker_telemetry_packet + .kicker_image_hash + .copy_from_slice(&kicker_img_hash_kicker[0..4]); + + // raw interpretaion of a struct for wire transmission is unsafe + unsafe { + // get a slice to packet for transmission + let struct_bytes = core::slice::from_raw_parts( + (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, + core::mem::size_of::(), ); - let charge_pct = rail_voltage_ave / CHARGE_TARGET_VOLTAGE; - kicker_telemetry_packet.charge_pct = (charge_pct * 100.0) as u16; - kicker_telemetry_packet.rail_voltage = rail_voltage_ave; - kicker_telemetry_packet.battery_voltage = battery_voltage; - - kicker_telemetry_packet.dribbler_motor = dribbler_motor_telemetry; - kicker_telemetry_packet - .kicker_image_hash - .copy_from_slice(&kicker_img_hash_kicker[0..4]); - - // raw interpretaion of a struct for wire transmission is unsafe - unsafe { - // get a slice to packet for transmission - let struct_bytes = core::slice::from_raw_parts( - (&kicker_telemetry_packet as *const KickerTelemetry) as *const u8, - core::mem::size_of::(), - ); - - // send the packet - let _res = coms_writer.enqueue_copy(struct_bytes); - } - - last_packet_sent_time = cur_time; + // send the packet + let _res = coms_writer.enqueue_copy(struct_bytes); } + + last_packet_sent_time = cur_time; } // LEDs diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 6a3d74b4..8ab6ffc7 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -104,6 +104,7 @@ int main() { // Turn off Red/Yellow LED after booting. turn_off_red_led(); turn_off_yellow_led(); + uint16_t green_led_ctr = 0; #ifdef UART_ENABLED // Initialize UART and logging status. @@ -289,7 +290,7 @@ int main() { // transmit packets #ifdef UART_ENABLED - if (telemetry_enabled && run_telemetry) { + if (run_telemetry) { // If previous UART transmit is still occurring, // wait for it to finish. uart_wait_for_transmission(); @@ -358,7 +359,7 @@ int main() { // Red LED means we are in an error state. // This latches and requires resetting the robot to clear. if (response_packet.data.motion.master_error || - (telemetry_enabled && ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS)) { + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { turn_on_red_led(); } @@ -371,17 +372,25 @@ int main() { turn_off_yellow_led(); } - // Green LED means we are able to send telemetry upstream. - // This means the upstream sent a packet downstream with telemetry enabled. + // Green LED means system is up. Flicker means we're acknowledging that + // control has enabled the system (non-zero motor commands allowed) if (!telemetry_enabled) { - turn_off_green_led(); - } else { turn_on_green_led(); + } else { + // 5Hz flicker + if (green_led_ctr > 200) { + green_led_ctr = 0; + } else if (green_led_ctr > 100) { + turn_off_green_led(); + } else { + turn_on_green_led(); + } + + green_led_ctr++; } #ifdef COMP_MODE - if (telemetry_enabled && - ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { // If have telemetry enabled (meaning we at one // point received a message from upstream) and haven't // received a command packet in a while, we need to reset diff --git a/motor-controller/bin/wheel-test/main.c b/motor-controller/bin/wheel-test/main.c index 45e78ae3..1bc6bb38 100644 --- a/motor-controller/bin/wheel-test/main.c +++ b/motor-controller/bin/wheel-test/main.c @@ -428,7 +428,7 @@ int main() { // transmit packets #ifdef UART_ENABLED - if (telemetry_enabled && run_telemetry) { + if (run_telemetry) { // If previous UART transmit is still occurring, // wait for it to finish. uart_wait_for_transmission(); @@ -500,7 +500,7 @@ int main() { // Red LED means we are in an error state. // This latches and requires resetting the robot to clear. if (response_packet.data.motion.master_error || - (telemetry_enabled && ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS)) { + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { turn_on_red_led(); } diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 03d58be3..bba485c7 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -195,6 +195,8 @@ int main() { turn_off_red_led(); turn_off_yellow_led(); + uint16_t green_led_ctr = 0; + #ifdef UART_ENABLED // Initialize UART and logging status. uart_initialize(); @@ -487,7 +489,7 @@ int main() { // transmit packets #ifdef UART_ENABLED - if (telemetry_enabled && run_telemetry) { + if (run_telemetry) { // If previous UART transmit is still occurring, // wait for it to finish. uart_wait_for_transmission(); @@ -562,7 +564,7 @@ int main() { // Red LED means we are in an error state. // This latches and requires resetting the robot to clear. if (response_packet.data.motion.master_error || - (telemetry_enabled && ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS)) { + ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { turn_on_red_led(); } @@ -575,17 +577,25 @@ int main() { turn_off_yellow_led(); } - // Green LED means we are able to send telemetry upstream. - // This means the upstream sent a packet downstream with telemetry enabled. + // Green LED means system is up. Flicker means we're acknowledging that + // control has enabled the system (non-zero motor commands allowed) if (!telemetry_enabled) { - turn_off_green_led(); - } else { turn_on_green_led(); + } else { + // 5Hz flicker + if (green_led_ctr > 200) { + green_led_ctr = 0; + } else if (green_led_ctr > 100) { + turn_off_green_led(); + } else { + turn_on_green_led(); + } + + green_led_ctr++; } #ifdef COMP_MODE - if (telemetry_enabled && - ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { + if (ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS) { // If have telemetry enabled (meaning we at one // point received a message from upstream) and haven't // received a command packet in a while, we need to reset From 2214b930e6d07d644e825461862bb0d319642d21 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 9 Sep 2025 20:47:51 -0400 Subject: [PATCH 02/18] update software communication --- software-communication | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/software-communication b/software-communication index a4ef89e4..9ce093cf 160000 --- a/software-communication +++ b/software-communication @@ -1 +1 @@ -Subproject commit a4ef89e4aa9081f9c3b3e7eff8546d8f2c7644fb +Subproject commit 9ce093cf5920b20ac466da2b5f12ed84c6786bb1 From 41b45784d790e9013e8ab437039deffe5aedc6f6 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 9 Sep 2025 21:11:10 -0400 Subject: [PATCH 03/18] telemetry enable removal seems to be working --- control-board/src/tasks/imu_task.rs | 3 +-- control-board/src/tasks/kicker_task.rs | 7 +++++-- control-board/src/tasks/radio_task.rs | 7 +++---- kicker-board/src/bin/hwtest-coms/main.rs | 3 +-- kicker-board/src/bin/kicker/main.rs | 3 +-- lib-stm32/src/drivers/boot/stm32_interface.rs | 2 +- 6 files changed, 12 insertions(+), 13 deletions(-) diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs index 4756e7df..107dd018 100644 --- a/control-board/src/tasks/imu_task.rs +++ b/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 { diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index ff9376a9..7bc51cce 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -143,8 +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() + 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); @@ -185,6 +184,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 +223,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 { diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 9335be5f..3ed734d6 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -392,8 +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() + && Instant::duration_since(&cur_time, self.last_software_packet) .as_millis() > Self::RESPONSE_FROM_PC_TIMEOUT_MS { @@ -425,8 +424,8 @@ 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(); diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index 556f67fd..bfc9bb3e 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -146,8 +146,7 @@ async fn high_pri_kick_task( // send telemetry packet let cur_time = Instant::now(); - if Instant::checked_duration_since(&cur_time, last_packet_sent_time) - .unwrap() + if Instant::duration_since(&cur_time, last_packet_sent_time) .as_millis() > 20 { diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index 6866232b..e55c93fb 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -404,8 +404,7 @@ async fn high_pri_kick_task( // send telemetry packet let cur_time = Instant::now(); - if Instant::checked_duration_since(&cur_time, last_packet_sent_time) - .unwrap() + if Instant::duration_since(&cur_time, last_packet_sent_time) .as_millis() > 20 { diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index ca697fe3..5f512ec9 100644 --- a/lib-stm32/src/drivers/boot/stm32_interface.rs +++ b/lib-stm32/src/drivers/boot/stm32_interface.rs @@ -239,7 +239,7 @@ impl< pub fn send_or_discard_data(&self, data: &[u8]) { if self.try_send_data(data).is_err() { - defmt::error!("Failed to send motor data"); + defmt::warn!("Failed to send motor data"); }; } From b4c17df7c04792bb8a29e6215aa303c25c97078e Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sat, 13 Sep 2025 21:55:26 -0400 Subject: [PATCH 04/18] builds on new packets --- control-board/src/stspin_motor.rs | 2 +- control-board/src/tasks/control_task.rs | 2 +- motor-controller/bin/dribbler/main.c | 2 +- motor-controller/bin/wheel-test/main.c | 8 ++++---- motor-controller/bin/wheel/main.c | 10 +++++----- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index e7608f07..ebc808f2 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -455,6 +455,6 @@ impl< } pub fn read_vel_computed_setpoint(&self) -> f32 { - return self.current_state.vel_computed_setpoint; + return self.current_state.vel_computed_rads; } } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 50d4f90c..fc367bdf 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -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, diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 8ab6ffc7..fbd5fdba 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -230,7 +230,7 @@ int main() { response_packet.data.motion.torque_setpoint = r; response_packet.data.motion.torque_estimate = cur_measurement; response_packet.data.motion.torque_computed_error = torque_pid.prev_err; - response_packet.data.motion.torque_computed_setpoint = torque_setpoint; + response_packet.data.motion.torque_computed_nm = torque_setpoint; } if (run_torque_loop) { diff --git a/motor-controller/bin/wheel-test/main.c b/motor-controller/bin/wheel-test/main.c index 1bc6bb38..c87abb25 100644 --- a/motor-controller/bin/wheel-test/main.c +++ b/motor-controller/bin/wheel-test/main.c @@ -336,7 +336,7 @@ int main() { response_packet.data.motion.current_estimate = current_sense_I; response_packet.data.motion.torque_estimate = measured_torque_Nm; response_packet.data.motion.torque_computed_error = torque_pid.prev_err; - response_packet.data.motion.torque_computed_setpoint = torque_setpoint_Nm; + response_packet.data.motion.torque_computed_nm = torque_setpoint_Nm; } // run velocity loop if applicable @@ -368,11 +368,11 @@ int main() { // velocity control data response_packet.data.motion.vel_setpoint = r_motor_board; - response_packet.data.motion.vel_setpoint_clamped = control_setpoint_vel_rads; + response_packet.data.motion.vel_computed_rads = control_setpoint_vel_rads; response_packet.data.motion.encoder_delta = enc_delta; response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt; response_packet.data.motion.vel_computed_error = vel_pid.prev_err; - response_packet.data.motion.vel_computed_setpoint = control_setpoint_vel_duty; + response_packet.data.motion.vel_computed_duty = control_setpoint_vel_duty; } if (run_torque_loop || run_vel_loop) { @@ -458,7 +458,7 @@ int main() { response_packet.data.params.version_major = VERSION_MAJOR; response_packet.data.params.version_major = VERSION_MINOR; response_packet.data.params.version_major = VERSION_PATCH; - response_packet.data.params.timestamp = time_local_epoch_s(); + response_packet.timestamp = time_local_epoch_s(); response_packet.data.params.vel_p = vel_pid_constants.kP; response_packet.data.params.vel_i = vel_pid_constants.kI; diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index bba485c7..e66baa75 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -375,7 +375,7 @@ int main() { response_packet.data.motion.current_estimate = current_sense_I; response_packet.data.motion.torque_estimate = measured_torque_Nm; response_packet.data.motion.torque_computed_error = torque_pid.prev_err; - response_packet.data.motion.torque_computed_setpoint = torque_setpoint_Nm; + response_packet.data.motion.torque_computed_nm = torque_setpoint_Nm; } // run velocity loop if applicable @@ -416,11 +416,11 @@ int main() { // velocity control data response_packet.data.motion.vel_setpoint = r_motor_board; - response_packet.data.motion.vel_setpoint_clamped = control_setpoint_vel_rads; + response_packet.data.motion.vel_computed_rads = control_setpoint_vel_rads; response_packet.data.motion.encoder_delta = enc_delta; response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt; response_packet.data.motion.vel_computed_error = vel_pid.prev_err; - response_packet.data.motion.vel_computed_setpoint = control_setpoint_vel_duty; + response_packet.data.motion.vel_computed_duty = control_setpoint_vel_duty; } if (run_torque_loop || run_vel_loop) { @@ -432,7 +432,7 @@ int main() { if (motion_control_type == OPEN_LOOP) { float r_motor = mm_rads_to_dc(&df45_model, r_motor_board); response_packet.data.motion.vel_setpoint = r_motor_board; - response_packet.data.motion.vel_computed_setpoint = r_motor; + response_packet.data.motion.vel_setpoint = r_motor; pwm6step_set_duty_cycle_f(r_motor); } else if (motion_control_type == VELOCITY) { pwm6step_set_duty_cycle_f(control_setpoint_vel_duty); @@ -519,7 +519,7 @@ int main() { response_packet.data.params.version_major = VERSION_MAJOR; response_packet.data.params.version_minor = VERSION_MINOR; response_packet.data.params.version_patch = VERSION_PATCH; - response_packet.data.params.timestamp = time_local_epoch_s(); + response_packet.timestamp = time_local_epoch_s(); // TODO parameter updates are off for gain scheduled PID // response_packet.data.params.vel_p = vel_pid_constants.kP; From cf3040cbe3836c994fa5dca4ef5cfa94ee5be886 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 23 Sep 2025 21:57:15 -0400 Subject: [PATCH 05/18] current sense timing updated --- control-board/src/bin/hwtest-motor/main.rs | 14 ++- control-board/src/tasks/control_task.rs | 4 +- motor-controller/bin/dribbler/main.c | 2 +- motor-controller/bin/wheel/main.c | 8 +- motor-controller/bin/wheel/system.h | 11 +- motor-controller/common/6step.c | 5 +- motor-controller/common/current_sensing.c | 118 ++++++++++----------- motor-controller/common/current_sensing.h | 10 +- 8 files changed, 99 insertions(+), 73 deletions(-) diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 9655cb6c..ede001b2 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -123,14 +123,26 @@ 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 > 100 { + ctr = 0; + } else if ctr > 50 { + 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: vel, vel_y_linear: 0.0, vel_z_angular: 0.0, kick_vel: 0.0, diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 50d4f90c..c517e5f4 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -459,8 +459,8 @@ 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 // diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 6a3d74b4..2f135d1d 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -212,7 +212,7 @@ int main() { bool run_telemetry = time_sync_ready_rst(&telemetry_timer); if (run_torque_loop) { - float cur_measurement = ((float) res.I_motor / (float) UINT16_MAX) * AVDD_V; + float cur_measurement = ((float) res.I_motor_filt / (float) UINT16_MAX) * AVDD_V; // TODO: recover current from voltage // TODO: estimate torque from current // TODO: filter? diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 03d58be3..f3227fd0 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -330,9 +330,13 @@ int main() { if (run_torque_loop) { // recover torque using the shunt voltage drop, amplification network model and motor model // pct of voltage range 0-3.3V - float current_sense_shunt_v = ((float) res.I_motor_filt / (float) UINT16_MAX) * AVDD_V; + float current_sense_shunt_v = (((float) res.I_motor_filt / (float) UINT16_MAX) * 3.0f) + 0.009f; + // if (current_sense_shunt_v < 0.0f) { + // current_sense_shunt_v = 0.0f; + // } // map voltage given by the amp network to current - float current_sense_I = mm_voltage_to_current(&df45_model, current_sense_shunt_v); + // float current_sense_I = current_sense_shunt_v / 0.25f; // mm_voltage_to_current(&df45_model, current_sense_shunt_v); + float current_sense_I = current_sense_shunt_v; // map current to torque using the motor model float measured_torque_Nm = mm_current_to_torque(&df45_model, current_sense_I); // filter torque diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index 4c6c1c1c..488b9745 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -26,9 +26,14 @@ ////////////////// #define ADC_MODE CS_MODE_DMA -#define ADC_NUM_CHANNELS 4 -#define ADC_CH_MASK (ADC_CHSELR_CHSEL3 | ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17) -#define ADC_SR_MASK (ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2) +#define ADC_NUM_CHANNELS 2 +// #define ADC_CH_MASK (ADC_CHSELR_CHSEL3 | ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17) +#define ADC_CH_MASK (ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL9) +// #define ADC_CH_MASK (ADC_CHSELR_CHSEL4) +// #define ADC_SR_MASK (ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1) +// #define ADC_SR_MASK (ADC_SMPR_SMP_0 | ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2) +#define ADC_SR_MASK (ADC_SMPR_SMP_0) +// #define ADC_SR_MASK 0 //////////////////// // TIME KEEPING // diff --git a/motor-controller/common/6step.c b/motor-controller/common/6step.c index 98d4ee6c..48195dec 100644 --- a/motor-controller/common/6step.c +++ b/motor-controller/common/6step.c @@ -677,7 +677,7 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { bool phase3_low = commutation_values[4]; bool phase3_high = commutation_values[5]; - uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) 48000 * (PWM_TIM_PRESCALER + 1)) - 1); + uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) PWM_FREQ_HZ * (PWM_TIM_PRESCALER + 1)) - 1); TIM1->CCR4 = arr_pwm_dc_value; // uint16_t ccer = 0; @@ -686,6 +686,9 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { // uint16_t ccmr2 = 0; uint16_t ccmr2 = CCMR2_TIM4_ADC_TRIG; TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; + + TIM1->CCR4 = 22; + if (phase1_low) { if (command_brake) { TIM1->CCR1 = current_duty_cycle; diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c index afbf0c22..03d47d20 100644 --- a/motor-controller/common/current_sensing.c +++ b/motor-controller/common/current_sensing.c @@ -41,60 +41,60 @@ void DMA1_Channel1_IRQHandler() { * * @return ADC_Result result of the ADC operation */ -void currsen_read(ADC_Result_t *res) -{ - const int NUM_CHANNELS = 4; - - ADC1->CR |= ADC_CR_ADSTART; - - //ADC_Result_t res; - res->status = CS_OK; - while (1) - { - - for (int i = 0; i < NUM_CHANNELS; i++) - { - // Start ADC conversion - ADC1->CR |= ADC_CR_ADSTART; - - // Wait until end of conversion - uint32_t timeout_count = 0; - while ((ADC1->ISR & ADC_ISR_EOC) == 0) - { - if (timeout_count < ADC_DIS_TIMEOUT) - { - //wait_ms(1); - //timeout_count += 1; - } - else - { - res->status = CS_TIMEOUT; - //return res; - } - } - // Store the ADC conversion - uint16_t currADC = ADC1->DR; - switch (i) - { - case 0: - res->I_motor_filt = currADC; - //return; - break; - case 1: - res->I_motor = currADC; - break; - case 2: - res->T_spin = currADC; - break; - case 3: - res->V_int = currADC; - return; - //return res; - } - } - } - -} +// void currsen_read(ADC_Result_t *res) +// { +// const int NUM_CHANNELS = 4; + +// ADC1->CR |= ADC_CR_ADSTART; + +// //ADC_Result_t res; +// res->status = CS_OK; +// while (1) +// { + +// for (int i = 0; i < NUM_CHANNELS; i++) +// { +// // Start ADC conversion +// ADC1->CR |= ADC_CR_ADSTART; + +// // Wait until end of conversion +// uint32_t timeout_count = 0; +// while ((ADC1->ISR & ADC_ISR_EOC) == 0) +// { +// if (timeout_count < ADC_DIS_TIMEOUT) +// { +// //wait_ms(1); +// //timeout_count += 1; +// } +// else +// { +// res->status = CS_TIMEOUT; +// //return res; +// } +// } +// // Store the ADC conversion +// uint16_t currADC = ADC1->DR; +// switch (i) +// { +// case 0: +// res->I_motor_filt = currADC; +// //return; +// break; +// case 1: +// res->I_motor = currADC; +// break; +// case 2: +// res->T_spin = currADC; +// break; +// case 3: +// res->V_int = currADC; +// return; +// //return res; +// } +// } +// } + +// } /** @@ -210,8 +210,8 @@ CS_Status_t currsen_adc_group_config() ); // Wake-up the VREFINT (only for VBAT, Temp sensor and VRefInt) - ADC->CCR |= ADC_CCR_VREFEN; // enable internal Vref source - ADC->CCR |= ADC_CCR_TSEN; // enable internal temp source + // ADC->CCR |= ADC_CCR_VREFEN; // enable internal Vref source + // ADC->CCR |= ADC_CCR_TSEN; // enable internal temp source return CS_OK; } @@ -237,18 +237,18 @@ CS_Status_t currsen_adc_conf() | ADC_CFGR1_WAIT | ADC_CFGR1_AUTOFF , - ADC_RESOLUTION_10B + ADC_RESOLUTION_12B | ADC_DATA_ALIGN_LEFT | ADC_LP_MODE_NONE ); // Set ADC clock - // PCLK DIV 2, Latency is deterministic (no jitter) and equal to + // PCLK DIV 4, Latency is deterministic (no jitter) and equal to // 2.75 ADC clock cycles MODIFY_REG(ADC1->CFGR2, ADC_CFGR2_CKMODE , - ADC_CFGR2_CKMODE_0 + ADC_CFGR2_CKMODE_1 ); return CS_OK; diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h index 0dc4ca3c..dbeeaf08 100644 --- a/motor-controller/common/current_sensing.h +++ b/motor-controller/common/current_sensing.h @@ -48,7 +48,8 @@ typedef enum #define ADC_LP_MODE_NONE (0x00000000U) /*!< No ADC low power mode activated */ // ADC Trigger mode -#define ADC_REG_TRIG_EXT_EDGE_DEFAULT (ADC_CFGR1_EXTEN_1) // falling edge only +// #define ADC_REG_TRIG_EXT_EDGE_DEFAULT (ADC_CFGR1_EXTEN_1) // falling edge only +#define ADC_REG_TRIG_EXT_EDGE_DEFAULT (ADC_CFGR1_EXTEN_0) // rising edge only #define ADC_REG_TRIG_SOFTWARE (0x00000000U) /*!< ADC group regular conversion trigger internal: SW start. */ #define ADC_REG_TRIG_EXT_TIM1_TRGO (ADC_REG_TRIG_EXT_EDGE_DEFAULT) /*!< ADC group regular conversion trigger from external IP: TIM1 TRGO. Trigger edge set to rising edge (default setting). */ #define ADC_REG_TRIG_EXT_TIM1_CH4 (ADC_CFGR1_EXTSEL_0 | ADC_REG_TRIG_EXT_EDGE_DEFAULT) /*!< ADC group regular conversion trigger from external IP: TIM1 channel 4 event (capture compare: input capture or output capture). Trigger edge set to rising edge (default setting). */ @@ -92,10 +93,11 @@ typedef enum // transfers typedef struct __attribute__((__packed__)) ADC_Result { + // uint16_t I_motor; uint16_t I_motor_filt; - uint16_t I_motor; - uint16_t T_spin; - uint16_t V_int; + uint16_t V_batt; + // uint16_t T_spin; + // uint16_t V_int; CS_Status_t status; } ADC_Result_t; From 7fc41d7c2c01d32ef8455ccaa035b8cf5834a9aa Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 23 Sep 2025 22:07:18 -0400 Subject: [PATCH 06/18] vbatt read working --- motor-controller/bin/wheel/main.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index f3227fd0..6c0cea63 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -331,12 +331,14 @@ int main() { // recover torque using the shunt voltage drop, amplification network model and motor model // pct of voltage range 0-3.3V float current_sense_shunt_v = (((float) res.I_motor_filt / (float) UINT16_MAX) * 3.0f) + 0.009f; + float vbatt = ((((float) res.V_batt / (float) UINT16_MAX) * 3.0f) / 2.016f) * 25.2f; // if (current_sense_shunt_v < 0.0f) { // current_sense_shunt_v = 0.0f; // } // map voltage given by the amp network to current // float current_sense_I = current_sense_shunt_v / 0.25f; // mm_voltage_to_current(&df45_model, current_sense_shunt_v); - float current_sense_I = current_sense_shunt_v; + // float current_sense_I = current_sense_shunt_v; + float current_sense_I = vbatt; // map current to torque using the motor model float measured_torque_Nm = mm_current_to_torque(&df45_model, current_sense_I); // filter torque From f25aa29b3c1587b04d527719fb7c784e8c7a71d7 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 28 Sep 2025 19:43:05 -0400 Subject: [PATCH 07/18] software communication update working --- control-board/src/bin/hwtest-motor/main.rs | 2 +- control-board/src/tasks/control_task.rs | 4 ++-- motor-controller/bin/dribbler/main.h | 2 -- motor-controller/bin/dribbler/system.h | 2 +- motor-controller/bin/wheel-test/main.h | 1 - motor-controller/bin/wheel/main.h | 1 - motor-controller/bin/wheel/system.h | 2 +- 7 files changed, 5 insertions(+), 9 deletions(-) diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 9655cb6c..7e337eed 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -130,7 +130,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { .publish(DataPacket::BasicControl(BasicControl { _bitfield_1: Default::default(), _bitfield_align_1: Default::default(), - vel_x_linear: 1.0, + vel_x_linear: 0.5, vel_y_linear: 0.0, vel_z_angular: 0.0, kick_vel: 0.0, diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index fc367bdf..9535fbd2 100644 --- a/control-board/src/tasks/control_task.rs +++ b/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 = diff --git a/motor-controller/bin/dribbler/main.h b/motor-controller/bin/dribbler/main.h index 2e1149ac..c5884915 100644 --- a/motor-controller/bin/dribbler/main.h +++ b/motor-controller/bin/dribbler/main.h @@ -15,8 +15,6 @@ // timeout ticks equates to 10 consecutive missed packets. #define COMMAND_PACKET_TIMEOUT_TICKS 100 -#define IOQ_BUF_LEN 64 - #define AVDD_V 3.3f /////////////////////////////////////////// diff --git a/motor-controller/bin/dribbler/system.h b/motor-controller/bin/dribbler/system.h index d0427c03..d26b06df 100644 --- a/motor-controller/bin/dribbler/system.h +++ b/motor-controller/bin/dribbler/system.h @@ -16,7 +16,7 @@ /////////////////// #define UART_ENABLED -#define IOQ_BUF_LENGTH 64 +#define IOQ_BUF_LENGTH 80 #define IOQ_BUF_DEPTH 4 ////////////////// diff --git a/motor-controller/bin/wheel-test/main.h b/motor-controller/bin/wheel-test/main.h index c89e19cf..178e00ef 100644 --- a/motor-controller/bin/wheel-test/main.h +++ b/motor-controller/bin/wheel-test/main.h @@ -21,7 +21,6 @@ // expect cmd packets at 100H Hz (10ms), ticks are currently 1ms per // timeout ticks equates to 10 consecutive missed packets. #define COMMAND_PACKET_TIMEOUT_TICKS 100 -#define IOQ_BUF_LEN 64 #define AVDD_V 3.3f diff --git a/motor-controller/bin/wheel/main.h b/motor-controller/bin/wheel/main.h index 63baec1e..3a9b7527 100644 --- a/motor-controller/bin/wheel/main.h +++ b/motor-controller/bin/wheel/main.h @@ -21,7 +21,6 @@ // expect cmd packets at 100H Hz (10ms), ticks are currently 1ms per // timeout ticks equates to 10 consecutive missed packets. #define COMMAND_PACKET_TIMEOUT_TICKS 100 -#define IOQ_BUF_LEN 64 #define AVDD_V 3.3f diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index 4c6c1c1c..059a50b1 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -18,7 +18,7 @@ /////////////////// #define UART_ENABLED -#define IOQ_BUF_LENGTH 64 +#define IOQ_BUF_LENGTH 80 #define IOQ_BUF_DEPTH 4 ////////////////// From d44c5b5febb066a041cc94c3fd1e1a5cfb9e1d39 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 28 Sep 2025 22:07:54 -0400 Subject: [PATCH 08/18] update boot and wheel driver from joe's branch --- control-board/src/stspin_motor.rs | 67 +++- lib-stm32/src/drivers/boot/stm32_interface.rs | 305 +++++++++++++++++- 2 files changed, 357 insertions(+), 15 deletions(-) diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index ebc808f2..7654ae4b 100644 --- a/control-board/src/stspin_motor.rs +++ b/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_rads; + 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; } } diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index 5f512ec9..e3f02db9 100644 --- a/lib-stm32/src/drivers/boot/stm32_interface.rs +++ b/lib-stm32/src/drivers/boot/stm32_interface.rs @@ -30,6 +30,11 @@ pub const STM32_BOOTLOADER_CMD_READ_PROT: u8 = 0x82; pub const STM32_BOOTLOADER_CMD_READ_UNPROT: u8 = 0x92; pub const STM32_BOOTLOADER_CMD_GET_CHECKSUM: u8 = 0xA1; +// TODO Make this shared in software-communication +pub const MOTOR_CURRENT_START_ADDRESS: u32 = 0x0800_7C00; // 512k flash, so this is the start of the last page +pub const MOTOR_CURRENT_PAGE: u8 = 31; +pub const MOTOR_CURRENT_MAGIC: [u8; 4] = [0xAA, 0xBB, 0xCC, 0xDD]; // Magic number to identify the motor current calibration data + pub fn get_bootloader_uart_config() -> Config { let mut config = usart::Config::default(); config.baudrate = 115_200; // max officially support baudrate @@ -190,10 +195,11 @@ impl< } else { defmt::debug!("bootloader replied with NACK after calibration."); } + } else { + defmt::debug!("bootloader reply too short after calibration."); } - }), - ) - .await; + res + })).await; if sync_res.is_err() { defmt::warn!("*** HARDWARE CHECK *** - bootloader baud calibration timed out."); @@ -239,7 +245,7 @@ impl< pub fn send_or_discard_data(&self, data: &[u8]) { if self.try_send_data(data).is_err() { - defmt::warn!("Failed to send motor data"); + defmt::error!("Failed to send motor data"); }; } @@ -350,8 +356,200 @@ impl< res } - pub async fn read_device_memory(&self) -> Result<(), ()> { - defmt::panic!("implement if needed."); + // Based on 3.4 of AN3155 + pub async fn read_device_memory(&self, data: &mut [u8], read_base_addr: u32) -> Result<(), ()> { + if !self.in_bootloader { + defmt::error!("called bootloader operation when not in bootloader context."); + return Err(()); + } + + let data_len = data.len(); + if data_len > 255 || data_len + 1 > LEN_TX { + defmt::error!("Data length too large for bootloader read mem command."); + return Err(()); + } + + // defmt::debug!("sending the read command..."); + self.writer + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_READ_MEM; + buf[1] = !STM32_BOOTLOADER_CMD_READ_MEM; + //defmt::info!("send buffer {:?}", buf); + 2 + }) + .await?; + + // Wait for the bootloader to acknowledge the command + let mut res = Err(()); + self.reader.read(|buf| { + // defmt::info!("Read cmd reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("Read mem cmd replied with NACK"); + } + } else { + defmt::error!("Read mem cmd reply too short."); + } + }).await?; + + if res.is_err() { + return res; + } + + // defmt::debug!("sending the load address {:?}...", read_base_addr); + self.writer + .write(|buf| { + let start_address_bytes: [u8; 4] = read_base_addr.to_be_bytes(); + let cs = Self::bootloader_checksum_u32(read_base_addr); + buf[0] = start_address_bytes[0]; + buf[1] = start_address_bytes[1]; + buf[2] = start_address_bytes[2]; + buf[3] = start_address_bytes[3]; + buf[4] = cs; + // defmt::debug!("send buffer {:?}", buf); + 5 + }) + .await?; + + res = Err(()); + // Wait for the bootloader to acknowledge the address + self.reader.read(|buf| { + // defmt::info!("go cmd reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("Address read mem replied with NACK"); + } + } else { + defmt::error!("Address read mem reply too short."); + } + }).await?; + + if res.is_err() { + return res; + } + + // defmt::debug!("sending the data length..."); + self.writer + .write(|buf| { + let data_len_minus_one = data_len as u8 - 1; + buf[0] = data_len_minus_one; + buf[1] = !data_len_minus_one; + // defmt::debug!("send buffer {:?}", buf); + 2 + }) + .await?; + + res = Err(()); + // defmt::debug!("reading the data..."); + self.reader.read(|buf| { + // defmt::info!("data reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + data.copy_from_slice(&buf[1..]); + res = Ok(()); + } else { + defmt::error!("Data read mem replied with NACK"); + } + } else { + defmt::error!("Data read mem reply too short!"); + } + }).await?; + + res + } + + // Based on 3.8 of AN3155 + // Have to use extended erase command since the normal erase command + // doesn't seem to work? + // Designed to erase a up to N + 1 page of device memory. + pub async fn erase_device_memory_to_page(&self, start_page: u8, end_page: u8) -> Result<(), ()> { + if !self.in_bootloader { + defmt::error!("Called bootloader operation when not in bootloader context."); + return Err(()); + } + + if end_page < start_page { + defmt::error!("End page must be greater than or equal to start page."); + return Err(()); + } + + // defmt::debug!("sending the erase command..."); + self.writer + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_EXTENDED_ERASE; + buf[1] = !STM32_BOOTLOADER_CMD_EXTENDED_ERASE; + 2 + }) + .await?; + + let mut res = Err(()); + self.reader.read(|buf| { + // defmt::info!("erase cmd reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("Bootloader replied to erase command with NACK"); + } + } else { + defmt::error!("Erase command reply too short."); + } + }).await?; + + if res.is_err() { + return res; + } + + // defmt::debug!("sending the page number..."); + self.writer + .write(|buf| { + // Quantity is N + 1 lead with MSB. Limited to 32 pages on STM32F1. + let erase_page_quantity = end_page - start_page; + buf[0] = 0x00; + buf[1] = erase_page_quantity; + // Need to send the page number for each page to erase in two bytes, MSB first + let mut checksum = erase_page_quantity; + // Track the index for use in returning the buffer length + let mut buf_indx: usize = 2; + for i in start_page ..= end_page { + // Won't erase more than 256 pages, so always lead with 0x00 + buf[buf_indx] = 0x00; + buf[buf_indx + 1] = i; + buf_indx += 2; + // Checksum is XOR of all previous bytes. Ignore 0x00 so just LSB + checksum ^= i; + } + // Checksum for all previous bytes is just the erase page number + buf[buf_indx] = checksum; + // defmt::debug!("send buffer {:?}", buf); + // Final size is buf_indx + 1 from checksum byte + buf_indx + 1 + }) + .await?; + + // defmt::debug!("wait for erase reply"); + self.reader.read(|buf| { + // defmt::info!("erase reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + // defmt::info!("erase accepted."); + } else { + defmt::error!("bootloader replied to erase payload with NACK"); + } + } + }).await?; + + res + } + + // Single page of device memory is just a page to the same page. + pub async fn erase_device_memory_single(&self, erase_page: u8) -> Result<(), ()> { + self.erase_device_memory_to_page(erase_page, erase_page).await } async fn write_device_memory_chunk(&self, data: &[u8], write_base_addr: u32) -> Result<(), ()> { @@ -463,7 +661,7 @@ impl< return Err(()); } - // ensure step size is below bootlaoder supported, below transmit buffer size (incl len and cs bytes), + // ensure step size is below bootloader supported, below transmit buffer size (incl len and cs bytes), // and is 4-byte aligned let step_size = min(256, LEN_TX - 2) & 0xFFFF_FFF8; defmt::debug!("bootloader will use data chunk sizes of {:?}", step_size); @@ -589,6 +787,99 @@ impl< Ok(()) } + pub async fn load_motor_firmware_image(&mut self, fw_image_bytes: &[u8]) -> Result<(), ()> { + if !self.in_bootloader { + if let Err(err) = self.reset_into_bootloader().await { + return Err(err); + } + } + + if let Err(err) = self.verify_bootloader().await { + return Err(err); + } + + match self.get_device_id().await { + Err(err) => return Err(err), + Ok(device_id) => match device_id { + 68 => { + defmt::trace!("found stm32f1 device"); + } + 19 => { + defmt::trace!("found stm32f40xxx device"); + } + 105 => { + defmt::trace!("found stm32g474xx device"); + } + _ => { + defmt::error!("found unknown device id {}", device_id); + return Err(()); + } + } + }; + + // Erase up to Page 31 since the last page is used for current calibration constants. + if let Err(err) = self.erase_device_memory_to_page(0, 15).await { + return Err(err); + } + + // Split up to 2 commands to reduce UART packet size. + if let Err(err) = self.erase_device_memory_to_page(16, MOTOR_CURRENT_PAGE).await { + return Err(err); + } + + // program image + if let Err(err) = self.write_device_memory(fw_image_bytes, None).await { + return Err(err); + } + + self.reset_into_program(true).await; + + Ok(()) + } + + pub async fn write_current_calibration_constants(&mut self, current_constant: f32) -> Result<(), ()> { + if !self.in_bootloader { + if let Err(err) = self.reset_into_bootloader().await { + return Err(err); + } + } + + if let Err(err) = self.verify_bootloader().await { + return Err(err); + } + + match self.get_device_id().await { + Err(err) => return Err(err), + Ok(device_id) => match device_id { + 68 => { + defmt::trace!("found STSPINF0 device"); + } + _ => { + defmt::error!("Invalid device id for current calibration constants {}", device_id); + return Err(()); + } + } + }; + + // Erase the last page + if let Err(err) = self.erase_device_memory_single(MOTOR_CURRENT_PAGE).await { + return Err(err); + } + + // Write the constants to the last page but with the magic number prepended + let mut data_to_write = [0u8; 8]; + data_to_write[..4].copy_from_slice(&MOTOR_CURRENT_MAGIC); + // Convert the f32 to bytes and write it after the magic number + let current_bytes = current_constant.to_le_bytes(); + data_to_write[4..8].copy_from_slice(¤t_bytes); + + if let Err(err) = self.write_device_memory(&data_to_write, Some(MOTOR_CURRENT_START_ADDRESS)).await { + return Err(err); + } + + Ok(()) + } + pub async fn execute_code(&mut self, start_address: Option) -> Result<(), ()> { defmt::warn!("firmware jump/go command implementation does not work on some parts. If this appears not to work, consider simply resetting the device."); From 050a2903f5b82670c8ff91196ce5aa02545a226f Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 28 Sep 2025 22:40:26 -0400 Subject: [PATCH 09/18] curr sen updates in progress --- motor-controller/bin/wheel/main.c | 10 +- motor-controller/common/current_sensing.c | 483 ++++++++++++---------- motor-controller/common/current_sensing.h | 111 +++-- 3 files changed, 356 insertions(+), 248 deletions(-) diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index a04f6103..898855f7 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -68,9 +68,6 @@ int main() { uint32_t ticks_since_last_command_packet = 0; bool telemetry_enabled = false; - // initialize current sensing setup - ADC_Result_t res; - currsen_setup(ADC_MODE, &res, ADC_NUM_CHANNELS, ADC_CH_MASK, ADC_SR_MASK); // initialize motor driver pwm6step_setup(); @@ -83,6 +80,13 @@ int main() { quadenc_setup(); quadenc_reset_encoder_delta(); + // initialize current sensing setup + CS_Status_t cs_status = currsen_setup(ADC_CH_MASK); + if (cs_status != CS_OK) { + // turn on red LED to indicate error + turn_on_red_led(); + } + // Initialized response_packet here to capture the reset method. MotorResponse response_packet; memset(&response_packet, 0, sizeof(MotorResponse)); diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c index 03d47d20..3099be57 100644 --- a/motor-controller/common/current_sensing.c +++ b/motor-controller/common/current_sensing.c @@ -1,12 +1,11 @@ /** * @file main.c - * @author your name (you@domain.com) - * @brief + * @author Joe Spall + * @brief * @version 0.1 - * @date 2022-04-10 - * - * @copyright Copyright (c) 2022 - * + * + * @copyright Copyright (c) 2025 + * */ #include @@ -15,156 +14,189 @@ #include "current_sensing.h" #include "time.h" - //////////////////////////// /// ADC Channel 4 -> PA4 /// //////////////////////////// -static CS_Mode_t cs_mode; +static CS_Mode_t m_cs_mode; +static ADC_Result_t m_adc_result; +static bool m_adc_calibrated = false; + +static float m_motor_current_adc_offset = 0.0f; void currsen_enable_ht() { ADC1->CR |= ADC_CR_ADSTART; } void currsen_read_dma() { - // GPIOB->BSRR |= GPIO_BSRR_BS_9; ADC1->CR |= ADC_CR_ADSTART; } void DMA1_Channel1_IRQHandler() { - // GPIOB->BSRR |= GPIO_BSRR_BR_9; DMA1->IFCR |= DMA_IFCR_CGIF1; } -/** - * @brief run current sense - * - * @return ADC_Result result of the ADC operation - */ -// void currsen_read(ADC_Result_t *res) -// { -// const int NUM_CHANNELS = 4; - -// ADC1->CR |= ADC_CR_ADSTART; - -// //ADC_Result_t res; -// res->status = CS_OK; -// while (1) -// { - -// for (int i = 0; i < NUM_CHANNELS; i++) -// { -// // Start ADC conversion -// ADC1->CR |= ADC_CR_ADSTART; - -// // Wait until end of conversion -// uint32_t timeout_count = 0; -// while ((ADC1->ISR & ADC_ISR_EOC) == 0) -// { -// if (timeout_count < ADC_DIS_TIMEOUT) -// { -// //wait_ms(1); -// //timeout_count += 1; -// } -// else -// { -// res->status = CS_TIMEOUT; -// //return res; -// } -// } -// // Store the ADC conversion -// uint16_t currADC = ADC1->DR; -// switch (i) -// { -// case 0: -// res->I_motor_filt = currADC; -// //return; -// break; -// case 1: -// res->I_motor = currADC; -// break; -// case 2: -// res->T_spin = currADC; -// break; -// case 3: -// res->V_int = currADC; -// return; -// //return res; -// } -// } -// } - -// } - - /** * @brief configure, calibrate, and enable the ADC - * + * * @return CS_Status_t status of the operation */ -CS_Status_t currsen_setup(CS_Mode_t mode, ADC_Result_t *res, uint8_t num_ch, uint32_t ch_sel, uint32_t sr_sel) +CS_Status_t currsen_setup(uint8_t motor_adc_ch) { - cs_mode = mode; - - memset(res, 0, sizeof(ADC_Result_t)); + memset(&m_adc_result, 0, sizeof(ADC_Result_t)); // Assume ADC has not been set up yet CS_Status_t status = CS_OK; - // pre config dma if enabled - if (cs_mode == CS_MODE_DMA || cs_mode == CS_MODE_TIMER_DMA) { - DMA1_Channel1->CPAR = (uint32_t) (&(ADC1->DR)); - DMA1_Channel1->CMAR = (uint32_t) res; - DMA1_Channel1->CNDTR = num_ch; - DMA1_Channel1->CCR |= (DMA_CCR_MINC | DMA_CCR_MSIZE_0 | DMA_CCR_PSIZE_0 | DMA_CCR_TEIE | DMA_CCR_CIRC); - DMA1_Channel1->CCR |= DMA_CCR_EN; - - // DMA1->IFCR |= DMA_IFCR_CGIF1; - // DMA1_Channel1->CCR |= DMA_CCR_TCIE; - - // NVIC_SetPriority(DMA1_Ch1_IRQn, 5); - // NVIC_EnableIRQ(DMA1_Ch1_IRQn); - } - - // Configuration - status = currsen_adc_conf(); + // Disable ADC before configuration. + status = currsen_adc_dis(); if (status != CS_OK) return status; - // Group Configuration - status = currsen_adc_group_config(); - - // Calibration + // DMA Setup + // Disable DMA1 Channel 1 before configuring. + DMA1_Channel1->CCR &= ~DMA_CCR_EN; + // Set DMA to 16-bit memory size (0b01) + DMA1_Channel1->CCR |= DMA_CCR_MSIZE_0; + DMA1_Channel1->CCR &= ~DMA_CCR_MSIZE_1; + // Set DMA to 16-bit peripheral size (0b01) + DMA1_Channel1->CCR |= DMA_CCR_PSIZE_0; + DMA1_Channel1->CCR &= ~DMA_CCR_PSIZE_1; + // Set DMA to circular mode + DMA1_Channel1->CCR |= DMA_CCR_CIRC; + // Increment memory address + DMA1_Channel1->CCR |= DMA_CCR_MINC; + // Set DMA Channel 1 transfer error interrupt enable. + DMA1_Channel1->CCR |= DMA_CCR_TEIE; + // Set DMA Channel 1 half transfer interrupt and transfer complete interrupt to disable. + DMA1_Channel1->CCR &= ~(DMA_CCR_HTIE | DMA_CCR_TCIE); + // Set DMA Channel 1 direction to read from peripheral. + DMA1_Channel1->CCR &= ~DMA_CCR_DIR; + // Set DMA Channel 1 priority to very high. + DMA1_Channel1->CCR |= DMA_CCR_PL; + + // Set DMA Channel 1 Peripheral Address to ADC1 Data Register. + DMA1_Channel1->CPAR = (uint32_t) (&(ADC1->DR)); + // Set DMA Channel 1 Memory Address to the result struct. + DMA1_Channel1->CMAR = (uint32_t) (&m_adc_result); + // Set DMA Channel 1 Number of Data to Transfer to the number of transfers. + // Motor and Vbus, so 2 transfers. + // Since in circular mode, this will reset. + DMA1_Channel1->CNDTR = 2; + // Enable DMA1 Channel 1. + DMA1_Channel1->CCR |= DMA_CCR_EN; + + // Calibration (should happen before DMAEN in CFGR1) status = currsen_adc_cal(); if (status != CS_OK) return status; + // Set ADC data resolution + // Based on Table 46 of RM0091. + // 12 bit has a t_SAR = 12.5 ADC clock cycles (have margin to do so below). + // Set ADC conversion data alignment - left align + MODIFY_REG( + ADC1->CFGR1, + ADC_CFGR1_RES | + ADC_CFGR1_ALIGN, + ADC_RESOLUTION_12B | + ADC_DATA_ALIGN_RIGHT); + + // Set ADC low power mode - None + MODIFY_REG( + ADC1->CFGR1, + (ADC_CFGR1_WAIT | ADC_CFGR1_AUTOFF), + ADC_LP_MODE_NONE); + + // Set ADC clock + // Based on Table 44 of RM0091. + // PCLK DIV 4, Latency on starting sample is deterministic. + // t_LATENCY = 2.625 ADC clock cycles. + // PCLK (48 MHz) / 4 = 12 MHz ADC clock. + // Must be DIV 4 for 48 MHz PCLK to be less than 14 MHz ADC clock. + // NOTE: ST does clock async CKMODE = 00. We are not doing the + // fastest (14 MHz > 12 MHz) but there is no jitter on start timing. + // From Table 50 of stm32f031c4.pdf + // W_LATENCY = 8.5 ADC clock cycles for PCLK/4. + MODIFY_REG( + ADC1->CFGR2, + ADC_CFGR2_CKMODE, + ADC_CFGR2_CKMODE_1); + + // TIM1 Ch4 Falling Edge trigger + // TIM1 -> Triggering 48 kHz + // 1 / 48 kHz = 20.833 us period + // Falling edge is used because the low-side gate should be closed + // before the ADC conversion starts. + // Not discontinuous sampling. + // Single conversion. + // Unlimited DMA transfers. + // Overwrite data on conversion. + MODIFY_REG(ADC1->CFGR1, + ADC_CFGR1_EXTSEL + | ADC_CFGR1_EXTEN + | ADC_CFGR1_DISCEN + | ADC_CFGR1_CONT + | ADC_CFGR1_DMAEN + | ADC_CFGR1_DMACFG + | ADC_CFGR1_OVRMOD + , + ADC_REG_TRIG_EXT_TIM1_CH4 + | ADC_REG_SEQ_DISCONT_DISABLE + | ADC_REG_CONV_SINGLE + | ADC_REG_DMA_TRANSFER_UNLIMITED + | ADC_REG_OVR_DATA_OVERWRITTEN); + + /* + FUTURE: If doing temperature, need sampling of ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1. + Motors need 0, so need to switch sampling duration on the fly so would require + DMA + ADC trigger. + + // Temperature sensor values + // Table 53. STM32F031x6.pdf + // ts_temp minimum ADC sampling time -> 4 us + // At 12 MHz ADC clock -> 48 ADC clock cycles. + // SMP must be 101 (55.5 ADC clock cycles) to 111 (239.5) for temperature sensor. + // Wake-up the Temperature Sensor + ADC->CCR |= (ADC_CCR_TSEN); + + // Since sampling + conversion period must be less than + // 20.833 us, we need to set the sampling time to be less, so + // SMP must be less than 111 (239.5 ADC clock cycles). + + // Total sampling + conversion time + // From Figure 31 of RM0091 + // Total first conversion -> (t_LATENCY + t_SMPL + t_SAR + W_LATENCY) + // All following conversions -> (t_SMPL + t_SAR + W_LATENCY) + // t_SMPL -> based on SMP[2:0] bits. Min is 1.5 and max is 239.5 ADC clock cycles (13.11.6 in RM0091). + // SMP = 110 (71.5 ADC clock cycles) seems to be the minimum for temperature sensor. + // Total first conversion -> (2.625 + 71.5 + 12.5 + 8.5) = 95.125 ADC clock cycles. + // first conversion * (1 / 12 MHz ADC Clock) = 7.92708 us for 1 channel. + // All following conversions -> (71.5 + 12.5 + 8.5) = 92.5 ADC clock cycles. + // following conversions * (1 / 12 MHz ADC Clock) = 7.7083 us for each additional. + // So if we do one motor and temp sensor, we are at 7.92708 + 7.7083 = 15.63538 us. + // FUTURE ^ This is not enough time I think so got to do the on the fly sampling. + ADC1->SMPR = ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1; + + // Set ADC channel selection + // Channel 16 is the temperature sensor. + ADC1->CHSELR = motor_adc_ch | ADC_CHSELR_CHSEL16; + */ + + // 7.5 ADC clock cycles (0.625 us) is the minimum sampling time for the motor. + // T_settling > ((Imax * Rs * G_real) / SR) = ((10.0 * 0.05 * 4.94) / 10V/us) = 0.247us + ADC1->SMPR = ADC_SMPR_SMP_0; + // ADC1->SMPR = 0; + + + // Set ADC channel selection. Ch9 is the Vbus. + ADC1->CHSELR = motor_adc_ch | ADC_CHSELR_CHSEL9; // Enable status = currsen_adc_en(); if (status != CS_OK) return status; - // Select ch 3 (pot), ch 4 (motor pin), ch5 (Vbat), ch16 (Temp) ch 17 (Vref), ch 18 (Vbat - internal spec not Vbus motor) - // in deployed firmware don't check pot, convert motor first - //ADC1->CHSELR = ADC_CHSELR_CHSEL3 | ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL5 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17; // | ADC_CHSELR_CHSEL18; - ADC1->CHSELR = ch_sel; - // ADC1->CHSELR = ADC_CHSELR_CHSEL3; - // Select a sampling mode of 111 i.e. 239.5 ADC clk to be greater than 17.1us - // at 10B rest 11.5 cycles + (sm 111)=239.5 = 251 cycles - // useing PCLK / 2 = 48MHz / 2 = 24MHz = 1 / 24_000_000 = 0.000_000_041 ADC clk - // 251 cycles -> 10.2 us conversion (max precision) - // - // PWM @48kHz = period 20.8uS - // have a max of 5 conversions, < 4us per conversion - // pick sm 110 = 71.5 clock cycles - // total conversion = 11.5 + 71.5 = 83 cyc - // 83 cyc * 0.000_000_041 = 3.4us per sample, 3.4 * 5 = 17us - // default 3 cycle + sample cycles * ADC clk 14MHz 0.000_000_071 s per cycle; - // TODO Need to configure timing based on rise resistance - //ADC1->SMPR |= ADC_SMPR_SMP_0 | ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2; - ADC1->SMPR = sr_sel; - // fires interrupt on end of sequence to drop GPIOB, pin 9 // used for current sampling/timing verification // ADC1->IER |= (ADC_IER_EOSEQIE); @@ -175,92 +207,23 @@ CS_Status_t currsen_setup(CS_Mode_t mode, ADC_Result_t *res, uint8_t num_ch, uin return status; } -void ADC1_IRQHandler() { - GPIOB->BSRR |= GPIO_BSRR_BR_9; - ADC1->ISR |= (ADC_ISR_EOSEQ); -} - -/** - * @brief configure the ADC group - * - * @return CS_Status_t status of the operation - */ -CS_Status_t currsen_adc_group_config() -{ - // Software trigger - // Not discontinuous sampling - // Continuous conversion - // No DMA - // Overwrite data - MODIFY_REG(ADC1->CFGR1, - ADC_CFGR1_EXTSEL - | ADC_CFGR1_EXTEN - | ADC_CFGR1_DISCEN - | ADC_CFGR1_CONT - | ADC_CFGR1_DMAEN - | ADC_CFGR1_DMACFG - | ADC_CFGR1_OVRMOD - , - ADC_REG_TRIG_EXT_TIM1_CH4 - // ((cs_mode == CS_MODE_TIMER_DMA) ? ADC_REG_TRIG_EXT_TIM1_TRGO : ADC_REG_TRIG_SOFTWARE) - | ADC_REG_SEQ_DISCONT_DISABLE //| ADC_REG_CONV_CONTINUOUS - | ADC_REG_CONV_SINGLE - | ADC_REG_DMA_TRANSFER_UNLIMITED // | ADC_REG_DMA_TRANSFER_NONE - | ADC_REG_OVR_DATA_OVERWRITTEN - ); - - // Wake-up the VREFINT (only for VBAT, Temp sensor and VRefInt) - // ADC->CCR |= ADC_CCR_VREFEN; // enable internal Vref source - // ADC->CCR |= ADC_CCR_TSEN; // enable internal temp source - - return CS_OK; -} - - -/** - * @brief configure the ADC - * - * @return CS_Status_t status of the operation - */ -CS_Status_t currsen_adc_conf() -{ - // Assume ADC not enabled - // TODO check if ADC enabled - - // Set ADC data resolution - // 10 bit, 11.5 t_SAR, 1.5 t_SMPL = 13 t_CONV clock cycles - // Set ADC conversion data alignment - left align - // Set ADC low power mode - None - MODIFY_REG(ADC1->CFGR1, - ADC_CFGR1_RES - | ADC_CFGR1_ALIGN - | ADC_CFGR1_WAIT - | ADC_CFGR1_AUTOFF - , - ADC_RESOLUTION_12B - | ADC_DATA_ALIGN_LEFT - | ADC_LP_MODE_NONE - ); - - // Set ADC clock - // PCLK DIV 4, Latency is deterministic (no jitter) and equal to - // 2.75 ADC clock cycles - MODIFY_REG(ADC1->CFGR2, - ADC_CFGR2_CKMODE - , - ADC_CFGR2_CKMODE_1 - ); - - return CS_OK; -} +//void ADC1_IRQHandler() { +// GPIOB->BSRR |= GPIO_BSRR_BR_9; +// ADC1->ISR |= (ADC_ISR_EOSEQ); +//} /** * @brief calibrate the ADC - * + * * @return CS_Status_t status of the operation */ CS_Status_t currsen_adc_cal() { + if (m_adc_calibrated) { + // If already calibrated, return OK. + return CS_OK; + } + // Ensure that ADEN = 0 before calibration if ((ADC1->CR & ADC_CR_ADEN) != 0) { @@ -283,10 +246,10 @@ CS_Status_t currsen_adc_cal() } } - // Clear DMAEN - ADC1->CFGR1 &= ~ADC_CFGR1_DMAEN; - // Launch the calibration by setting ADCAL - ADC1->CR |= ADC_CR_ADCAL; + // Clear DMAEN just to be safe. + ADC1->CFGR1 &= ~ADC_CFGR1_DMAEN; + // Launch the calibration by setting ADCAL + ADC1->CR |= ADC_CR_ADCAL; // Waiting for calibration to finish, timeout if too long timeout_count = 0; @@ -303,30 +266,28 @@ CS_Status_t currsen_adc_cal() } } + m_adc_calibrated = true; + return CS_OK; } - /** * @brief enables the ADC - * + * * @return CS_Status_t status of the operation */ CS_Status_t currsen_adc_en() -{ +{ // Check if ADC is already powered up, and clear if so if ((ADC1->ISR & ADC_ISR_ADRDY) != 0) { - // Clear ADRDY + // Clear ADRDY ADC1->ISR |= ADC_ISR_ADRDY; } // Enable the ADC ADC1->CR |= ADC_CR_ADEN; - if (cs_mode == CS_MODE_DMA || cs_mode == CS_MODE_TIMER_DMA) { - ADC1->CFGR1 |= ADC_CFGR1_DMAEN; - } - + // Waiting for ADC to be powered up, timeout if too long uint32_t timeout_count = 0; while ((ADC1->ISR & ADC_ISR_ADRDY) == 0) @@ -342,20 +303,31 @@ CS_Status_t currsen_adc_en() } } + // Enable the DMA after the ADC is powered up. + ADC1->CFGR1 |= ADC_CFGR1_DMAEN; + return CS_OK; } /** - * @brief disables the ADC - * + * @brief disables the ADC. Procedure pulled directly from RM0091 page 238. + * * @return CS_Status_t status of the operation */ CS_Status_t currsen_adc_dis() -{ - // Stop any ongoing conversion +{ + // Check if ADC is already disabled + // (not enabled and not converting). + if ((ADC1->CR & ADC_CR_ADEN) == 0 && + (ADC1->CR & ADC_CR_ADSTART) == 0) + { + return CS_OK; + } + + // Stop any ongoing conversion. ADC1->CR |= ADC_CR_ADSTP; - // Waiting until conversion is stopped + // Waiting until conversion is stopped. uint32_t timeout_count = 0; while ((ADC1->CR & ADC_CR_ADSTP) != 0) { @@ -372,7 +344,7 @@ CS_Status_t currsen_adc_dis() // Disable the ADC ADC1->CR |= ADC_CR_ADDIS; - // Waiting until ADC is fully disabled + // Waiting until ADC is fully disabled. timeout_count = 0; while ((ADC1->CR & ADC_CR_ADEN) != 0) { @@ -388,4 +360,87 @@ CS_Status_t currsen_adc_dis() } return CS_OK; +} + +/** + * @brief gets the motor current. Translate from raw ADC value to + * to scaled raw ADC value to motor current. + * + * @return float motor current + */ +float currsen_get_motor_current() +{ + float v_adc = ((float) (m_adc_result.Motor_current_raw)) * V_ADC_SCALE_V; + float v_motor_sense = (v_adc - V_MIN_OP_AMP) / MOTOR_OPAMP_GAIN_REAL; + float i_motor = v_motor_sense / MOTOR_OPAMP_RESISTOR_SENSE; + + return i_motor; +} + +float currsen_get_motor_current_offset() +{ + return m_motor_current_adc_offset; +} + +float currsen_get_motor_current_with_offset() +{ + // Get the current in amps from the current sense ADC. + // Should always be positive with the current electrical architecture. + float i_motor = currsen_get_motor_current(); + // Apply the offset to the motor current. + i_motor -= currsen_get_motor_current_offset(); + + if (i_motor < 0.0f) { + // If the current is negative, set it to 0.0f. + // This is to prevent negative current readings. + i_motor = 0.0f; + } + + return i_motor; +} + +/** + * @brief gets the Vbus voltage. Translate from raw ADC value to + * to Vbus voltage in volts. + * + * @return float Vbus voltage + */ +float currsen_get_vbus_voltage() +{ + return VBUS_SCALE * ((float) m_adc_result.Vbus_raw); +} + +/** + * @brief gets the temperature. Translate from raw ADC value to + * to scaled raw ADC value to temperature. + * + * @return int32_t temperature in C + */ + +int32_t currsen_get_temperature() +{ + // From A.7.16 of RM0091 + int32_t temperature; + // Scale the raw ADC value to the VDD_CALIB value and center based on the + // temperature calibration points. + temperature = (((int32_t) m_adc_result.Spin_temperature_raw * VDD_APPLI / VDD_CALIB) - (int32_t)(*TEMP30_CAL_ADDR)); + // Scale by the difference between the two calibration points. + temperature = temperature * (int32_t)(110 - 30); + temperature = temperature / (int32_t)(*TEMP110_CAL_ADDR - *TEMP30_CAL_ADDR); + // Add the offset of 30 degrees Celsius from the base calibration point. + temperature = temperature + 30; + return temperature; +} + +void init_motor_current_adc_offset() +{ + // Read the motor current offset from flash. + // If not set, set to 0.0f. + volatile uint32_t* current_offset_ptr = (uint32_t*) CURRENT_OFFSET_ADDRESS; + if (*current_offset_ptr != CURRENT_OFFSET_MAGIC) { + m_motor_current_adc_offset = 0.0f; + } else { + // If the magic number matches, read the float value one after the magic number. + m_motor_current_adc_offset = *((float*) (current_offset_ptr + 1)); + } } \ No newline at end of file diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h index dbeeaf08..c33d8c38 100644 --- a/motor-controller/common/current_sensing.h +++ b/motor-controller/common/current_sensing.h @@ -1,28 +1,75 @@ /** * @file setup.h - * @author your name (you@domain.com) - * @brief + * @author Joe Spall + * @brief * @version 0.1 - * @date 2022-04-10 - * - * @copyright Copyright (c) 2022 - * + * + * @copyright Copyright (c) 2025 + * */ #pragma once +#include +#include + +static const float V_DDA_MV = 3000.0f; // mV +static const float V_DDA_V = 3.0f; // V +static const float V_ADC_SCALE_MV = (V_DDA_MV / 4095.0f); // mV +static const float V_ADC_SCALE_V = (V_DDA_V / 4095.0f); // V + +// From A.7.16 of RM0091 +#define TEMP110_CAL_ADDR ((uint16_t*) ((uint32_t) 0x1FFFF7C2)) +#define TEMP30_CAL_ADDR ((uint16_t*) ((uint32_t) 0x1FFFF7B8)) + +#define VDD_CALIB ((uint16_t) (3300)) +#define VDD_APPLI ((uint16_t) (V_DDA_MV)) + +// Motor Current scaling +// In v1.0 motor-controller board, schematic based off of AN5397. +// Rs = 2x 0.1 ohm in parallel -> 0.05 ohm +static const float MOTOR_OPAMP_RESISTOR_A = 140E3f; // ohm +static const float MOTOR_OPAMP_RESISTOR_B = 2.370E3f; // ohm +static const float MOTOR_OPAMP_RESISTOR_1 = 10E3f; // ohm +static const float MOTOR_OPAMP_RESISTOR_2 = 40.2E3f; // ohm +static const float MOTOR_OPAMP_RESISTOR_SENSE = 0.05f; // ohm +// V_dyn_range = VDD - (0.2 * 2) = 2.6V +// NOTE: 0.2V is the safety margin for the op-amp from STSPIN32 datasheet. +// SR = 10V/us op-amp slew rate +// Gmax = V_dyn_range / (Imax * Rs) = 2.6 / (10.0 * 0.05) = 5.2 +// NOTE: Using Imax * Rs instead of 2 * Imax * Rs since not measuring negative current. +// G_real = (Ra/(Ra + Rb)) * (1 + R2/R1) = (140k/(140k + 2.37k)) * (1 + 40.2k/10.0k) = 0.983 * 5.02 = 4.94 +static const float MOTOR_OPAMP_GAIN_REAL = ((MOTOR_OPAMP_RESISTOR_A / (MOTOR_OPAMP_RESISTOR_A + MOTOR_OPAMP_RESISTOR_B)) * (1.0f + (MOTOR_OPAMP_RESISTOR_2 / MOTOR_OPAMP_RESISTOR_1))); +// BW = GBP/(1 + R2/R1) = 18MHz / (1 + 40.2k/10k) = 18MHz / 5.02 = 3.58MHz +// Closed Loop Gain = 1 + R2/R1 = 1 + 40.2k/10k = 5.02 > Closed Loop Gain Min of STSPIN = 4 +// T_settling > ((Imax * Rs * G_real) / SR) = ((10.0 * 0.05 * 4.94) / 10V/us) = 0.247us + +// Op-amp is configured so 0A motor current = 0.2V +static const float V_MIN_OP_AMP = 0.2f; // V + +// VBUS voltage scaling +// 11.5k / 1k resistor divider -> 12.5 scaling +// 12.5 * V_ADC_SCALE = VBUS_SCALE +// V_ADC_SCALE_V = 3.0V / 4095.0f ADC resolution +// NOTE: Compute ahead of time to reduce floating point math. +// V_bus = V_adc_raw * VBUS_SCALE +static const float VBUS_SCALE = 0.0091575f; + +// TODO Move to software-communication +static const uint32_t CURRENT_OFFSET_ADDRESS = 0x08007C00; +static const uint32_t CURRENT_OFFSET_MAGIC = 0xAABBCCDD; + typedef enum { - CS_MODE_POLLING, - CS_MODE_DMA, - CS_MODE_TIMER_DMA + CS_MODE_PWM_DMA, + CS_MODE_SOFTWARE } CS_Mode_t; -typedef enum +typedef enum { - CS_OK = 0x00U, - CS_ERROR = 0x01U, - CS_BUSY = 0x02U, - CS_TIMEOUT = 0x03U + CS_OK = 0x00U, + CS_ERROR = 0x01U, + CS_BUSY = 0x02U, + CS_TIMEOUT = 0x03U } CS_Status_t; #define SET_BIT(REG, BIT) ((REG) |= (BIT)) @@ -48,8 +95,7 @@ typedef enum #define ADC_LP_MODE_NONE (0x00000000U) /*!< No ADC low power mode activated */ // ADC Trigger mode -// #define ADC_REG_TRIG_EXT_EDGE_DEFAULT (ADC_CFGR1_EXTEN_1) // falling edge only -#define ADC_REG_TRIG_EXT_EDGE_DEFAULT (ADC_CFGR1_EXTEN_0) // rising edge only +#define ADC_REG_TRIG_EXT_EDGE_DEFAULT (ADC_CFGR1_EXTEN_1) // falling edge only #define ADC_REG_TRIG_SOFTWARE (0x00000000U) /*!< ADC group regular conversion trigger internal: SW start. */ #define ADC_REG_TRIG_EXT_TIM1_TRGO (ADC_REG_TRIG_EXT_EDGE_DEFAULT) /*!< ADC group regular conversion trigger from external IP: TIM1 TRGO. Trigger edge set to rising edge (default setting). */ #define ADC_REG_TRIG_EXT_TIM1_CH4 (ADC_CFGR1_EXTSEL_0 | ADC_REG_TRIG_EXT_EDGE_DEFAULT) /*!< ADC group regular conversion trigger from external IP: TIM1 channel 4 event (capture compare: input capture or output capture). Trigger edge set to rising edge (default setting). */ @@ -83,32 +129,35 @@ typedef enum // TODO tune timing #define ADC_STP_TIMEOUT 5 //ms +#define ADC_SOFTWARE_CONVERSION_TIMEOUT 2 // ms +#define ADC_MOTOR_CURRENT_ZERO_SAMPLE_NUM 4 +#define UINT_12_BIT_MAX 4095 // 2^12 - 1, max value for 12 bit ADC -// this struct is used as a DMA target. +// this struct is used as a DMA target. // ADC->DR reads are two bytes, DMA will do half word transfers // rm0091 tells us the 16->32 bit port mapping packing scheme // which all us to derive that this struct should be packed -// hald words. Additionally, result must be at the end since a +// half words. Additionally, result must be at the end since a // ref to this struct will be passed into DMAR register for N // transfers -typedef struct +typedef struct __attribute__((__packed__)) ADC_Result { - // uint16_t I_motor; - uint16_t I_motor_filt; - uint16_t V_batt; - // uint16_t T_spin; - // uint16_t V_int; - CS_Status_t status; + uint16_t Motor_current_raw; + uint16_t Vbus_raw; + uint16_t Spin_temperature_raw; } ADC_Result_t; void currsen_enable_ht(); void currsen_read_dma(); -void currsen_read(ADC_Result_t *res); -CS_Status_t currsen_setup(CS_Mode_t mode, ADC_Result_t *res, uint8_t num_ch, uint32_t ch_sel, uint32_t sr_sel); -CS_Status_t currsen_adc_group_config(); -CS_Status_t currsen_adc_conf(); +CS_Status_t currsen_setup(uint8_t motor_adc_ch); CS_Status_t currsen_adc_cal(); CS_Status_t currsen_adc_en(); CS_Status_t currsen_adc_dis(); - - +CS_Status_t calculate_motor_zero_current_setpoint(); + +float currsen_get_motor_current(); +float currsen_get_motor_current_with_offset(); +float currsen_get_motor_current_offset(); +float currsen_get_vbus_voltage(); +int32_t currsen_get_temperature(); +uint16_t currsen_get_motor_current_zero_adc_raw(); From 6ea48b5530784f53a9194a4fa6f6e100fc7bd04c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 30 Sep 2025 22:24:44 -0400 Subject: [PATCH 10/18] merge of joe currsen API working, simple auto current cal added --- motor-controller/bin/dribbler/main.c | 9 ++++-- motor-controller/bin/wheel-test/main.c | 10 ++++--- motor-controller/bin/wheel/main.c | 35 ++++++++++++----------- motor-controller/common/current_sensing.c | 21 ++++++++++++++ motor-controller/common/current_sensing.h | 7 +++-- 5 files changed, 57 insertions(+), 25 deletions(-) diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index a0387ad5..eb18e3a2 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -59,8 +59,11 @@ int main() { bool telemetry_enabled = false; // initialize current sensing setup - ADC_Result_t res; - currsen_setup(ADC_MODE, &res, ADC_NUM_CHANNELS, ADC_CH_MASK, ADC_SR_MASK); + CS_Status_t cs_status = currsen_setup(ADC_CH_MASK); + if (cs_status != CS_OK) { + // turn on red LED to indicate error + turn_on_red_led(); + } // initialize motor driver pwm6step_setup(); @@ -213,7 +216,7 @@ int main() { bool run_telemetry = time_sync_ready_rst(&telemetry_timer); if (run_torque_loop) { - float cur_measurement = ((float) res.I_motor_filt / (float) UINT16_MAX) * AVDD_V; + float cur_measurement = currsen_get_motor_current_with_offset(); // TODO: recover current from voltage // TODO: estimate torque from current // TODO: filter? diff --git a/motor-controller/bin/wheel-test/main.c b/motor-controller/bin/wheel-test/main.c index c87abb25..8b8445aa 100644 --- a/motor-controller/bin/wheel-test/main.c +++ b/motor-controller/bin/wheel-test/main.c @@ -62,8 +62,11 @@ int main() { bool telemetry_enabled = false; // initialize current sensing setup - ADC_Result_t res; - currsen_setup(ADC_MODE, &res, ADC_NUM_CHANNELS, ADC_CH_MASK, ADC_SR_MASK); + CS_Status_t cs_status = currsen_setup(ADC_CH_MASK); + if (cs_status != CS_OK) { + // turn on red LED to indicate error + turn_on_red_led(); + } // initialize motor driver pwm6step_setup(); @@ -293,9 +296,8 @@ int main() { if (run_torque_loop) { // recover torque using the shunt voltage drop, amplification network model and motor model // pct of voltage range 0-3.3V - float current_sense_shunt_v = ((float) res.I_motor_filt / (float) UINT16_MAX) * AVDD_V; // map voltage given by the amp network to current - float current_sense_I = mm_voltage_to_current(&df45_model, current_sense_shunt_v); + float current_sense_I = currsen_get_motor_current_with_offset(); // map current to torque using the motor model float measured_torque_Nm = mm_current_to_torque(&df45_model, current_sense_I); // filter torque diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 898855f7..b9bbf6a2 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -57,18 +57,28 @@ int main() { // Setups clocks setup(); + // setup encoder + quadenc_setup(); + quadenc_reset_encoder_delta(); + + // initialize current sensing setup + CS_Status_t cs_status = currsen_setup(ADC_CH_MASK); + if (cs_status != CS_OK) { + // turn on red LED to indicate error + turn_on_red_led(); + } + // start watchdog IWDG->KR = 0x0000CCCC; // enable the module IWDG->KR = 0x00005555; // enable register writes IWDG->PR = 0x4; // set prescaler to 64, 40kHz -> 625Hz, 1.6ms per tick - IWDG->RLR = 5; // count to 10 ticks, 16ms then trigger a system reset + IWDG->RLR = 10; // count to 10 ticks, 16ms then trigger a system reset while (IWDG->SR) {} // wait for value to take IWDG->KR = 0x0000AAAA; // feed the watchdog uint32_t ticks_since_last_command_packet = 0; bool telemetry_enabled = false; - // initialize motor driver pwm6step_setup(); pwm6step_set_duty_cycle_f(0.0f); @@ -76,15 +86,12 @@ int main() { // enable ADC hardware trigger (tied to 6step timer) currsen_enable_ht(); - // setup encoder - quadenc_setup(); - quadenc_reset_encoder_delta(); + wait_ms(5); - // initialize current sensing setup - CS_Status_t cs_status = currsen_setup(ADC_CH_MASK); - if (cs_status != CS_OK) { - // turn on red LED to indicate error - turn_on_red_led(); + // calibrate current + while (!currsen_calibrate_sense()) { + wait_ms(1); + IWDG->KR = 0x0000AAAA; // feed the watchdog } // Initialized response_packet here to capture the reset method. @@ -336,13 +343,9 @@ int main() { if (run_torque_loop) { // recover torque using the shunt voltage drop, amplification network model and motor model // pct of voltage range 0-3.3V - float current_sense_shunt_v = (((float) res.I_motor_filt / (float) UINT16_MAX) * AVDD_V) + 0.009f - 0.2492f; - float vbatt = ((((float) res.V_batt / (float) UINT16_MAX) * AVDD_V) / 2.016f) * 25.2f; - if (current_sense_shunt_v < 0.0f) { - current_sense_shunt_v = 0.0f; - } + float vbatt = currsen_get_vbus_voltage(); // map voltage given by the amp network to current - float current_sense_I = current_sense_shunt_v / 0.25f; // mm_voltage_to_current(&df45_model, current_sense_shunt_v); + float current_sense_I = currsen_get_motor_current_with_offset(); // mm_voltage_to_current(&df45_model, current_sense_shunt_v); // float current_sense_I = current_sense_shunt_v; // float current_sense_I = current_sense_shunt_v; // map current to torque using the motor model diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c index 3099be57..3aa7c4de 100644 --- a/motor-controller/common/current_sensing.c +++ b/motor-controller/common/current_sensing.c @@ -22,6 +22,8 @@ static CS_Mode_t m_cs_mode; static ADC_Result_t m_adc_result; static bool m_adc_calibrated = false; +static bool m_motor_current_adc_offset_set = false; +static size_t m_motor_current_adc_offset_ctr = 0; static float m_motor_current_adc_offset = 0.0f; void currsen_enable_ht() { @@ -399,6 +401,25 @@ float currsen_get_motor_current_with_offset() return i_motor; } +bool currsen_calibrate_sense() +{ + if (m_motor_current_adc_offset_set) { + m_motor_current_adc_offset_set = false; + m_motor_current_adc_offset_ctr = 0; + m_motor_current_adc_offset = 0.0f; + } + + m_motor_current_adc_offset += currsen_get_motor_current(); + m_motor_current_adc_offset_ctr++; + + if (m_motor_current_adc_offset_ctr >= 10) { + m_motor_current_adc_offset /= 10.0f; + m_motor_current_adc_offset_set = true; + } + + return m_motor_current_adc_offset_set; +} + /** * @brief gets the Vbus voltage. Translate from raw ADC value to * to Vbus voltage in volts. diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h index c33d8c38..2919a4f7 100644 --- a/motor-controller/common/current_sensing.h +++ b/motor-controller/common/current_sensing.h @@ -39,13 +39,15 @@ static const float MOTOR_OPAMP_RESISTOR_SENSE = 0.05f; // ohm // Gmax = V_dyn_range / (Imax * Rs) = 2.6 / (10.0 * 0.05) = 5.2 // NOTE: Using Imax * Rs instead of 2 * Imax * Rs since not measuring negative current. // G_real = (Ra/(Ra + Rb)) * (1 + R2/R1) = (140k/(140k + 2.37k)) * (1 + 40.2k/10.0k) = 0.983 * 5.02 = 4.94 -static const float MOTOR_OPAMP_GAIN_REAL = ((MOTOR_OPAMP_RESISTOR_A / (MOTOR_OPAMP_RESISTOR_A + MOTOR_OPAMP_RESISTOR_B)) * (1.0f + (MOTOR_OPAMP_RESISTOR_2 / MOTOR_OPAMP_RESISTOR_1))); +// static const float MOTOR_OPAMP_GAIN_REAL = ((MOTOR_OPAMP_RESISTOR_A / (MOTOR_OPAMP_RESISTOR_A + MOTOR_OPAMP_RESISTOR_B)) * (1.0f + (MOTOR_OPAMP_RESISTOR_2 / MOTOR_OPAMP_RESISTOR_1))); // BW = GBP/(1 + R2/R1) = 18MHz / (1 + 40.2k/10k) = 18MHz / 5.02 = 3.58MHz // Closed Loop Gain = 1 + R2/R1 = 1 + 40.2k/10k = 5.02 > Closed Loop Gain Min of STSPIN = 4 // T_settling > ((Imax * Rs * G_real) / SR) = ((10.0 * 0.05 * 4.94) / 10V/us) = 0.247us +static const float MOTOR_OPAMP_GAIN_REAL = 5.5; + // Op-amp is configured so 0A motor current = 0.2V -static const float V_MIN_OP_AMP = 0.2f; // V +static const float V_MIN_OP_AMP = 0.25f; // V // VBUS voltage scaling // 11.5k / 1k resistor divider -> 12.5 scaling @@ -158,6 +160,7 @@ CS_Status_t calculate_motor_zero_current_setpoint(); float currsen_get_motor_current(); float currsen_get_motor_current_with_offset(); float currsen_get_motor_current_offset(); +bool currsen_calibrate_sense(); float currsen_get_vbus_voltage(); int32_t currsen_get_temperature(); uint16_t currsen_get_motor_current_zero_adc_raw(); From 66c0235547736ea09a828ab879c02f17f684fbdc Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 30 Sep 2025 23:22:34 -0400 Subject: [PATCH 11/18] format code --- control-board/src/bin/hwtest-motor/main.rs | 1 - control-board/src/stspin_motor.rs | 13 +- control-board/src/tasks/control_task.rs | 16 +- control-board/src/tasks/kicker_task.rs | 3 +- control-board/src/tasks/radio_task.rs | 7 +- kicker-board/src/bin/hwtest-coms/main.rs | 6 +- kicker-board/src/bin/kicker/main.rs | 5 +- lib-stm32/src/drivers/boot/stm32_interface.rs | 263 ++++++++++-------- 8 files changed, 175 insertions(+), 139 deletions(-) diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index ede001b2..e599eafc 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -137,7 +137,6 @@ async fn main(main_spawner: embassy_executor::Spawner) { vel = 0.0; } - test_command_publisher .publish(DataPacket::BasicControl(BasicControl { _bitfield_1: Default::default(), diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index 7654ae4b..31e806eb 100644 --- a/control-board/src/stspin_motor.rs +++ b/control-board/src/stspin_motor.rs @@ -281,14 +281,17 @@ 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 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(); diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 149ee3cf..c3616e1b 100644 --- a/control-board/src/tasks/control_task.rs +++ b/control-board/src/tasks/control_task.rs @@ -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 // diff --git a/control-board/src/tasks/kicker_task.rs b/control-board/src/tasks/kicker_task.rs index 7bc51cce..ce3f43c6 100644 --- a/control-board/src/tasks/kicker_task.rs +++ b/control-board/src/tasks/kicker_task.rs @@ -143,8 +143,7 @@ impl< } else if self.kicker_task_state >= KickerTaskState::ConnectUart { // Check if connection timeout occurred let time_elapsed = - Instant::duration_since(&Instant::now(), connection_timeout_start) - .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; diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 3ed734d6..891d4c95 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -392,8 +392,7 @@ impl< // reboot the robot (unless we had a shutdown request). let cur_time = Instant::now(); if !cur_robot_state.shutdown_requested - && Instant::duration_since(&cur_time, self.last_software_packet) - .as_millis() + && Instant::duration_since(&cur_time, self.last_software_packet).as_millis() > Self::RESPONSE_FROM_PC_TIMEOUT_MS { defmt::warn!("software timeout - rebooting..."); @@ -424,7 +423,9 @@ 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) && self.connection_state == RadioConnectionState::Connected { + 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); } diff --git a/kicker-board/src/bin/hwtest-coms/main.rs b/kicker-board/src/bin/hwtest-coms/main.rs index bfc9bb3e..9926382e 100644 --- a/kicker-board/src/bin/hwtest-coms/main.rs +++ b/kicker-board/src/bin/hwtest-coms/main.rs @@ -146,10 +146,7 @@ async fn high_pri_kick_task( // send telemetry packet let cur_time = Instant::now(); - if Instant::duration_since(&cur_time, last_packet_sent_time) - .as_millis() - > 20 - { + if Instant::duration_since(&cur_time, last_packet_sent_time).as_millis() > 20 { kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( res.is_err() as u16, 0, @@ -183,7 +180,6 @@ async fn high_pri_kick_task( last_packet_sent_time = cur_time; } - // LEDs if res.is_err() { err_led.set_high(); diff --git a/kicker-board/src/bin/kicker/main.rs b/kicker-board/src/bin/kicker/main.rs index e55c93fb..63bf1f57 100644 --- a/kicker-board/src/bin/kicker/main.rs +++ b/kicker-board/src/bin/kicker/main.rs @@ -404,10 +404,7 @@ async fn high_pri_kick_task( // send telemetry packet let cur_time = Instant::now(); - if Instant::duration_since(&cur_time, last_packet_sent_time) - .as_millis() - > 20 - { + if Instant::duration_since(&cur_time, last_packet_sent_time).as_millis() > 20 { kicker_telemetry_packet._bitfield_1 = KickerTelemetry::new_bitfield_1( res.is_err() as u16, dribbler_motor_telemetry.master_error() as u16, diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index e3f02db9..1a0c7477 100644 --- a/lib-stm32/src/drivers/boot/stm32_interface.rs +++ b/lib-stm32/src/drivers/boot/stm32_interface.rs @@ -199,7 +199,9 @@ impl< defmt::debug!("bootloader reply too short after calibration."); } res - })).await; + }), + ) + .await; if sync_res.is_err() { defmt::warn!("*** HARDWARE CHECK *** - bootloader baud calibration timed out."); @@ -371,28 +373,30 @@ impl< // defmt::debug!("sending the read command..."); self.writer - .write(|buf| { - buf[0] = STM32_BOOTLOADER_CMD_READ_MEM; - buf[1] = !STM32_BOOTLOADER_CMD_READ_MEM; - //defmt::info!("send buffer {:?}", buf); - 2 - }) - .await?; + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_READ_MEM; + buf[1] = !STM32_BOOTLOADER_CMD_READ_MEM; + //defmt::info!("send buffer {:?}", buf); + 2 + }) + .await?; // Wait for the bootloader to acknowledge the command let mut res = Err(()); - self.reader.read(|buf| { - // defmt::info!("Read cmd reply {:?}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - res = Ok(()); + self.reader + .read(|buf| { + // defmt::info!("Read cmd reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("Read mem cmd replied with NACK"); + } } else { - defmt::error!("Read mem cmd replied with NACK"); + defmt::error!("Read mem cmd reply too short."); } - } else { - defmt::error!("Read mem cmd reply too short."); - } - }).await?; + }) + .await?; if res.is_err() { return res; @@ -400,33 +404,35 @@ impl< // defmt::debug!("sending the load address {:?}...", read_base_addr); self.writer - .write(|buf| { - let start_address_bytes: [u8; 4] = read_base_addr.to_be_bytes(); - let cs = Self::bootloader_checksum_u32(read_base_addr); - buf[0] = start_address_bytes[0]; - buf[1] = start_address_bytes[1]; - buf[2] = start_address_bytes[2]; - buf[3] = start_address_bytes[3]; - buf[4] = cs; - // defmt::debug!("send buffer {:?}", buf); - 5 - }) - .await?; + .write(|buf| { + let start_address_bytes: [u8; 4] = read_base_addr.to_be_bytes(); + let cs = Self::bootloader_checksum_u32(read_base_addr); + buf[0] = start_address_bytes[0]; + buf[1] = start_address_bytes[1]; + buf[2] = start_address_bytes[2]; + buf[3] = start_address_bytes[3]; + buf[4] = cs; + // defmt::debug!("send buffer {:?}", buf); + 5 + }) + .await?; res = Err(()); // Wait for the bootloader to acknowledge the address - self.reader.read(|buf| { - // defmt::info!("go cmd reply {:?}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - res = Ok(()); + self.reader + .read(|buf| { + // defmt::info!("go cmd reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("Address read mem replied with NACK"); + } } else { - defmt::error!("Address read mem replied with NACK"); + defmt::error!("Address read mem reply too short."); } - } else { - defmt::error!("Address read mem reply too short."); - } - }).await?; + }) + .await?; if res.is_err() { return res; @@ -434,30 +440,32 @@ impl< // defmt::debug!("sending the data length..."); self.writer - .write(|buf| { - let data_len_minus_one = data_len as u8 - 1; - buf[0] = data_len_minus_one; - buf[1] = !data_len_minus_one; - // defmt::debug!("send buffer {:?}", buf); - 2 - }) - .await?; + .write(|buf| { + let data_len_minus_one = data_len as u8 - 1; + buf[0] = data_len_minus_one; + buf[1] = !data_len_minus_one; + // defmt::debug!("send buffer {:?}", buf); + 2 + }) + .await?; res = Err(()); // defmt::debug!("reading the data..."); - self.reader.read(|buf| { - // defmt::info!("data reply {:?}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - data.copy_from_slice(&buf[1..]); - res = Ok(()); + self.reader + .read(|buf| { + // defmt::info!("data reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + data.copy_from_slice(&buf[1..]); + res = Ok(()); + } else { + defmt::error!("Data read mem replied with NACK"); + } } else { - defmt::error!("Data read mem replied with NACK"); + defmt::error!("Data read mem reply too short!"); } - } else { - defmt::error!("Data read mem reply too short!"); - } - }).await?; + }) + .await?; res } @@ -466,7 +474,11 @@ impl< // Have to use extended erase command since the normal erase command // doesn't seem to work? // Designed to erase a up to N + 1 page of device memory. - pub async fn erase_device_memory_to_page(&self, start_page: u8, end_page: u8) -> Result<(), ()> { + pub async fn erase_device_memory_to_page( + &self, + start_page: u8, + end_page: u8, + ) -> Result<(), ()> { if !self.in_bootloader { defmt::error!("Called bootloader operation when not in bootloader context."); return Err(()); @@ -479,26 +491,28 @@ impl< // defmt::debug!("sending the erase command..."); self.writer - .write(|buf| { - buf[0] = STM32_BOOTLOADER_CMD_EXTENDED_ERASE; - buf[1] = !STM32_BOOTLOADER_CMD_EXTENDED_ERASE; - 2 - }) - .await?; + .write(|buf| { + buf[0] = STM32_BOOTLOADER_CMD_EXTENDED_ERASE; + buf[1] = !STM32_BOOTLOADER_CMD_EXTENDED_ERASE; + 2 + }) + .await?; let mut res = Err(()); - self.reader.read(|buf| { - // defmt::info!("erase cmd reply {:?}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - res = Ok(()); + self.reader + .read(|buf| { + // defmt::info!("erase cmd reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + } else { + defmt::error!("Bootloader replied to erase command with NACK"); + } } else { - defmt::error!("Bootloader replied to erase command with NACK"); + defmt::error!("Erase command reply too short."); } - } else { - defmt::error!("Erase command reply too short."); - } - }).await?; + }) + .await?; if res.is_err() { return res; @@ -506,50 +520,53 @@ impl< // defmt::debug!("sending the page number..."); self.writer - .write(|buf| { - // Quantity is N + 1 lead with MSB. Limited to 32 pages on STM32F1. - let erase_page_quantity = end_page - start_page; - buf[0] = 0x00; - buf[1] = erase_page_quantity; - // Need to send the page number for each page to erase in two bytes, MSB first - let mut checksum = erase_page_quantity; - // Track the index for use in returning the buffer length - let mut buf_indx: usize = 2; - for i in start_page ..= end_page { - // Won't erase more than 256 pages, so always lead with 0x00 - buf[buf_indx] = 0x00; - buf[buf_indx + 1] = i; - buf_indx += 2; - // Checksum is XOR of all previous bytes. Ignore 0x00 so just LSB - checksum ^= i; - } - // Checksum for all previous bytes is just the erase page number - buf[buf_indx] = checksum; - // defmt::debug!("send buffer {:?}", buf); - // Final size is buf_indx + 1 from checksum byte - buf_indx + 1 - }) - .await?; + .write(|buf| { + // Quantity is N + 1 lead with MSB. Limited to 32 pages on STM32F1. + let erase_page_quantity = end_page - start_page; + buf[0] = 0x00; + buf[1] = erase_page_quantity; + // Need to send the page number for each page to erase in two bytes, MSB first + let mut checksum = erase_page_quantity; + // Track the index for use in returning the buffer length + let mut buf_indx: usize = 2; + for i in start_page..=end_page { + // Won't erase more than 256 pages, so always lead with 0x00 + buf[buf_indx] = 0x00; + buf[buf_indx + 1] = i; + buf_indx += 2; + // Checksum is XOR of all previous bytes. Ignore 0x00 so just LSB + checksum ^= i; + } + // Checksum for all previous bytes is just the erase page number + buf[buf_indx] = checksum; + // defmt::debug!("send buffer {:?}", buf); + // Final size is buf_indx + 1 from checksum byte + buf_indx + 1 + }) + .await?; // defmt::debug!("wait for erase reply"); - self.reader.read(|buf| { - // defmt::info!("erase reply {:?}", buf); - if buf.len() >= 1 { - if buf[0] == STM32_BOOTLOADER_ACK { - res = Ok(()); - // defmt::info!("erase accepted."); - } else { - defmt::error!("bootloader replied to erase payload with NACK"); + self.reader + .read(|buf| { + // defmt::info!("erase reply {:?}", buf); + if buf.len() >= 1 { + if buf[0] == STM32_BOOTLOADER_ACK { + res = Ok(()); + // defmt::info!("erase accepted."); + } else { + defmt::error!("bootloader replied to erase payload with NACK"); + } } - } - }).await?; + }) + .await?; res } // Single page of device memory is just a page to the same page. pub async fn erase_device_memory_single(&self, erase_page: u8) -> Result<(), ()> { - self.erase_device_memory_to_page(erase_page, erase_page).await + self.erase_device_memory_to_page(erase_page, erase_page) + .await } async fn write_device_memory_chunk(&self, data: &[u8], write_base_addr: u32) -> Result<(), ()> { @@ -814,7 +831,7 @@ impl< defmt::error!("found unknown device id {}", device_id); return Err(()); } - } + }, }; // Erase up to Page 31 since the last page is used for current calibration constants. @@ -823,7 +840,10 @@ impl< } // Split up to 2 commands to reduce UART packet size. - if let Err(err) = self.erase_device_memory_to_page(16, MOTOR_CURRENT_PAGE).await { + if let Err(err) = self + .erase_device_memory_to_page(16, MOTOR_CURRENT_PAGE) + .await + { return Err(err); } @@ -837,7 +857,10 @@ impl< Ok(()) } - pub async fn write_current_calibration_constants(&mut self, current_constant: f32) -> Result<(), ()> { + pub async fn write_current_calibration_constants( + &mut self, + current_constant: f32, + ) -> Result<(), ()> { if !self.in_bootloader { if let Err(err) = self.reset_into_bootloader().await { return Err(err); @@ -855,10 +878,13 @@ impl< defmt::trace!("found STSPINF0 device"); } _ => { - defmt::error!("Invalid device id for current calibration constants {}", device_id); + defmt::error!( + "Invalid device id for current calibration constants {}", + device_id + ); return Err(()); } - } + }, }; // Erase the last page @@ -873,7 +899,10 @@ impl< let current_bytes = current_constant.to_le_bytes(); data_to_write[4..8].copy_from_slice(¤t_bytes); - if let Err(err) = self.write_device_memory(&data_to_write, Some(MOTOR_CURRENT_START_ADDRESS)).await { + if let Err(err) = self + .write_device_memory(&data_to_write, Some(MOTOR_CURRENT_START_ADDRESS)) + .await + { return Err(err); } From 0ff10995cbe7b3d708acf9eb92b1f328f997a8cc Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 12 Oct 2025 22:34:56 -0400 Subject: [PATCH 12/18] motor commutation and PWM fixes --- control-board/src/bin/hwtest-motor/main.rs | 2 +- motor-controller/common/6step.c | 179 ++++++++++++++------- motor-controller/common/6step.h | 21 +-- 3 files changed, 130 insertions(+), 72 deletions(-) diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index e599eafc..c4085c5e 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -132,7 +132,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { if ctr > 100 { ctr = 0; } else if ctr > 50 { - vel = 2.0; + vel = 1.0; } else { vel = 0.0; } diff --git a/motor-controller/common/6step.c b/motor-controller/common/6step.c index 48195dec..f64c86ee 100644 --- a/motor-controller/common/6step.c +++ b/motor-controller/common/6step.c @@ -29,6 +29,7 @@ #include #include "6step.h" +#include "debug.h" #include "system.h" #include "time.h" @@ -219,7 +220,7 @@ static uint8_t ccw_expected_hall_transition_table[8] = { static void pwm6step_setup_hall_timer(); static void pwm6step_setup_commutation_timer(uint16_t duty_cycle); static void TIM2_IRQHandler_HallTransition(); -static void perform_commutation_cycle(); +static void perform_commutation_cycle(bool); static uint8_t read_hall(); static void set_commutation_estop(); static void set_commutation_for_hall(uint8_t hall_value, bool estop); @@ -254,79 +255,93 @@ static void pwm6step_setup_hall_timer() { /////////////////////// // htim2.Init.Prescaler = LF_TIMX_PSC; // 11 - TIM2->PSC = 11; //div by 11 - 1 = 10, 4.8MHz. Period 208ns + TIM2->PSC = 0; //div by 11 - 1 = 10, 4.8MHz. Period 208ns + TIM2->CNT = 0; // htim2.Init.CounterMode = TIM_COUNTERMODE_UP; - TIM2->CR1 &= ~(TIM_CR1_DIR); + // TIM2->CR1 &= ~(TIM_CR1_DIR); // htim2.Init.Period = LF_TIMX_ARR; // 24000 - TIM2->ARR = 24000; // probs wrong, but unused rn + // TIM2->ARR = 24000; // probs wrong, but unused rn // htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - TIM2->CR1 &= ~(TIM_CR1_CKD_0 | TIM_CR1_CKD_1); + // TIM2->CR1 &= ~(TIM_CR1_CKD_0 | TIM_CR1_CKD_1); // htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - TIM2->CR1 &= ~(TIM_CR1_ARPE); + // TIM2->CR1 &= ~(TIM_CR1_ARPE); + TIM2->CR1 = 0; // enable hall sense interface be selecting the XOR function of input 1-3 TIM2->CR2 |= TIM_CR2_TI1S; // sSlaveConfig.SlaveMode = TIM_SLAVEMODE_RESET; + // sSlaveConfig.InputTrigger = TIM_TS_TI1F_ED; // clear SMS (required to set TS) TIM2->SMCR &= ~(TIM_SMCR_SMS_Msk); - //sSlaveConfig.InputTrigger = TIM_TS_TI1F_ED; TIM2->SMCR &= ~(TIM_SMCR_TS_Msk); - TIM2->SMCR |= (0x4 << TIM_SMCR_TS_Pos); // b100 TI1F_ED mode (Timer Input 1 Edge Detect) + TIM2->SMCR |= TIM_SMCR_TS_2; // b100 TI1F_ED mode (Timer Input 1 Edge Detect) // now that TS is set, set SMS - TIM2->SMCR |= (0x4 << TIM_SMCR_SMS_Pos); // b100 reset mode + TIM2->SMCR |= TIM_SMCR_SMS_2; // b100 reset mode + + TIM2->CCMR1 &= ~(TIM_CCMR1_IC1F_Msk); + TIM2->CCMR1 = TIM_CCMR1_CC1S | (7 << TIM_CCMR1_IC1F_Pos); + TIM2->CCMR2 = 0; + //sSlaveConfig.TriggerFilter = 8; TIM2->CCER &= ~(TIM_CCER_CC1E); - TIM2->CCMR1 &= ~(TIM_CCMR1_IC1F_Msk); - TIM2->CCMR1 |= (0xF << TIM_CCMR1_IC1F_Pos); + TIM2->CCER |= TIM_CCER_CC1E; + + TIM2->ARR = UINT32_MAX; + TIM2->EGR = TIM_EGR_UG; // set the master mode output trigger to OC2REF // TIM2 is a master to TIM1, so an event will trigger COM in TIM1 // the source of outbound trigger will be Output Compare Channel 2 REF (count down to 0) // this allows us to delay COM until the hall edge detection interrupt finishes - TIM2->CR2 &= ~TIM_CR2_MMS; - TIM2->CR2 |= (TIM_CR2_MMS_0 | TIM_CR2_MMS_2); // 0b101 OC2REF for TRGO + // TIM2->CR2 &= ~TIM_CR2_MMS; + // TIM2->CR2 |= (TIM_CR2_MMS_0 | TIM_CR2_MMS_2); // 0b101 OC2REF for TRGO // enable master slave mode - TIM2->SMCR &= ~TIM_SMCR_MSM; - TIM2->SMCR |= (TIM_SMCR_MSM); + // TIM2->SMCR &= ~TIM_SMCR_MSM; + // TIM2->SMCR |= (TIM_SMCR_MSM); /////////////////////////////////////// // TIM2 CH1 setup as delay capture // /////////////////////////////////////// - /* Disable the Channel 1: Reset the CC1E Bit */ - TIM2->CCER &= ~TIM_CCER_CC1E; + // /* Disable the Channel 1: Reset the CC1E Bit */ + // TIM2->CCER &= ~TIM_CCER_CC1E; - // only writeable when channel is off - // CC1S = 2'b11 -> sets input capture on TRC (internal trigger set by TS bits) - // this is done in general config above, TRC source is edge detect - TIM2->CCMR1 &= ~TIM_CCMR1_CC1S; - TIM2->CCMR1 |= (0x3 << TIM_CCMR1_CC1S_Pos); + // // only writeable when channel is off + // // CC1S = 2'b11 -> sets input capture on TRC (internal trigger set by TS bits) + // // this is done in general config above, TRC source is edge detect + // TIM2->CCMR1 &= ~TIM_CCMR1_CC1S; + // TIM2->CCMR1 |= (0x3 << TIM_CCMR1_CC1S_Pos); - // disable filter, not used when edge detect TRC is selected - TIM2->CCMR1 &= ~TIM_CCMR1_IC1F; - TIM2->CCMR1 |= ((0x0 << 4U) & TIM_CCMR1_IC1F); + // // disable filter, not used when edge detect TRC is selected + // TIM2->CCMR1 &= ~TIM_CCMR1_IC1F; + // TIM2->CCMR1 |= ((0x0 << 4U) & TIM_CCMR1_IC1F); - // set edge detection mode to non-inverted/rising edge. - // it is forbidden to use dual edge detect for hall sensing mode - TIM2->CCER &= ~(TIM_CCER_CC1P | TIM_CCER_CC1NP); - TIM2->CCER |= (0x0U & (TIM_CCER_CC1P | TIM_CCER_CC1NP)); + // // set edge detection mode to non-inverted/rising edge. + // // it is forbidden to use dual edge detect for hall sensing mode + // TIM2->CCER &= ~(TIM_CCER_CC1P | TIM_CCER_CC1NP); + // TIM2->CCER |= (0x0U & (TIM_CCER_CC1P | TIM_CCER_CC1NP)); - // set the channel prescaler to 0 - TIM2->CCMR1 &= ~TIM_CCMR1_IC1PSC; - TIM2->CCMR1 |= (0 << TIM_CCMR1_IC1PSC_Pos); + // // set the channel prescaler to 0 + // TIM2->CCMR1 &= ~TIM_CCMR1_IC1PSC; + // TIM2->CCMR1 |= (0 << TIM_CCMR1_IC1PSC_Pos); - // enable the interrupt - TIM2->DIER |= (TIM_DIER_CC1IE); - TIM2->SR &= ~(TIM_SR_CC1IF); + // // enable the interrupt + // TIM2->DIER |= (TIM_DIER_CC1IE, TIM_DIER_UIE); + // TIM2->SR &= ~(TIM_SR_CC1IF); - // enable channel 1 - TIM2->CCER |= TIM_CCER_CC1E; + // // enable channel 1 + // TIM2->CCER |= TIM_CCER_CC1E; // Enable in NVIC NVIC_SetPriority(TIM2_IRQn, 5); NVIC_EnableIRQ(TIM2_IRQn); + TIM2->CR1 |= TIM_CR1_CEN; + + TIM2->DIER = TIM_DIER_UIE; + TIM2->EGR = TIM_EGR_UG; + //////////////////////////////////////// // setup timer for delay triggering // //////////////////////////////////////// @@ -336,7 +351,7 @@ static void pwm6step_setup_hall_timer() { // create a congiruable delay TIM16->SR = 0; - // prescaler of 48 -> 1MHz input cleck -> each cycle is 1us + // prescaler of 48 -> 1MHz input clock -> each cycle is 1us TIM16->PSC = 49; TIM16->EGR |= (TIM_EGR_UG); // this might should be a function of velocity (estimated or measured) @@ -433,12 +448,15 @@ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) { TIM1->PSC = PWM_TIM_PRESCALER; // set PWM period relative to the scaled sysclk - uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) pwm_freq_hz * (PWM_TIM_PRESCALER + 1)) - 1); + const uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) pwm_freq_hz * (PWM_TIM_PRESCALER + 1)) - 1); TIM1->ARR = arr_pwm_dc_value; TIM1->CR1 = (TIM_CR1_ARPE | TIM_CR1_CMS); TIM1->CR2 = (TIM_CR2_CCPC); + // TS_0 = ITR1 = TIM2 + TIM1->SMCR = TIM_SMCR_OCCS | TIM_SMCR_ETF | TIM_SMCR_TS_0; + // setup default PWM states TIM1->CCMR1 = CCMR1_PHASE1_OFF | CCMR1_PHASE2_OFF; TIM1->CCMR2 = CCMR2_PHASE3_OFF; @@ -451,8 +469,11 @@ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) { // generate an update event to reload the PSC TIM1->EGR |= (TIM_EGR_UG | TIM_EGR_COMG); + TIM1->BDTR |= TIM_BDTR_MOE | TIM_BDTR_OSSI | TIM_BDTR_OSSR | (DEAD_TIME << TIM_BDTR_DTG_Pos) | TIM_BDTR_BKE | TIM_BDTR_BKP; TIM1->CR1 |= TIM_CR1_CEN; - TIM1->BDTR |= TIM_BDTR_MOE; + + trigger_commutation(); + // enable interrupt to set GPIOB pin 9 on CH2 PWM low -> high transition // used for ADC timing/alignment verification @@ -462,6 +483,7 @@ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) { // NVIC_EnableIRQ(TIM1_CC_IRQn); } +#ifdef FORCE_COMUTATION_TRANSITION_DELAY void TIM2_IRQHandler() { // if Capture Compare 1 (hall updated) if (TIM2->SR & TIM_SR_CC1IF) { @@ -481,6 +503,8 @@ void TIM2_IRQHandler() { // handle errors } + + void TIM16_IRQHandler() { if (TIM16->SR & TIM_SR_CC1IF) { // disable timer @@ -495,6 +519,29 @@ void TIM16_IRQHandler() { TIM16->SR &= ~(TIM_SR_CC1IF); } } +#else // no forced commutation delay +void TIM2_IRQHandler() { + // if Capture Compare 1 (hall updated) + if (TIM2->SR & TIM_SR_CC1IF) { + // read of CCR1 should clear the int enable (PENDING?) flag + // rm0091, SR, pg 442/1004 + uint32_t hall_transition_elapsed_ticks = TIM2->CCR1; + + // clear interrupt + TIM2->SR &= ~(TIM_SR_CC1IF); + } + + if (TIM2->SR & TIM_SR_UIF) { + turn_on_red_led(); + + TIM2_IRQHandler_HallTransition(); + + TIM2->SR &= ~(TIM_SR_UIF); + } + + // handle errors +} +#endif /** * @brief @@ -523,16 +570,14 @@ static void TIM2_IRQHandler_HallTransition() { had_multiple_transitions = true; } - - - perform_commutation_cycle(); + perform_commutation_cycle(false); } /** * @brief * */ -static void perform_commutation_cycle() { +static void perform_commutation_cycle(bool comg_manual_trig) { // mask off Hall lines PA0-PA2 (already the LSBs) // Input Data Register is before all AF muxing so this should be valid always //hall_recorded_state_on_transition = (GPIOA->IDR & (GPIO_IDR_2 | GPIO_IDR_1 | GPIO_IDR_0)); @@ -619,7 +664,7 @@ static void perform_commutation_cycle() { } set_commutation_for_hall(hall_recorded_state_on_transition, false); - trigger_commutation(); + // trigger_commutation(); } /** @@ -678,73 +723,73 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { bool phase3_high = commutation_values[5]; uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) PWM_FREQ_HZ * (PWM_TIM_PRESCALER + 1)) - 1); - TIM1->CCR4 = arr_pwm_dc_value; + // TIM1->CCR4 = arr_pwm_dc_value; // uint16_t ccer = 0; uint16_t ccer = CCER_TIM4_ADC_TRIG; uint16_t ccmr1 = 0; // uint16_t ccmr2 = 0; uint16_t ccmr2 = CCMR2_TIM4_ADC_TRIG; - TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; + // TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; - TIM1->CCR4 = 22; + TIM1->CCR4 = 4; if (phase1_low) { if (command_brake) { - TIM1->CCR1 = current_duty_cycle; + // TIM1->CCR1 = current_duty_cycle; ccmr1 |= CCMR1_PHASE1_PWM_BRAKE; ccer |= CCER_PHASE1_PWM_BRAKE; } else { - TIM1->CCR1 = 0; + // TIM1->CCR1 = 0; ccmr1 |= CCMR1_PHASE1_LOW; ccer |= CCER_PHASE1_LOW; } } else if (phase1_high) { - TIM1->CCR1 = current_duty_cycle; + // TIM1->CCR1 = current_duty_cycle; ccmr1 |= CCMR1_PHASE1_PWM; ccer |= CCER_PHASE1_PWM; } else { - TIM1->CCR1 = 0; + // TIM1->CCR1 = 0; ccmr1 |= CCMR1_PHASE1_OFF; ccer |= CCER_PHASE1_OFF; } if (phase2_low) { if (command_brake) { - TIM1->CCR2 = current_duty_cycle; + // TIM1->CCR2 = current_duty_cycle; ccmr1 |= CCMR1_PHASE2_PWM_BRAKE; ccer |= CCER_PHASE2_PWM_BRAKE; } else { - TIM1->CCR2 = 0; + // TIM1->CCR2 = 0; ccmr1 |= CCMR1_PHASE2_LOW; ccer |= CCER_PHASE2_LOW; } } else if (phase2_high) { - TIM1->CCR2 = current_duty_cycle; + // TIM1->CCR2 = current_duty_cycle; ccmr1 |= CCMR1_PHASE2_PWM; ccer |= CCER_PHASE2_PWM; } else { - TIM1->CCR2 = 0; + // TIM1->CCR2 = 0; ccmr1 |= CCMR1_PHASE2_OFF; ccer |= CCER_PHASE2_OFF; } if (phase3_low) { if (command_brake) { - TIM1->CCR3 = current_duty_cycle; + // TIM1->CCR3 = current_duty_cycle; ccmr2 |= CCMR2_PHASE3_PWM_BRAKE; ccer |= CCER_PHASE3_PWM_BRAKE; } else { - TIM1->CCR3 = 0; + // TIM1->CCR3 = 0; ccmr2 |= CCMR2_PHASE3_LOW; ccer |= CCER_PHASE3_LOW; } } else if (phase3_high) { - TIM1->CCR3 = current_duty_cycle; + // TIM1->CCR3 = current_duty_cycle; ccmr2 |= CCMR2_PHASE3_PWM; ccer |= CCER_PHASE3_PWM; } else { - TIM1->CCR3 = 0; + // TIM1->CCR3 = 0; ccmr2 |= CCMR2_PHASE3_OFF; ccer |= CCER_PHASE3_OFF; } @@ -771,6 +816,8 @@ void TIM1_CC_IRQHandler() { TIM1->SR &= ~(TIM_SR_CC4IF); } +static bool init_comm = false; + /** * @brief * @@ -790,7 +837,17 @@ static void pwm6step_set_direct(uint16_t duty_cycle, MotorDirection_t motor_dire commanded_motor_direction = motor_direction; current_duty_cycle = scaled_dc_inv; - perform_commutation_cycle(); + + TIM1->CCR1 = current_duty_cycle; + TIM1->CCR2 = current_duty_cycle; + TIM1->CCR3 = current_duty_cycle; + + TIM2->EGR = TIM_EGR_UG; + // perform_commutation_cycle(false); + if (!init_comm) { + trigger_commutation(); + init_comm = false; + } } //////////////////////// diff --git a/motor-controller/common/6step.h b/motor-controller/common/6step.h index e32acce9..d305373d 100644 --- a/motor-controller/common/6step.h +++ b/motor-controller/common/6step.h @@ -17,10 +17,10 @@ #define MAX_DUTYCYCLE_COMMAND 65535U #define MIN_DUTYCYCLE_COMMAND -(MAX_DUTYCYCLE_COMMAND) -#define NUM_RAW_DC_STEPS 1000U -#define MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW 10U -#define SCALING_FACTOR ((MAX_DUTYCYCLE_COMMAND / (NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW)) + 1U) -#define MAP_UINT16_TO_RAW_DC(dc) (dc > 0 ? ((dc / SCALING_FACTOR) + MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW) : 0U) +// period 20833ns +// be conscious of dead time ratio +#define PWM_TIM_PRESCALER 0 +#define PWM_FREQ_HZ 40000 ////////////////////// // ERROR HANDLING // @@ -49,12 +49,6 @@ typedef struct MotorErrors { // LOW LEVEL CONTROL PARAMS // //////////////////////////////// -#define PWM_TIM_PRESCALER 0 - -// period 20833ns -// be conscious of dead time ratio -#define PWM_FREQ_HZ 48000 - // no div on CK_INT = 48MHz // Tdts = CK_INT = 48MHz // xxx yyyyy -> 0xx selects no multiplier @@ -64,6 +58,13 @@ typedef struct MotorErrors { // 0000 0111 = 0x07 #define DEAD_TIME 0x07 +#define NUM_RAW_DC_STEPS (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) PWM_FREQ_HZ * (PWM_TIM_PRESCALER + 1))) +#define MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW 10U +#define SCALING_FACTOR ((MAX_DUTYCYCLE_COMMAND / (NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW)) + 1U) +#define MAP_UINT16_TO_RAW_DC(dc) (dc > 0 ? ((dc / SCALING_FACTOR) + MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW) : 0U) + +// #define FORCE_COMUTATION_TRANSITION_DELAY + //////////////////////// // PUBLIC FUNCTIONS // //////////////////////// From f4248fcae16fb62c14fb79a4c4b9376bcd455b62 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Sun, 12 Oct 2025 22:57:43 -0400 Subject: [PATCH 13/18] initial sample calib attempt --- control-board/src/bin/hwtest-motor/main.rs | 6 +++--- motor-controller/common/6step.c | 4 +--- motor-controller/common/current_sensing.c | 4 ++-- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index c4085c5e..1118a906 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -129,10 +129,10 @@ async fn main(main_spawner: embassy_executor::Spawner) { Timer::after_millis(100).await; ctr += 1; - if ctr > 100 { + if ctr > 50 { ctr = 0; - } else if ctr > 50 { - vel = 1.0; + } else if ctr > 25 { + vel = 2.0; } else { vel = 0.0; } diff --git a/motor-controller/common/6step.c b/motor-controller/common/6step.c index f64c86ee..8d605a3c 100644 --- a/motor-controller/common/6step.c +++ b/motor-controller/common/6step.c @@ -532,8 +532,6 @@ void TIM2_IRQHandler() { } if (TIM2->SR & TIM_SR_UIF) { - turn_on_red_led(); - TIM2_IRQHandler_HallTransition(); TIM2->SR &= ~(TIM_SR_UIF); @@ -732,7 +730,7 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { uint16_t ccmr2 = CCMR2_TIM4_ADC_TRIG; // TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; - TIM1->CCR4 = 4; + TIM1->CCR4 = 7; if (phase1_low) { if (command_brake) { diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c index 3aa7c4de..b952be64 100644 --- a/motor-controller/common/current_sensing.c +++ b/motor-controller/common/current_sensing.c @@ -187,8 +187,8 @@ CS_Status_t currsen_setup(uint8_t motor_adc_ch) // 7.5 ADC clock cycles (0.625 us) is the minimum sampling time for the motor. // T_settling > ((Imax * Rs * G_real) / SR) = ((10.0 * 0.05 * 4.94) / 10V/us) = 0.247us - ADC1->SMPR = ADC_SMPR_SMP_0; - // ADC1->SMPR = 0; + // ADC1->SMPR = ADC_SMPR_SMP_0; + ADC1->SMPR = 0; // Set ADC channel selection. Ch9 is the Vbus. From d11052e7df11108f6c716e9dcc45d45f4e32856c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 14 Oct 2025 20:53:59 -0400 Subject: [PATCH 14/18] cleanup 6step file --- control-board/src/bin/hwtest-motor/main.rs | 2 +- motor-controller/common/6step.c | 104 +-------------------- motor-controller/common/6step.h | 2 +- 3 files changed, 6 insertions(+), 102 deletions(-) diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 1118a906..4c63bbaa 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -132,7 +132,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { if ctr > 50 { ctr = 0; } else if ctr > 25 { - vel = 2.0; + vel = 3.0; } else { vel = 0.0; } diff --git a/motor-controller/common/6step.c b/motor-controller/common/6step.c index 8d605a3c..b9c9b314 100644 --- a/motor-controller/common/6step.c +++ b/motor-controller/common/6step.c @@ -254,17 +254,8 @@ static void pwm6step_setup_hall_timer() { // TIM2 core setup // /////////////////////// - // htim2.Init.Prescaler = LF_TIMX_PSC; // 11 - TIM2->PSC = 0; //div by 11 - 1 = 10, 4.8MHz. Period 208ns + TIM2->PSC = 0; TIM2->CNT = 0; - // htim2.Init.CounterMode = TIM_COUNTERMODE_UP; - // TIM2->CR1 &= ~(TIM_CR1_DIR); - // htim2.Init.Period = LF_TIMX_ARR; // 24000 - // TIM2->ARR = 24000; // probs wrong, but unused rn - // htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - // TIM2->CR1 &= ~(TIM_CR1_CKD_0 | TIM_CR1_CKD_1); - // htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - // TIM2->CR1 &= ~(TIM_CR1_ARPE); TIM2->CR1 = 0; // enable hall sense interface be selecting the XOR function of input 1-3 @@ -289,51 +280,7 @@ static void pwm6step_setup_hall_timer() { TIM2->ARR = UINT32_MAX; TIM2->EGR = TIM_EGR_UG; - // set the master mode output trigger to OC2REF - // TIM2 is a master to TIM1, so an event will trigger COM in TIM1 - // the source of outbound trigger will be Output Compare Channel 2 REF (count down to 0) - // this allows us to delay COM until the hall edge detection interrupt finishes - // TIM2->CR2 &= ~TIM_CR2_MMS; - // TIM2->CR2 |= (TIM_CR2_MMS_0 | TIM_CR2_MMS_2); // 0b101 OC2REF for TRGO - // enable master slave mode - // TIM2->SMCR &= ~TIM_SMCR_MSM; - // TIM2->SMCR |= (TIM_SMCR_MSM); - - /////////////////////////////////////// - // TIM2 CH1 setup as delay capture // - /////////////////////////////////////// - - // /* Disable the Channel 1: Reset the CC1E Bit */ - // TIM2->CCER &= ~TIM_CCER_CC1E; - - // // only writeable when channel is off - // // CC1S = 2'b11 -> sets input capture on TRC (internal trigger set by TS bits) - // // this is done in general config above, TRC source is edge detect - // TIM2->CCMR1 &= ~TIM_CCMR1_CC1S; - // TIM2->CCMR1 |= (0x3 << TIM_CCMR1_CC1S_Pos); - - // // disable filter, not used when edge detect TRC is selected - // TIM2->CCMR1 &= ~TIM_CCMR1_IC1F; - // TIM2->CCMR1 |= ((0x0 << 4U) & TIM_CCMR1_IC1F); - - // // set edge detection mode to non-inverted/rising edge. - // // it is forbidden to use dual edge detect for hall sensing mode - // TIM2->CCER &= ~(TIM_CCER_CC1P | TIM_CCER_CC1NP); - // TIM2->CCER |= (0x0U & (TIM_CCER_CC1P | TIM_CCER_CC1NP)); - - // // set the channel prescaler to 0 - // TIM2->CCMR1 &= ~TIM_CCMR1_IC1PSC; - // TIM2->CCMR1 |= (0 << TIM_CCMR1_IC1PSC_Pos); - - // // enable the interrupt - // TIM2->DIER |= (TIM_DIER_CC1IE, TIM_DIER_UIE); - // TIM2->SR &= ~(TIM_SR_CC1IF); - - // // enable channel 1 - // TIM2->CCER |= TIM_CCER_CC1E; - // Enable in NVIC - NVIC_SetPriority(TIM2_IRQn, 5); NVIC_EnableIRQ(TIM2_IRQn); @@ -341,26 +288,6 @@ static void pwm6step_setup_hall_timer() { TIM2->DIER = TIM_DIER_UIE; TIM2->EGR = TIM_EGR_UG; - - //////////////////////////////////////// - // setup timer for delay triggering // - //////////////////////////////////////// - - // there seems to be chatter when TIM1 COM is triggered - // immediated on a hall transition, so we use TIM16 to - // create a congiruable delay - - TIM16->SR = 0; - // prescaler of 48 -> 1MHz input clock -> each cycle is 1us - TIM16->PSC = 49; - TIM16->EGR |= (TIM_EGR_UG); - // this might should be a function of velocity (estimated or measured) - TIM16->CCR1 = 61; // commutation delay of 20us - - // enable timer delay for commutation - TIM16->DIER |= TIM_DIER_CC1IE; - NVIC_SetPriority(TIM16_IRQn, 5); - NVIC_EnableIRQ(TIM16_IRQn); } #define CCMR1_PHASE1_OFF (TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_2) @@ -472,15 +399,10 @@ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) { TIM1->BDTR |= TIM_BDTR_MOE | TIM_BDTR_OSSI | TIM_BDTR_OSSR | (DEAD_TIME << TIM_BDTR_DTG_Pos) | TIM_BDTR_BKE | TIM_BDTR_BKP; TIM1->CR1 |= TIM_CR1_CEN; - trigger_commutation(); - + // ADC sample delay + TIM1->CCR4 = 7; - // enable interrupt to set GPIOB pin 9 on CH2 PWM low -> high transition - // used for ADC timing/alignment verification - // TIM1->SR = 0; - // TIM1->DIER |= TIM_DIER_CC4IE; - // NVIC_SetPriority(TIM1_CC_IRQn, 5); - // NVIC_EnableIRQ(TIM1_CC_IRQn); + trigger_commutation(); } #ifdef FORCE_COMUTATION_TRANSITION_DELAY @@ -720,54 +642,38 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { bool phase3_low = commutation_values[4]; bool phase3_high = commutation_values[5]; - uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) PWM_FREQ_HZ * (PWM_TIM_PRESCALER + 1)) - 1); - // TIM1->CCR4 = arr_pwm_dc_value; - - // uint16_t ccer = 0; uint16_t ccer = CCER_TIM4_ADC_TRIG; uint16_t ccmr1 = 0; - // uint16_t ccmr2 = 0; uint16_t ccmr2 = CCMR2_TIM4_ADC_TRIG; - // TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; - - TIM1->CCR4 = 7; if (phase1_low) { if (command_brake) { - // TIM1->CCR1 = current_duty_cycle; ccmr1 |= CCMR1_PHASE1_PWM_BRAKE; ccer |= CCER_PHASE1_PWM_BRAKE; } else { - // TIM1->CCR1 = 0; ccmr1 |= CCMR1_PHASE1_LOW; ccer |= CCER_PHASE1_LOW; } } else if (phase1_high) { - // TIM1->CCR1 = current_duty_cycle; ccmr1 |= CCMR1_PHASE1_PWM; ccer |= CCER_PHASE1_PWM; } else { - // TIM1->CCR1 = 0; ccmr1 |= CCMR1_PHASE1_OFF; ccer |= CCER_PHASE1_OFF; } if (phase2_low) { if (command_brake) { - // TIM1->CCR2 = current_duty_cycle; ccmr1 |= CCMR1_PHASE2_PWM_BRAKE; ccer |= CCER_PHASE2_PWM_BRAKE; } else { - // TIM1->CCR2 = 0; ccmr1 |= CCMR1_PHASE2_LOW; ccer |= CCER_PHASE2_LOW; } } else if (phase2_high) { - // TIM1->CCR2 = current_duty_cycle; ccmr1 |= CCMR1_PHASE2_PWM; ccer |= CCER_PHASE2_PWM; } else { - // TIM1->CCR2 = 0; ccmr1 |= CCMR1_PHASE2_OFF; ccer |= CCER_PHASE2_OFF; } @@ -857,8 +763,6 @@ static void pwm6step_set_direct(uint16_t duty_cycle, MotorDirection_t motor_dire * */ void pwm6step_setup() { - - pwm6step_setup_hall_timer(); pwm6step_setup_commutation_timer(PWM_FREQ_HZ); } diff --git a/motor-controller/common/6step.h b/motor-controller/common/6step.h index d305373d..c0752d55 100644 --- a/motor-controller/common/6step.h +++ b/motor-controller/common/6step.h @@ -20,7 +20,7 @@ // period 20833ns // be conscious of dead time ratio #define PWM_TIM_PRESCALER 0 -#define PWM_FREQ_HZ 40000 +#define PWM_FREQ_HZ 48000 ////////////////////// // ERROR HANDLING // From 9ef63a68b525b4c76dfa2ffcbc4baa33f0ae1053 Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 14 Oct 2025 21:31:25 -0400 Subject: [PATCH 15/18] revet commutation method --- motor-controller/common/6step.c | 189 +++++++++++++++++++------------- motor-controller/common/6step.h | 2 +- 2 files changed, 116 insertions(+), 75 deletions(-) diff --git a/motor-controller/common/6step.c b/motor-controller/common/6step.c index b9c9b314..48195dec 100644 --- a/motor-controller/common/6step.c +++ b/motor-controller/common/6step.c @@ -29,7 +29,6 @@ #include #include "6step.h" -#include "debug.h" #include "system.h" #include "time.h" @@ -220,7 +219,7 @@ static uint8_t ccw_expected_hall_transition_table[8] = { static void pwm6step_setup_hall_timer(); static void pwm6step_setup_commutation_timer(uint16_t duty_cycle); static void TIM2_IRQHandler_HallTransition(); -static void perform_commutation_cycle(bool); +static void perform_commutation_cycle(); static uint8_t read_hall(); static void set_commutation_estop(); static void set_commutation_for_hall(uint8_t hall_value, bool estop); @@ -254,40 +253,99 @@ static void pwm6step_setup_hall_timer() { // TIM2 core setup // /////////////////////// - TIM2->PSC = 0; - TIM2->CNT = 0; + // htim2.Init.Prescaler = LF_TIMX_PSC; // 11 + TIM2->PSC = 11; //div by 11 - 1 = 10, 4.8MHz. Period 208ns + // htim2.Init.CounterMode = TIM_COUNTERMODE_UP; + TIM2->CR1 &= ~(TIM_CR1_DIR); + // htim2.Init.Period = LF_TIMX_ARR; // 24000 + TIM2->ARR = 24000; // probs wrong, but unused rn + // htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + TIM2->CR1 &= ~(TIM_CR1_CKD_0 | TIM_CR1_CKD_1); + // htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + TIM2->CR1 &= ~(TIM_CR1_ARPE); - TIM2->CR1 = 0; // enable hall sense interface be selecting the XOR function of input 1-3 TIM2->CR2 |= TIM_CR2_TI1S; // sSlaveConfig.SlaveMode = TIM_SLAVEMODE_RESET; - // sSlaveConfig.InputTrigger = TIM_TS_TI1F_ED; // clear SMS (required to set TS) TIM2->SMCR &= ~(TIM_SMCR_SMS_Msk); + //sSlaveConfig.InputTrigger = TIM_TS_TI1F_ED; TIM2->SMCR &= ~(TIM_SMCR_TS_Msk); - TIM2->SMCR |= TIM_SMCR_TS_2; // b100 TI1F_ED mode (Timer Input 1 Edge Detect) + TIM2->SMCR |= (0x4 << TIM_SMCR_TS_Pos); // b100 TI1F_ED mode (Timer Input 1 Edge Detect) // now that TS is set, set SMS - TIM2->SMCR |= TIM_SMCR_SMS_2; // b100 reset mode - - TIM2->CCMR1 &= ~(TIM_CCMR1_IC1F_Msk); - TIM2->CCMR1 = TIM_CCMR1_CC1S | (7 << TIM_CCMR1_IC1F_Pos); - TIM2->CCMR2 = 0; - + TIM2->SMCR |= (0x4 << TIM_SMCR_SMS_Pos); // b100 reset mode //sSlaveConfig.TriggerFilter = 8; TIM2->CCER &= ~(TIM_CCER_CC1E); + TIM2->CCMR1 &= ~(TIM_CCMR1_IC1F_Msk); + TIM2->CCMR1 |= (0xF << TIM_CCMR1_IC1F_Pos); + + // set the master mode output trigger to OC2REF + // TIM2 is a master to TIM1, so an event will trigger COM in TIM1 + // the source of outbound trigger will be Output Compare Channel 2 REF (count down to 0) + // this allows us to delay COM until the hall edge detection interrupt finishes + TIM2->CR2 &= ~TIM_CR2_MMS; + TIM2->CR2 |= (TIM_CR2_MMS_0 | TIM_CR2_MMS_2); // 0b101 OC2REF for TRGO + // enable master slave mode + TIM2->SMCR &= ~TIM_SMCR_MSM; + TIM2->SMCR |= (TIM_SMCR_MSM); + + /////////////////////////////////////// + // TIM2 CH1 setup as delay capture // + /////////////////////////////////////// + + /* Disable the Channel 1: Reset the CC1E Bit */ + TIM2->CCER &= ~TIM_CCER_CC1E; + + // only writeable when channel is off + // CC1S = 2'b11 -> sets input capture on TRC (internal trigger set by TS bits) + // this is done in general config above, TRC source is edge detect + TIM2->CCMR1 &= ~TIM_CCMR1_CC1S; + TIM2->CCMR1 |= (0x3 << TIM_CCMR1_CC1S_Pos); + + // disable filter, not used when edge detect TRC is selected + TIM2->CCMR1 &= ~TIM_CCMR1_IC1F; + TIM2->CCMR1 |= ((0x0 << 4U) & TIM_CCMR1_IC1F); + + // set edge detection mode to non-inverted/rising edge. + // it is forbidden to use dual edge detect for hall sensing mode + TIM2->CCER &= ~(TIM_CCER_CC1P | TIM_CCER_CC1NP); + TIM2->CCER |= (0x0U & (TIM_CCER_CC1P | TIM_CCER_CC1NP)); + + // set the channel prescaler to 0 + TIM2->CCMR1 &= ~TIM_CCMR1_IC1PSC; + TIM2->CCMR1 |= (0 << TIM_CCMR1_IC1PSC_Pos); + + // enable the interrupt + TIM2->DIER |= (TIM_DIER_CC1IE); + TIM2->SR &= ~(TIM_SR_CC1IF); + + // enable channel 1 TIM2->CCER |= TIM_CCER_CC1E; - TIM2->ARR = UINT32_MAX; - TIM2->EGR = TIM_EGR_UG; - // Enable in NVIC + NVIC_SetPriority(TIM2_IRQn, 5); NVIC_EnableIRQ(TIM2_IRQn); - TIM2->CR1 |= TIM_CR1_CEN; - - TIM2->DIER = TIM_DIER_UIE; - TIM2->EGR = TIM_EGR_UG; + //////////////////////////////////////// + // setup timer for delay triggering // + //////////////////////////////////////// + + // there seems to be chatter when TIM1 COM is triggered + // immediated on a hall transition, so we use TIM16 to + // create a congiruable delay + + TIM16->SR = 0; + // prescaler of 48 -> 1MHz input cleck -> each cycle is 1us + TIM16->PSC = 49; + TIM16->EGR |= (TIM_EGR_UG); + // this might should be a function of velocity (estimated or measured) + TIM16->CCR1 = 61; // commutation delay of 20us + + // enable timer delay for commutation + TIM16->DIER |= TIM_DIER_CC1IE; + NVIC_SetPriority(TIM16_IRQn, 5); + NVIC_EnableIRQ(TIM16_IRQn); } #define CCMR1_PHASE1_OFF (TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_2) @@ -375,15 +433,12 @@ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) { TIM1->PSC = PWM_TIM_PRESCALER; // set PWM period relative to the scaled sysclk - const uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) pwm_freq_hz * (PWM_TIM_PRESCALER + 1)) - 1); + uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) pwm_freq_hz * (PWM_TIM_PRESCALER + 1)) - 1); TIM1->ARR = arr_pwm_dc_value; TIM1->CR1 = (TIM_CR1_ARPE | TIM_CR1_CMS); TIM1->CR2 = (TIM_CR2_CCPC); - // TS_0 = ITR1 = TIM2 - TIM1->SMCR = TIM_SMCR_OCCS | TIM_SMCR_ETF | TIM_SMCR_TS_0; - // setup default PWM states TIM1->CCMR1 = CCMR1_PHASE1_OFF | CCMR1_PHASE2_OFF; TIM1->CCMR2 = CCMR2_PHASE3_OFF; @@ -396,16 +451,17 @@ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) { // generate an update event to reload the PSC TIM1->EGR |= (TIM_EGR_UG | TIM_EGR_COMG); - TIM1->BDTR |= TIM_BDTR_MOE | TIM_BDTR_OSSI | TIM_BDTR_OSSR | (DEAD_TIME << TIM_BDTR_DTG_Pos) | TIM_BDTR_BKE | TIM_BDTR_BKP; TIM1->CR1 |= TIM_CR1_CEN; - - // ADC sample delay - TIM1->CCR4 = 7; - - trigger_commutation(); + TIM1->BDTR |= TIM_BDTR_MOE; + + // enable interrupt to set GPIOB pin 9 on CH2 PWM low -> high transition + // used for ADC timing/alignment verification + // TIM1->SR = 0; + // TIM1->DIER |= TIM_DIER_CC4IE; + // NVIC_SetPriority(TIM1_CC_IRQn, 5); + // NVIC_EnableIRQ(TIM1_CC_IRQn); } -#ifdef FORCE_COMUTATION_TRANSITION_DELAY void TIM2_IRQHandler() { // if Capture Compare 1 (hall updated) if (TIM2->SR & TIM_SR_CC1IF) { @@ -425,8 +481,6 @@ void TIM2_IRQHandler() { // handle errors } - - void TIM16_IRQHandler() { if (TIM16->SR & TIM_SR_CC1IF) { // disable timer @@ -441,27 +495,6 @@ void TIM16_IRQHandler() { TIM16->SR &= ~(TIM_SR_CC1IF); } } -#else // no forced commutation delay -void TIM2_IRQHandler() { - // if Capture Compare 1 (hall updated) - if (TIM2->SR & TIM_SR_CC1IF) { - // read of CCR1 should clear the int enable (PENDING?) flag - // rm0091, SR, pg 442/1004 - uint32_t hall_transition_elapsed_ticks = TIM2->CCR1; - - // clear interrupt - TIM2->SR &= ~(TIM_SR_CC1IF); - } - - if (TIM2->SR & TIM_SR_UIF) { - TIM2_IRQHandler_HallTransition(); - - TIM2->SR &= ~(TIM_SR_UIF); - } - - // handle errors -} -#endif /** * @brief @@ -490,14 +523,16 @@ static void TIM2_IRQHandler_HallTransition() { had_multiple_transitions = true; } - perform_commutation_cycle(false); + + + perform_commutation_cycle(); } /** * @brief * */ -static void perform_commutation_cycle(bool comg_manual_trig) { +static void perform_commutation_cycle() { // mask off Hall lines PA0-PA2 (already the LSBs) // Input Data Register is before all AF muxing so this should be valid always //hall_recorded_state_on_transition = (GPIOA->IDR & (GPIO_IDR_2 | GPIO_IDR_1 | GPIO_IDR_0)); @@ -584,7 +619,7 @@ static void perform_commutation_cycle(bool comg_manual_trig) { } set_commutation_for_hall(hall_recorded_state_on_transition, false); - // trigger_commutation(); + trigger_commutation(); } /** @@ -642,58 +677,74 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { bool phase3_low = commutation_values[4]; bool phase3_high = commutation_values[5]; + uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) PWM_FREQ_HZ * (PWM_TIM_PRESCALER + 1)) - 1); + TIM1->CCR4 = arr_pwm_dc_value; + + // uint16_t ccer = 0; uint16_t ccer = CCER_TIM4_ADC_TRIG; uint16_t ccmr1 = 0; + // uint16_t ccmr2 = 0; uint16_t ccmr2 = CCMR2_TIM4_ADC_TRIG; + TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; + + TIM1->CCR4 = 22; if (phase1_low) { if (command_brake) { + TIM1->CCR1 = current_duty_cycle; ccmr1 |= CCMR1_PHASE1_PWM_BRAKE; ccer |= CCER_PHASE1_PWM_BRAKE; } else { + TIM1->CCR1 = 0; ccmr1 |= CCMR1_PHASE1_LOW; ccer |= CCER_PHASE1_LOW; } } else if (phase1_high) { + TIM1->CCR1 = current_duty_cycle; ccmr1 |= CCMR1_PHASE1_PWM; ccer |= CCER_PHASE1_PWM; } else { + TIM1->CCR1 = 0; ccmr1 |= CCMR1_PHASE1_OFF; ccer |= CCER_PHASE1_OFF; } if (phase2_low) { if (command_brake) { + TIM1->CCR2 = current_duty_cycle; ccmr1 |= CCMR1_PHASE2_PWM_BRAKE; ccer |= CCER_PHASE2_PWM_BRAKE; } else { + TIM1->CCR2 = 0; ccmr1 |= CCMR1_PHASE2_LOW; ccer |= CCER_PHASE2_LOW; } } else if (phase2_high) { + TIM1->CCR2 = current_duty_cycle; ccmr1 |= CCMR1_PHASE2_PWM; ccer |= CCER_PHASE2_PWM; } else { + TIM1->CCR2 = 0; ccmr1 |= CCMR1_PHASE2_OFF; ccer |= CCER_PHASE2_OFF; } if (phase3_low) { if (command_brake) { - // TIM1->CCR3 = current_duty_cycle; + TIM1->CCR3 = current_duty_cycle; ccmr2 |= CCMR2_PHASE3_PWM_BRAKE; ccer |= CCER_PHASE3_PWM_BRAKE; } else { - // TIM1->CCR3 = 0; + TIM1->CCR3 = 0; ccmr2 |= CCMR2_PHASE3_LOW; ccer |= CCER_PHASE3_LOW; } } else if (phase3_high) { - // TIM1->CCR3 = current_duty_cycle; + TIM1->CCR3 = current_duty_cycle; ccmr2 |= CCMR2_PHASE3_PWM; ccer |= CCER_PHASE3_PWM; } else { - // TIM1->CCR3 = 0; + TIM1->CCR3 = 0; ccmr2 |= CCMR2_PHASE3_OFF; ccer |= CCER_PHASE3_OFF; } @@ -720,8 +771,6 @@ void TIM1_CC_IRQHandler() { TIM1->SR &= ~(TIM_SR_CC4IF); } -static bool init_comm = false; - /** * @brief * @@ -741,17 +790,7 @@ static void pwm6step_set_direct(uint16_t duty_cycle, MotorDirection_t motor_dire commanded_motor_direction = motor_direction; current_duty_cycle = scaled_dc_inv; - - TIM1->CCR1 = current_duty_cycle; - TIM1->CCR2 = current_duty_cycle; - TIM1->CCR3 = current_duty_cycle; - - TIM2->EGR = TIM_EGR_UG; - // perform_commutation_cycle(false); - if (!init_comm) { - trigger_commutation(); - init_comm = false; - } + perform_commutation_cycle(); } //////////////////////// @@ -763,6 +802,8 @@ static void pwm6step_set_direct(uint16_t duty_cycle, MotorDirection_t motor_dire * */ void pwm6step_setup() { + + pwm6step_setup_hall_timer(); pwm6step_setup_commutation_timer(PWM_FREQ_HZ); } diff --git a/motor-controller/common/6step.h b/motor-controller/common/6step.h index c0752d55..d305373d 100644 --- a/motor-controller/common/6step.h +++ b/motor-controller/common/6step.h @@ -20,7 +20,7 @@ // period 20833ns // be conscious of dead time ratio #define PWM_TIM_PRESCALER 0 -#define PWM_FREQ_HZ 48000 +#define PWM_FREQ_HZ 40000 ////////////////////// // ERROR HANDLING // From f93684717c3d21bd876c2fae428039f1a3bef0ee Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 14 Oct 2025 21:37:47 -0400 Subject: [PATCH 16/18] cleanup 6step adc trigger --- motor-controller/common/6step.c | 6 +++--- motor-controller/common/current_sensing.c | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/motor-controller/common/6step.c b/motor-controller/common/6step.c index 48195dec..1b00f4b2 100644 --- a/motor-controller/common/6step.c +++ b/motor-controller/common/6step.c @@ -677,15 +677,15 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { bool phase3_low = commutation_values[4]; bool phase3_high = commutation_values[5]; - uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) PWM_FREQ_HZ * (PWM_TIM_PRESCALER + 1)) - 1); - TIM1->CCR4 = arr_pwm_dc_value; + // uint16_t arr_pwm_dc_value = (uint16_t) (F_SYS_CLK_HZ / ((uint32_t) PWM_FREQ_HZ * (PWM_TIM_PRESCALER + 1)) - 1); + // TIM1->CCR4 = arr_pwm_dc_value; // uint16_t ccer = 0; uint16_t ccer = CCER_TIM4_ADC_TRIG; uint16_t ccmr1 = 0; // uint16_t ccmr2 = 0; uint16_t ccmr2 = CCMR2_TIM4_ADC_TRIG; - TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; + // TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; TIM1->CCR4 = 22; diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c index b952be64..3aa7c4de 100644 --- a/motor-controller/common/current_sensing.c +++ b/motor-controller/common/current_sensing.c @@ -187,8 +187,8 @@ CS_Status_t currsen_setup(uint8_t motor_adc_ch) // 7.5 ADC clock cycles (0.625 us) is the minimum sampling time for the motor. // T_settling > ((Imax * Rs * G_real) / SR) = ((10.0 * 0.05 * 4.94) / 10V/us) = 0.247us - // ADC1->SMPR = ADC_SMPR_SMP_0; - ADC1->SMPR = 0; + ADC1->SMPR = ADC_SMPR_SMP_0; + // ADC1->SMPR = 0; // Set ADC channel selection. Ch9 is the Vbus. From 7888082e31d317c12d88086e6408acde77ad601c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 14 Oct 2025 21:40:30 -0400 Subject: [PATCH 17/18] lower motor test vel --- control-board/src/bin/hwtest-motor/main.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 4c63bbaa..fdee895a 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -132,7 +132,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { if ctr > 50 { ctr = 0; } else if ctr > 25 { - vel = 3.0; + vel = 1.0; } else { vel = 0.0; } From 94936ca377201e722b0c1c6d665eee14c181f02c Mon Sep 17 00:00:00 2001 From: Will Stuckey Date: Tue, 14 Oct 2025 23:12:33 -0400 Subject: [PATCH 18/18] current sense calibration tweaks --- control-board/src/bin/hwtest-motor/main.rs | 6 +-- motor-controller/bin/wheel/main.c | 5 ++- motor-controller/common/current_sensing.c | 51 ++++++++++++++-------- motor-controller/common/current_sensing.h | 2 + 4 files changed, 41 insertions(+), 23 deletions(-) diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index fdee895a..7cad285e 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -132,7 +132,7 @@ async fn main(main_spawner: embassy_executor::Spawner) { if ctr > 50 { ctr = 0; } else if ctr > 25 { - vel = 1.0; + vel = 2.0; } else { vel = 0.0; } @@ -141,9 +141,9 @@ async fn main(main_spawner: embassy_executor::Spawner) { .publish(DataPacket::BasicControl(BasicControl { _bitfield_1: Default::default(), _bitfield_align_1: Default::default(), - vel_x_linear: vel, + 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, diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index b9bbf6a2..7ffb0635 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -345,7 +345,10 @@ int main() { // pct of voltage range 0-3.3V float vbatt = currsen_get_vbus_voltage(); // map voltage given by the amp network to current - float current_sense_I = currsen_get_motor_current_with_offset(); // mm_voltage_to_current(&df45_model, current_sense_shunt_v); + float current_sense_I = currsen_get_motor_current(); // mm_voltage_to_current(&df45_model, current_sense_shunt_v); + current_sense_I *= (0.25f / currsen_get_motor_current_offset()); + // current_sense_I = currsen_get_motor_current_offset(); + // current_sense_I = currsen_get_shunt_voltage_raw(); // float current_sense_I = current_sense_shunt_v; // float current_sense_I = current_sense_shunt_v; // map current to torque using the motor model diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c index 3aa7c4de..16ac7264 100644 --- a/motor-controller/common/current_sensing.c +++ b/motor-controller/common/current_sensing.c @@ -22,9 +22,9 @@ static CS_Mode_t m_cs_mode; static ADC_Result_t m_adc_result; static bool m_adc_calibrated = false; -static bool m_motor_current_adc_offset_set = false; -static size_t m_motor_current_adc_offset_ctr = 0; -static float m_motor_current_adc_offset = 0.0f; +static bool m_motor_adc_offset_set = false; +static size_t m_motor_adc_offset_ctr = 0; +static float m_motor_adc_offset = 0.0f; void currsen_enable_ht() { ADC1->CR |= ADC_CR_ADSTART; @@ -364,6 +364,21 @@ CS_Status_t currsen_adc_dis() return CS_OK; } +float currsen_get_shunt_voltage_raw() { + float v_adc = ((float) (m_adc_result.Motor_current_raw)) * V_ADC_SCALE_V; + + return v_adc; +} + +float currsen_get_shunt_voltage() { + float v_motor_sense = (currsen_get_shunt_voltage_raw() - currsen_get_motor_current_offset()) / MOTOR_OPAMP_GAIN_REAL; + if (v_motor_sense < 0.0f) { + v_motor_sense = 0.0f; + } + + return v_motor_sense; +} + /** * @brief gets the motor current. Translate from raw ADC value to * to scaled raw ADC value to motor current. @@ -372,16 +387,14 @@ CS_Status_t currsen_adc_dis() */ float currsen_get_motor_current() { - float v_adc = ((float) (m_adc_result.Motor_current_raw)) * V_ADC_SCALE_V; - float v_motor_sense = (v_adc - V_MIN_OP_AMP) / MOTOR_OPAMP_GAIN_REAL; - float i_motor = v_motor_sense / MOTOR_OPAMP_RESISTOR_SENSE; + float i_motor = currsen_get_shunt_voltage() / MOTOR_OPAMP_RESISTOR_SENSE; return i_motor; } float currsen_get_motor_current_offset() { - return m_motor_current_adc_offset; + return m_motor_adc_offset; } float currsen_get_motor_current_with_offset() @@ -403,21 +416,21 @@ float currsen_get_motor_current_with_offset() bool currsen_calibrate_sense() { - if (m_motor_current_adc_offset_set) { - m_motor_current_adc_offset_set = false; - m_motor_current_adc_offset_ctr = 0; - m_motor_current_adc_offset = 0.0f; + if (m_motor_adc_offset_set) { + m_motor_adc_offset_set = false; + m_motor_adc_offset_ctr = 0; + m_motor_adc_offset = 0.0f; } - m_motor_current_adc_offset += currsen_get_motor_current(); - m_motor_current_adc_offset_ctr++; + m_motor_adc_offset += currsen_get_shunt_voltage_raw(); + m_motor_adc_offset_ctr++; - if (m_motor_current_adc_offset_ctr >= 10) { - m_motor_current_adc_offset /= 10.0f; - m_motor_current_adc_offset_set = true; + if (m_motor_adc_offset_ctr >= 10) { + m_motor_adc_offset /= 10.0f; + m_motor_adc_offset_set = true; } - return m_motor_current_adc_offset_set; + return m_motor_adc_offset_set; } /** @@ -459,9 +472,9 @@ void init_motor_current_adc_offset() // If not set, set to 0.0f. volatile uint32_t* current_offset_ptr = (uint32_t*) CURRENT_OFFSET_ADDRESS; if (*current_offset_ptr != CURRENT_OFFSET_MAGIC) { - m_motor_current_adc_offset = 0.0f; + m_motor_adc_offset = 0.0f; } else { // If the magic number matches, read the float value one after the magic number. - m_motor_current_adc_offset = *((float*) (current_offset_ptr + 1)); + m_motor_adc_offset = *((float*) (current_offset_ptr + 1)); } } \ No newline at end of file diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h index 2919a4f7..bcec03fc 100644 --- a/motor-controller/common/current_sensing.h +++ b/motor-controller/common/current_sensing.h @@ -157,6 +157,8 @@ CS_Status_t currsen_adc_en(); CS_Status_t currsen_adc_dis(); CS_Status_t calculate_motor_zero_current_setpoint(); +float currsen_get_shunt_voltage_raw(); +float currsen_get_shunt_voltage(); float currsen_get_motor_current(); float currsen_get_motor_current_with_offset(); float currsen_get_motor_current_offset();