diff --git a/control-board/.cargo/config.toml b/control-board/.cargo/config.toml index dd405941..b4eb8861 100644 --- a/control-board/.cargo/config.toml +++ b/control-board/.cargo/config.toml @@ -8,4 +8,4 @@ runner = 'probe-rs run --preverify --chip STM32H723ZGTx' [env] # DEFMT_LOG = "error,ateam-control-board::tasks::control_task=trace" -DEFMT_LOG="info" +DEFMT_LOG="trace" diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml index cc34d27e..1fc7e2ec 100644 --- a/control-board/Cargo.toml +++ b/control-board/Cargo.toml @@ -26,7 +26,7 @@ embassy-time = { version = "0.4.0", features = [ embassy-stm32 = { version = "0.3.0", features = [ "defmt", "stm32h723zg", - "unstable-pac", + "unstable-pac", "time-driver-tim1", "exti", "chrono" @@ -124,6 +124,11 @@ name = "hwtest-radio" test = false harness = false +[[bin]] +name = "calibrate-motor" +test = false +harness = false + [[test]] name = "basic-test" harness = false @@ -140,4 +145,3 @@ embassy-sync = { git = "https://github.com/embassy-rs/embassy"} embassy-time = { git = "https://github.com/embassy-rs/embassy"} embassy-stm32 = { git = "https://github.com/embassy-rs/embassy"} embassy-futures = { git = "https://github.com/embassy-rs/embassy"} - diff --git a/control-board/src/bin/calibrate-motor/main.rs b/control-board/src/bin/calibrate-motor/main.rs new file mode 100644 index 00000000..1a4e7d69 --- /dev/null +++ b/control-board/src/bin/calibrate-motor/main.rs @@ -0,0 +1,127 @@ +#![no_std] +#![no_main] + +use embassy_executor::InterruptExecutor; +use embassy_stm32::{ + interrupt, pac::Interrupt +}; +use embassy_sync::pubsub::PubSubChannel; + +use defmt_rtt as _; + +use ateam_control_board::{create_motor_calibrate_task, create_imu_task, create_io_task, create_kicker_task, create_power_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, KickerTelemetryPubSub, LedCommandPubSub, PowerTelemetryPubSub, TelemetryPubSub}, robot_state::SharedRobotState}; + +use embassy_time::Timer; +// provide embedded panic probe +use panic_probe as _; +use static_cell::ConstStaticCell; + +static ROBOT_STATE: ConstStaticCell = ConstStaticCell::new(SharedRobotState::new()); + +static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new(); +static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new(); +static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new(); +static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new(); +static POWER_DATA_CHANNEL: PowerTelemetryPubSub = PubSubChannel::new(); +static KICKER_DATA_CHANNEL: KickerTelemetryPubSub = PubSubChannel::new(); +static LED_COMMAND_PUBSUB: LedCommandPubSub = PubSubChannel::new(); + +static RADIO_UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); +static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new(); + +#[allow(non_snake_case)] +#[interrupt] +unsafe fn CEC() { + UART_QUEUE_EXECUTOR.on_interrupt(); +} + +#[allow(non_snake_case)] +#[interrupt] +unsafe fn CORDIC() { + RADIO_UART_QUEUE_EXECUTOR.on_interrupt(); +} + +#[embassy_executor::main] +async fn main(main_spawner: embassy_executor::Spawner) { + // init system + let sys_config = get_system_config(); + let p = embassy_stm32::init(sys_config); + + defmt::info!("embassy HAL configured."); + + let robot_state = ROBOT_STATE.take(); + + //////////////////////// + // setup task pools // + //////////////////////// + + // uart queue executor should be highest priority + // NOTE: maybe this should be all DMA tasks? No computation tasks here + interrupt::InterruptExt::set_priority(embassy_stm32::interrupt::CEC, embassy_stm32::interrupt::Priority::P7); + let uart_queue_spawner = UART_QUEUE_EXECUTOR.start(Interrupt::CEC); + + ////////////////////////////////////// + // setup inter-task coms channels // + ////////////////////////////////////// + + // commands channel + let control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); + let kicker_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap(); + + // telemetry channel + let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap(); + + // IMU channels + let imu_gyro_data_publisher = GYRO_DATA_CHANNEL.publisher().unwrap(); + let control_gyro_data_subscriber = GYRO_DATA_CHANNEL.subscriber().unwrap(); + let imu_led_cmd_publisher = LED_COMMAND_PUBSUB.publisher().unwrap(); + + let imu_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap(); + let control_accel_data_subscriber = ACCEL_DATA_CHANNEL.subscriber().unwrap(); + + // power channel + let power_board_telemetry_publisher = POWER_DATA_CHANNEL.publisher().unwrap(); + let control_task_power_telemetry_subscriber = POWER_DATA_CHANNEL.subscriber().unwrap(); + + // kicker channel + let kicker_board_telemetry_publisher = KICKER_DATA_CHANNEL.publisher().unwrap(); + let control_task_kicker_telemetry_subscriber = KICKER_DATA_CHANNEL.subscriber().unwrap(); + + // power board + let power_led_cmd_publisher = LED_COMMAND_PUBSUB.publisher().unwrap(); + + /////////////////// + // start tasks // + /////////////////// + + create_io_task!(main_spawner, + robot_state, + p); + + create_power_task!(main_spawner, uart_queue_spawner, + robot_state, power_board_telemetry_publisher, power_led_cmd_publisher, + p); + + create_imu_task!(main_spawner, + robot_state, + imu_gyro_data_publisher, imu_accel_data_publisher, imu_led_cmd_publisher, + p); + + create_motor_calibrate_task!(main_spawner, uart_queue_spawner, + robot_state, + control_command_subscriber, control_telemetry_publisher, + control_task_power_telemetry_subscriber, control_task_kicker_telemetry_subscriber, + control_gyro_data_subscriber, control_accel_data_subscriber, + p); + + create_kicker_task!( + main_spawner, uart_queue_spawner, + robot_state, + kicker_command_subscriber, + kicker_board_telemetry_publisher, + p); + + loop { + Timer::after_millis(10).await; + } +} \ No newline at end of file diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs index 9655cb6c..70f213be 100644 --- a/control-board/src/bin/hwtest-motor/main.rs +++ b/control-board/src/bin/hwtest-motor/main.rs @@ -124,19 +124,17 @@ async fn main(main_spawner: embassy_executor::Spawner) { ); loop { - Timer::after_millis(100).await; - - test_command_publisher - .publish(DataPacket::BasicControl(BasicControl { - _bitfield_1: Default::default(), - _bitfield_align_1: Default::default(), - vel_x_linear: 1.0, - vel_y_linear: 0.0, - vel_z_angular: 0.0, - kick_vel: 0.0, - dribbler_speed: 10.0, - kick_request: KickRequest::KR_DISABLE, - })) - .await; + Timer::after_millis(10).await; + + test_command_publisher.publish(DataPacket::BasicControl(BasicControl { + _bitfield_1: Default::default(), + _bitfield_align_1: Default::default(), + vel_x_linear: 1.0, + vel_y_linear: 0.0, + vel_z_angular: 5.0, + kick_vel: 0.0, + dribbler_speed: 10.0, + kick_request: KickRequest::KR_DISABLE, + })).await; } } diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs index e7608f07..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_setpoint; + pub fn read_vel_computed_duty(&self) -> f32 { + return self.current_state.vel_computed_duty; + } + + pub fn read_current(&self) -> f32 { + return self.current_state.current_estimate; + } + + pub fn read_torque_setpoint(&self) -> f32 { + return self.current_state.torque_setpoint; + } + + pub fn read_torque_estimate(&self) -> f32 { + return self.current_state.torque_estimate; + } + + pub fn read_torque_computed_error(&self) -> f32 { + return self.current_state.torque_computed_error; + } + + pub fn read_torque_computed_nm(&self) -> f32 { + return self.current_state.torque_computed_nm; + } + + pub fn read_torque_computed_duty(&self) -> f32 { + return self.current_state.torque_computed_duty; + } + + pub fn read_vbus_voltage(&self) -> f32 { + return self.current_state.vbus_voltage; } } diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs index 50d4f90c..519b92fa 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 = @@ -43,13 +43,29 @@ static_idle_buffered_uart!(FRONT_RIGHT, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX const TICKS_WITHOUT_PACKET_STOP: usize = 20; +// This is Gyro for all axes. +const MAX_STATIONARY_GYRO_RADS: f32 = 0.1; // rad/s +const MIN_GYRO_STABLE_COUNT: usize = 1000; // number of ticks to wait for gyro to stabilize +// This is Acceleration for x and y. +const MAX_STATIONARY_ACCEL_MS: f32 = 0.1; // m/s^2 + +const WHEEL_VELOCITY_STATIONARY_RADS_MAX: f32 = 0.1; // rad/s +const CURRENT_CALIBRATION_SAMPLES: usize = 1000; // number of samples to take for current calibration + +const CONTROL_LOOP_RATE_MS: u64 = 10; // 100Hz +const CONTROL_LOOP_RATE_S: f32 = CONTROL_LOOP_RATE_MS as f32 / 1000.0; +const CONTROL_MOTION_TYPE: MotionCommandType::Type = MotionCommandType::OPEN_LOOP; + +// Internal macro with do_motor_calibrate as a parameter #[macro_export] -macro_rules! create_control_task { - ($main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident, +macro_rules! __create_control_task_internal { + ( + $main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident, $control_command_subscriber:ident, $control_telemetry_publisher:ident, $power_telemetry_subscriber:ident, $kicker_telemetry_subscriber:ident, $control_gyro_data_subscriber:ident, $control_accel_data_subscriber:ident, - $p:ident + $p:ident, + $do_motor_calibrate:expr ) => { ateam_control_board::tasks::control_task::start_control_task( $main_spawner, @@ -89,11 +105,52 @@ macro_rules! create_control_task { $p.DMA1_CH6, $p.PB12, $p.PB13, + $do_motor_calibrate, ) .await; }; } +// External macro for normal control task +#[macro_export] +macro_rules! create_control_task { + ( + $main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident, + $control_command_subscriber:ident, $control_telemetry_publisher:ident, + $power_telemetry_subscriber:ident, $kicker_telemetry_subscriber:ident, + $control_gyro_data_subscriber:ident, $control_accel_data_subscriber:ident, + $p:ident + ) => { + $crate::__create_control_task_internal!( + $main_spawner, $uart_queue_spawner, $robot_state, + $control_command_subscriber, $control_telemetry_publisher, + $power_telemetry_subscriber, $kicker_telemetry_subscriber, + $control_gyro_data_subscriber, $control_accel_data_subscriber, + $p, false + ); + }; +} + +// External macro for motor calibration task +#[macro_export] +macro_rules! create_motor_calibrate_task { + ( + $main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident, + $control_command_subscriber:ident, $control_telemetry_publisher:ident, + $power_telemetry_subscriber:ident, $kicker_telemetry_subscriber:ident, + $control_gyro_data_subscriber:ident, $control_accel_data_subscriber:ident, + $p:ident + ) => { + $crate::__create_control_task_internal!( + $main_spawner, $uart_queue_spawner, $robot_state, + $control_command_subscriber, $control_telemetry_publisher, + $power_telemetry_subscriber, $kicker_telemetry_subscriber, + $control_gyro_data_subscriber, $control_accel_data_subscriber, + $p, true + ); + }; +} + pub struct ControlTask< const MAX_RX_PACKET_SIZE: usize, const MAX_TX_PACKET_SIZE: usize, @@ -108,9 +165,12 @@ pub struct ControlTask< power_telemetry_subscriber: PowerTelemetrySubscriber, kicker_telemetry_subscriber: KickerTelemetrySubscriber, - last_gyro_rads: f32, + last_gyro_x_rads: f32, + last_gyro_y_rads: f32, + last_gyro_z_rads: f32, last_accel_x_ms: f32, last_accel_y_ms: f32, + last_accel_z_ms: f32, last_command: BasicControl, last_power_telemetry: PowerTelemetry, last_kicker_telemetry: KickerTelemetry, @@ -121,490 +181,687 @@ pub struct ControlTask< motor_fr: ControlWheelMotor, } -impl< - const MAX_RX_PACKET_SIZE: usize, - const MAX_TX_PACKET_SIZE: usize, - const RX_BUF_DEPTH: usize, - const TX_BUF_DEPTH: usize, - > ControlTask -{ - pub fn new( - robot_state: &'static SharedRobotState, - command_subscriber: CommandsSubscriber, - telemetry_publisher: TelemetryPublisher, - gyro_subscriber: GyroDataSubscriber, - accel_subscriber: AccelDataSubscriber, - power_telemetry_subscriber: PowerTelemetrySubscriber, - kicker_telemetry_subscriber: KickerTelemetrySubscriber, - motor_fl: ControlWheelMotor, - motor_bl: ControlWheelMotor, - motor_br: ControlWheelMotor, - motor_fr: ControlWheelMotor, - ) -> Self { - ControlTask { - shared_robot_state: robot_state, - command_subscriber: command_subscriber, - telemetry_publisher: telemetry_publisher, - gyro_subscriber: gyro_subscriber, - accel_subscriber: accel_subscriber, - power_telemetry_subscriber, - kicker_telemetry_subscriber, - last_gyro_rads: 0.0, - last_accel_x_ms: 0.0, - last_accel_y_ms: 0.0, - last_command: Default::default(), - last_power_telemetry: Default::default(), - last_kicker_telemetry: Default::default(), - motor_fl: motor_fl, - motor_bl: motor_bl, - motor_br: motor_br, - motor_fr: motor_fr, +impl < + const MAX_RX_PACKET_SIZE: usize, + const MAX_TX_PACKET_SIZE: usize, + const RX_BUF_DEPTH: usize, + const TX_BUF_DEPTH: usize> + ControlTask + { + + pub fn new(robot_state: &'static SharedRobotState, + command_subscriber: CommandsSubscriber, + telemetry_publisher: TelemetryPublisher, + gyro_subscriber: GyroDataSubscriber, + accel_subscriber: AccelDataSubscriber, + power_telemetry_subscriber: PowerTelemetrySubscriber, + kicker_telemetry_subscriber: KickerTelemetrySubscriber, + motor_fl: ControlWheelMotor, + motor_bl: ControlWheelMotor, + motor_br: ControlWheelMotor, + motor_fr: ControlWheelMotor, + ) -> Self { + ControlTask { + shared_robot_state: robot_state, + command_subscriber: command_subscriber, + telemetry_publisher: telemetry_publisher, + gyro_subscriber: gyro_subscriber, + accel_subscriber: accel_subscriber, + power_telemetry_subscriber: power_telemetry_subscriber, + kicker_telemetry_subscriber: kicker_telemetry_subscriber, + last_gyro_x_rads: 0.0, + last_gyro_y_rads: 0.0, + last_gyro_z_rads: 0.0, + last_accel_x_ms: 0.0, + last_accel_y_ms: 0.0, + last_accel_z_ms: 0.0, + last_command: Default::default(), + last_power_telemetry: Default::default(), + last_kicker_telemetry: Default::default(), + motor_fl: motor_fl, + motor_bl: motor_bl, + motor_br: motor_br, + motor_fr: motor_fr, + } } - } - fn do_control_update( - &mut self, - robot_controller: &mut BodyVelocityController, - cmd_vel: Vector3, - gyro_rads: f32, - controls_enabled: bool, - ) -> Vector4 -/* + fn do_control_update(&mut self, + robot_controller: &mut BodyVelocityController, + cmd_vel: Vector3, + gyro_rads: f32, + controls_enabled: bool + ) -> Vector4 + /* Provide the motion controller with the current wheel velocities and torques from the appropriate sensors, then get a set of wheel velocities to apply based on the controller's current state. - */ { - let wheel_vels = Vector4::new( - self.motor_fl.read_rads(), - self.motor_bl.read_rads(), - self.motor_br.read_rads(), - self.motor_fr.read_rads(), - ); - - // torque values are computed on the spin but put in the current variable - // TODO update this when packet/var names are updated to match software - let wheel_torques = Vector4::new( - self.motor_fl.read_current(), - self.motor_bl.read_current(), - self.motor_br.read_current(), - self.motor_fr.read_current(), - ); + */ + { + let wheel_vels = Vector4::new( + self.motor_fl.read_rads(), + self.motor_bl.read_rads(), + self.motor_br.read_rads(), + self.motor_fr.read_rads() + ); - // TODO read from channel or something + // torque values are computed on the spin but put in the current variable + // TODO update this when packet/var names are updated to match software + let wheel_torques = Vector4::new( + self.motor_fl.read_current(), + self.motor_bl.read_current(), + self.motor_br.read_current(), + self.motor_fr.read_current() + ); - robot_controller.control_update( - &cmd_vel, - &wheel_vels, - &wheel_torques, - gyro_rads, - controls_enabled, - ); - robot_controller.get_wheel_velocities() - } + // TODO read from channel or something - fn send_motor_commands_and_telemetry( - &mut self, - seq_number: u16, - robot_controller: &mut BodyVelocityController, - cur_state: RobotState, - ) { - self.motor_fl.send_motion_command(); - self.motor_bl.send_motion_command(); - self.motor_br.send_motion_command(); - self.motor_fr.send_motion_command(); - - let front_left_motor_error = self.motor_fl.read_is_error() as u32; - let back_left_motor_error = self.motor_bl.read_is_error() as u32; - let back_right_motor_error = self.motor_br.read_is_error() as u32; - let front_right_motor_error = self.motor_fr.read_is_error() as u32; - let dribbler_motor_error = self.last_kicker_telemetry.dribbler_motor.master_error() as u32; - - let front_left_hall_error = self.motor_fl.check_hall_error() as u32; - let back_left_hall_error = self.motor_bl.check_hall_error() as u32; - let back_right_hall_error = self.motor_br.check_hall_error() as u32; - let front_right_hall_error = self.motor_fr.check_hall_error() as u32; - let dribbler_motor_hall_error = self - .last_kicker_telemetry - .dribbler_motor - .hall_disconnected_error() as u32; - - let basic_telem = TelemetryPacket::Basic(BasicTelemetry { - control_data_sequence_number: seq_number as u8, - transmission_sequence_number: 0, - robot_revision_major: ROBOT_VERSION_MAJOR, - robot_revision_minor: ROBOT_VERSION_MINOR, - _bitfield_align_1: Default::default(), - _bitfield_1: BasicTelemetry::new_bitfield_1( - !self.last_power_telemetry.power_ok() as u32, // power error - cur_state.power_inop as u32, // power board error - !self.last_power_telemetry.battery_info.battery_ok() as u32, // battery error - self.last_power_telemetry.battery_info.battery_low() as u32, // battery low - self.last_power_telemetry.battery_info.battery_critical() as u32, // battery crit - self.last_power_telemetry.shutdown_requested() as u32, // shutdown pending - cur_state.robot_tipped as u32, // tipped error - self.last_kicker_telemetry.error_detected() as u32, // breakbeam error - self.last_kicker_telemetry.ball_detected() as u32, // ball detected - cur_state.imu_inop as u32, // accel 0 error - false as u32, // accel 1 error, uninstalled - cur_state.imu_inop as u32, // gyro 0 error - false as u32, // gyro 1 error, uninstalled - front_left_motor_error, - front_left_hall_error, - back_left_motor_error, - back_left_hall_error, - back_right_motor_error, - back_right_hall_error, - front_right_motor_error, - front_right_hall_error, - dribbler_motor_error, - dribbler_motor_hall_error, - self.last_kicker_telemetry.error_detected() as u32, - false as u32, // chipper available - (!cur_state.kicker_inop && self.last_kicker_telemetry.error_detected() == 0) as u32, - self.last_command.body_vel_controls_enabled(), - self.last_command.wheel_vel_control_enabled(), - self.last_command.wheel_torque_control_enabled(), - Default::default(), - ), - battery_percent: self.last_power_telemetry.battery_info.battery_pct as u16, - kicker_charge_percent: self.last_kicker_telemetry.charge_pct, - }); - - if cur_state.radio_bridge_ok { - self.telemetry_publisher.publish_immediate(basic_telem); + robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads, controls_enabled); + robot_controller.get_wheel_velocities() } - let mut control_debug_telem = robot_controller.get_control_debug_telem(); + fn send_motor_commands_and_telemetry(&mut self, + seq_number: u8, + robot_controller: &mut BodyVelocityController, + cur_state: RobotState) + { + + + self.motor_fl.send_motion_command(); + self.motor_bl.send_motion_command(); + self.motor_br.send_motion_command(); + self.motor_fr.send_motion_command(); + + let front_left_motor_error = self.motor_fl.read_is_error() as u32; + let back_left_motor_error = self.motor_bl.read_is_error() as u32; + let back_right_motor_error = self.motor_br.read_is_error() as u32; + let front_right_motor_error = self.motor_fr.read_is_error() as u32; + let dribbler_motor_error = self.last_kicker_telemetry.dribbler_motor.master_error() as u32; + + let front_left_hall_error = self.motor_fl.check_hall_error() as u32; + let back_left_hall_error = self.motor_bl.check_hall_error() as u32; + let back_right_hall_error = self.motor_br.check_hall_error() as u32; + let front_right_hall_error = self.motor_fr.check_hall_error() as u32; + let dribbler_motor_hall_error = self.last_kicker_telemetry.dribbler_motor.hall_disconnected_error() as u32; + + let basic_telem = TelemetryPacket::Basic(BasicTelemetry { + transmission_sequence_number: Default::default(), + control_data_sequence_number: seq_number, + robot_revision_major: ROBOT_VERSION_MAJOR, + robot_revision_minor: ROBOT_VERSION_MINOR, + _bitfield_align_1: Default::default(), + _bitfield_1: BasicTelemetry::new_bitfield_1( + !self.last_power_telemetry.power_ok() as u32, // power error + cur_state.power_inop as u32, // power board error + !self.last_power_telemetry.battery_info.battery_ok() as u32, // battery error + self.last_power_telemetry.battery_info.battery_low() as u32, // battery low + self.last_power_telemetry.battery_info.battery_critical() as u32, // battery crit + self.last_power_telemetry.shutdown_requested() as u32, // shutdown pending + cur_state.robot_tipped as u32, // tipped error + self.last_kicker_telemetry.error_detected() as u32, // breakbeam error + self.last_kicker_telemetry.ball_detected() as u32, // ball detected + cur_state.imu_inop as u32, // accel 0 error + false as u32, // accel 1 error, uninstalled + cur_state.imu_inop as u32, // gyro 0 error + false as u32, // gyro 1 error, uninstalled + front_left_motor_error, + front_left_hall_error, + back_left_motor_error, + back_left_hall_error, + back_right_motor_error, + back_right_hall_error, + front_right_motor_error, + front_right_hall_error, + dribbler_motor_error, + dribbler_motor_hall_error, + self.last_kicker_telemetry.error_detected() as u32, + false as u32, // chipper available + (!cur_state.kicker_inop && self.last_kicker_telemetry.error_detected() == 0) as u32, + self.last_command.body_vel_controls_enabled(), + self.last_command.wheel_vel_control_enabled(), + self.last_command.wheel_torque_control_enabled(), + Default::default()), + battery_percent: self.last_power_telemetry.battery_info.battery_pct as u16, + kicker_charge_percent: self.last_kicker_telemetry.charge_pct, + }); + + if cur_state.radio_bridge_ok { + self.telemetry_publisher.publish_immediate(basic_telem); + } + + let mut control_debug_telem = robot_controller.get_control_debug_telem(); - control_debug_telem.front_left_motor = self.motor_fl.get_latest_state(); - control_debug_telem.back_left_motor = self.motor_bl.get_latest_state(); - control_debug_telem.back_right_motor = self.motor_br.get_latest_state(); - control_debug_telem.front_right_motor = self.motor_fr.get_latest_state(); + control_debug_telem.front_left_motor = self.motor_fl.get_latest_state(); + control_debug_telem.back_left_motor = self.motor_bl.get_latest_state(); + control_debug_telem.back_right_motor = self.motor_br.get_latest_state(); + control_debug_telem.front_right_motor = self.motor_fr.get_latest_state(); - control_debug_telem.imu_accel[0] = self.last_accel_x_ms; - control_debug_telem.imu_accel[1] = self.last_accel_y_ms; + control_debug_telem.imu_accel[0] = self.last_accel_x_ms; + control_debug_telem.imu_accel[1] = self.last_accel_y_ms; - control_debug_telem.kicker_status = self.last_kicker_telemetry; - control_debug_telem.power_status = self.last_power_telemetry; + control_debug_telem.kicker_status = self.last_kicker_telemetry; + control_debug_telem.power_status = self.last_power_telemetry; - let control_debug_telem = TelemetryPacket::Extended(control_debug_telem); - if cur_state.radio_bridge_ok { - self.telemetry_publisher - .publish_immediate(control_debug_telem); + let control_debug_telem = TelemetryPacket::Extended(control_debug_telem); + if cur_state.radio_bridge_ok { + self.telemetry_publisher.publish_immediate(control_debug_telem); + } } - } - async fn control_task_entry(&mut self) { - defmt::info!("control task init."); + async fn control_task_entry(&mut self) { + defmt::info!("control task init."); - // wait for the switch state to be read - while !self.shared_robot_state.hw_init_state_valid() { - Timer::after_millis(10).await; - } + // wait for the switch state to be read + while !self.shared_robot_state.hw_init_state_valid() { + Timer::after_millis(10).await; + } - self.flash_motor_firmware(self.shared_robot_state.hw_in_debug_mode()) - .await; + self.motor_fl.set_telemetry_enabled(true); + self.motor_bl.set_telemetry_enabled(true); + self.motor_br.set_telemetry_enabled(true); + self.motor_fr.set_telemetry_enabled(true); - embassy_futures::join::join4( - self.motor_fl.leave_reset(), - self.motor_bl.leave_reset(), - self.motor_br.leave_reset(), - self.motor_fr.leave_reset(), - ) - .await; + self.motor_fl.send_motion_command(); + self.motor_bl.send_motion_command(); + self.motor_br.send_motion_command(); + self.motor_fr.send_motion_command(); - self.motor_fl.set_telemetry_enabled(true); - self.motor_bl.set_telemetry_enabled(true); - self.motor_br.set_telemetry_enabled(true); - self.motor_fr.set_telemetry_enabled(true); - Timer::after_millis(10).await; + self.flash_motor_firmware( + self.shared_robot_state.hw_in_debug_mode()).await; - let robot_model = self.get_robot_model(); - let mut robot_controller = - BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model); + embassy_futures::join::join4( + self.motor_fl.leave_reset(), + self.motor_bl.leave_reset(), + self.motor_br.leave_reset(), + self.motor_fr.leave_reset(), + ).await; - let mut ctrl_seq_number = 0; - let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10)); + let robot_model = self.get_robot_model(); + let mut robot_controller = BodyVelocityController::new_from_global_params(CONTROL_LOOP_RATE_S, robot_model); - let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); - let mut ticks_since_control_packet = 0; + let mut seq_number = 0; + let mut loop_rate_ticker = Ticker::every(Duration::from_millis(CONTROL_LOOP_RATE_MS)); - let mut last_loop_term_time = Instant::now(); + let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0); + let mut ticks_since_control_packet = 0; - loop { - let loop_start_time = Instant::now(); - let loop_invocation_dead_time = loop_start_time - last_loop_term_time; - if loop_start_time - last_loop_term_time > Duration::from_millis(11) { - defmt::warn!("control loop scheuling lagged. Expected ~10ms between loop invocations, but got {:?}us", loop_invocation_dead_time.as_micros()); + while self.shared_robot_state.get_imu_inop() { + defmt::info!("Waiting for IMU to be ready."); + loop_rate_ticker.next().await; } - self.motor_fl.process_packets(); - self.motor_bl.process_packets(); - self.motor_br.process_packets(); - self.motor_fr.process_packets(); - - let cur_state = self.shared_robot_state.get_state(); - - // self.motor_fl.log_reset("FL"); - // self.motor_bl.log_reset("BL"); - // self.motor_br.log_reset("BR"); - // self.motor_fr.log_reset("FR"); - - ticks_since_control_packet += 1; - while let Some(latest_packet) = self.command_subscriber.try_next_message_pure() { - match latest_packet { - ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { - let new_cmd_vel = Vector3::new( - latest_control.vel_x_linear, - latest_control.vel_y_linear, - latest_control.vel_z_angular, - ); - - cmd_vel = new_cmd_vel; - ticks_since_control_packet = 0; - - if latest_control.reboot_robot() != 0 { - loop { - cortex_m::peripheral::SCB::sys_reset(); + self.motor_fl.set_telemetry_enabled(true); + self.motor_bl.set_telemetry_enabled(true); + self.motor_br.set_telemetry_enabled(true); + self.motor_fr.set_telemetry_enabled(true); + + self.motor_fl.set_motion_enabled(true); + self.motor_bl.set_motion_enabled(true); + self.motor_br.set_motion_enabled(true); + self.motor_fr.set_motion_enabled(true); + + self.motor_fl.set_motion_type(CONTROL_MOTION_TYPE); + self.motor_bl.set_motion_type(CONTROL_MOTION_TYPE); + self.motor_br.set_motion_type(CONTROL_MOTION_TYPE); + self.motor_fr.set_motion_type(CONTROL_MOTION_TYPE); + + Timer::after_millis(10).await; + + // Need to send one motion command to get telemetry started. + self.motor_fl.send_motion_command(); + self.motor_bl.send_motion_command(); + self.motor_br.send_motion_command(); + self.motor_fr.send_motion_command(); + + let mut motor_fl_last_timestamp_ms = self.motor_fl.read_current_timestamp_ms(); + let mut motor_bl_last_timestamp_ms = self.motor_bl.read_current_timestamp_ms(); + let mut motor_br_last_timestamp_ms = self.motor_br.read_current_timestamp_ms(); + let mut motor_fr_last_timestamp_ms = self.motor_fr.read_current_timestamp_ms(); + + let mut last_loop_term_time = Instant::now(); + + loop { + let loop_start_time = Instant::now(); + let loop_invocation_dead_time = loop_start_time - last_loop_term_time; + if loop_start_time - last_loop_term_time > Duration::from_millis(11) { + defmt::warn!("control loop scheuling lagged. Expected ~10ms between loop invocations, but got {:?}us", loop_invocation_dead_time.as_micros()); + } + + self.motor_fl.process_packets(); + self.motor_bl.process_packets(); + self.motor_br.process_packets(); + self.motor_fr.process_packets(); + + // Check if the motors are still responding, and log which ones are not. + let motor_fl_current_timestamp_ms = self.motor_fl.read_current_timestamp_ms(); + let motor_bl_current_timestamp_ms = self.motor_bl.read_current_timestamp_ms(); + let motor_br_current_timestamp_ms = self.motor_br.read_current_timestamp_ms(); + let motor_fr_current_timestamp_ms = self.motor_fr.read_current_timestamp_ms(); + // Log which ones have the same timestamp as before. + if motor_fl_current_timestamp_ms == motor_fl_last_timestamp_ms { + defmt::warn!("FL motor responded with the same timestamp."); + } + + if motor_bl_current_timestamp_ms == motor_bl_last_timestamp_ms { + defmt::warn!("BL motor responded with the same timestamp."); + } + + if motor_br_current_timestamp_ms == motor_br_last_timestamp_ms { + defmt::warn!("BR motor responded with the same timestamp."); + } + + if motor_fr_current_timestamp_ms == motor_fr_last_timestamp_ms { + defmt::warn!("FR motor responded with the same timestamp."); + } + + motor_fl_last_timestamp_ms = motor_fl_current_timestamp_ms; + motor_bl_last_timestamp_ms = motor_bl_current_timestamp_ms; + motor_br_last_timestamp_ms = motor_br_current_timestamp_ms; + motor_fr_last_timestamp_ms = motor_fr_current_timestamp_ms; + + let cur_state = self.shared_robot_state.get_state(); + + // self.motor_fl.log_reset("FL"); + // self.motor_bl.log_reset("BL"); + // self.motor_br.log_reset("BR"); + // self.motor_fr.log_reset("FR"); + + ticks_since_control_packet += 1; + while let Some(latest_packet) = self.command_subscriber.try_next_message_pure() { + match latest_packet { + ateam_common_packets::radio::DataPacket::BasicControl(latest_control) => { + + let new_cmd_vel = Vector3::new( + latest_control.vel_x_linear, + latest_control.vel_y_linear, + latest_control.vel_z_angular, + ); + + cmd_vel = new_cmd_vel; + ticks_since_control_packet = 0; + + if latest_control.reboot_robot() != 0 { + loop { + cortex_m::peripheral::SCB::sys_reset(); + } } - } - - if latest_control.request_shutdown() != 0 { - self.shared_robot_state.flag_shutdown_requested(); - } - - let wheel_motion_type = match ( - self.last_command.wheel_vel_control_enabled() != 0, - self.last_command.wheel_torque_control_enabled() != 0, - ) { - (true, true) => MotionCommandType::BOTH, - (true, false) => MotionCommandType::VELOCITY, - (false, true) => MotionCommandType::TORQUE, - (false, false) => MotionCommandType::OPEN_LOOP, - }; - - self.motor_fl.set_motion_type(wheel_motion_type); - self.motor_bl.set_motion_type(wheel_motion_type); - self.motor_br.set_motion_type(wheel_motion_type); - self.motor_fr.set_motion_type(wheel_motion_type); - - self.last_command = latest_control; - } - ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param_cmd) => { - let param_cmd_resp = robot_controller.apply_command(&latest_param_cmd); - - if let Ok(resp) = param_cmd_resp { - defmt::info!("sending successful parameter update command response"); - let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); - self.telemetry_publisher.publish(tp_resp).await; - } else if let Err(resp) = param_cmd_resp { - defmt::warn!("sending failed parameter updated command response"); - let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); - self.telemetry_publisher.publish(tp_resp).await; - } + + if latest_control.request_shutdown() != 0 { + self.shared_robot_state.flag_shutdown_requested(); + } + + let wheel_motion_type = match (self.last_command.wheel_vel_control_enabled() != 0, self.last_command.wheel_torque_control_enabled() != 0) { + (true, true) => MotionCommandType::VELOCITY_W_TORQUE, + (true, false) => MotionCommandType::VELOCITY, + (false, true) => MotionCommandType::TORQUE, + (false, false) => MotionCommandType::OPEN_LOOP, + }; + + self.motor_fl.set_motion_type(wheel_motion_type); + self.motor_bl.set_motion_type(wheel_motion_type); + self.motor_br.set_motion_type(wheel_motion_type); + self.motor_fr.set_motion_type(wheel_motion_type); + + self.last_command = latest_control; + }, + ateam_common_packets::radio::DataPacket::ParameterCommand(latest_param_cmd) => { + let param_cmd_resp = robot_controller.apply_command(&latest_param_cmd); + + if let Ok(resp) = param_cmd_resp { + defmt::info!("sending successful parameter update command response"); + let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); + self.telemetry_publisher.publish(tp_resp).await; + } else if let Err(resp) = param_cmd_resp { + defmt::warn!("sending failed parameter updated command response"); + let tp_resp = TelemetryPacket::ParameterCommandResponse(resp); + self.telemetry_publisher.publish(tp_resp).await; + } + }, } } - } - if ticks_since_control_packet >= TICKS_WITHOUT_PACKET_STOP { - cmd_vel = Vector3::new(0., 0., 0.); - //defmt::warn!("ticks since packet lockout"); - } + if ticks_since_control_packet >= TICKS_WITHOUT_PACKET_STOP { + cmd_vel = Vector3::new(0., 0., 0.); + //defmt::warn!("ticks since packet lockout"); + } - // now we have setpoint r(t) in self.cmd_vel + // now we have setpoint r(t) in self.cmd_vel - while let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { - self.last_gyro_rads = gyro_rads[2]; - } + while let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { + self.last_gyro_x_rads = gyro_rads[0]; + self.last_gyro_y_rads = gyro_rads[1]; + self.last_gyro_z_rads = gyro_rads[2]; + } - while let Some(accel_ms) = self.accel_subscriber.try_next_message_pure() { - self.last_accel_x_ms = accel_ms[0]; - self.last_accel_y_ms = accel_ms[1]; - } + while let Some(accel_ms) = self.accel_subscriber.try_next_message_pure() { + self.last_accel_x_ms = accel_ms[0]; + self.last_accel_y_ms = accel_ms[1]; + self.last_accel_z_ms = accel_ms[2]; + } + + while let Some(kicker_telemetry) = self.kicker_telemetry_subscriber.try_next_message_pure() { + self.last_kicker_telemetry = kicker_telemetry; + } - while let Some(kicker_telemetry) = - self.kicker_telemetry_subscriber.try_next_message_pure() - { - self.last_kicker_telemetry = kicker_telemetry; + while let Some(power_telemetry) = self.power_telemetry_subscriber.try_next_message_pure() { + self.last_power_telemetry = power_telemetry; + } + + if self.stop_wheels() { + cmd_vel = Vector3::new(0.0, 0.0, 0.0); + } else if self.last_command.game_state_in_stop() != 0 { + // TODO impl 1.5m/s clamping or something + } + + let wheel_vels = if self.stop_wheels() { + defmt::warn!("control task - motor commands locked out"); + Vector4::new(0.0, 0.0, 0.0, 0.0) + } else { + let controls_enabled = self.last_command.body_vel_controls_enabled() != 0; + self.do_control_update(&mut robot_controller, cmd_vel, self.last_gyro_z_rads, controls_enabled) + }; + + self.motor_fl.set_setpoint(wheel_vels[0]); + self.motor_bl.set_setpoint(wheel_vels[1]); + 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 curr offset: {} {} {} {}", self.motor_fl.read_vbus_voltage(), self.motor_bl.read_vbus_voltage(), self.motor_br.read_vbus_voltage(), self.motor_fr.read_vbus_voltage()); + + /////////////////////////////////// + // send commands and telemetry // + /////////////////////////////////// + + self.send_motor_commands_and_telemetry(seq_number, + &mut robot_controller, cur_state); + + // increment seq number + seq_number += 1; + seq_number &= 0x3F; + + 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!("control 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); + } + + last_loop_term_time = Instant::now(); + + loop_rate_ticker.next().await; } + } + + async fn motor_calibrate_task_entry(&mut self) { + defmt::info!("Motor Calibrate task init."); - while let Some(power_telemetry) = - self.power_telemetry_subscriber.try_next_message_pure() - { - self.last_power_telemetry = power_telemetry; + // wait for the switch state to be read + while !self.shared_robot_state.hw_init_state_valid() { + Timer::after_millis(10).await; } - if self.stop_wheels() { - cmd_vel = Vector3::new(0.0, 0.0, 0.0); - } else if self.last_command.game_state_in_stop() != 0 { - // TODO impl 1.5m/s clamping or something + self.flash_motor_firmware( + self.shared_robot_state.hw_in_debug_mode()).await; + + embassy_futures::join::join4( + self.motor_fl.leave_reset(), + self.motor_bl.leave_reset(), + self.motor_br.leave_reset(), + self.motor_fr.leave_reset(), + ).await; + + let mut loop_rate_ticker = Ticker::every(Duration::from_millis(CONTROL_LOOP_RATE_MS)); + + while self.shared_robot_state.get_imu_inop() { + defmt::info!("Waiting for IMU to be ready."); + loop_rate_ticker.next().await; } - let wheel_vels = if self.stop_wheels() { - defmt::warn!("control task - motor commands locked out"); - Vector4::new(0.0, 0.0, 0.0, 0.0) - } else { - let controls_enabled = self.last_command.body_vel_controls_enabled() != 0; - self.do_control_update( - &mut robot_controller, - cmd_vel, - self.last_gyro_rads, - controls_enabled, - ) - }; + self.motor_fl.set_telemetry_enabled(true); + self.motor_bl.set_telemetry_enabled(true); + self.motor_br.set_telemetry_enabled(true); + self.motor_fr.set_telemetry_enabled(true); - self.motor_fl.set_setpoint(wheel_vels[0]); - self.motor_bl.set_setpoint(wheel_vels[1]); - self.motor_br.set_setpoint(wheel_vels[2]); - self.motor_fr.set_setpoint(wheel_vels[3]); + self.motor_fl.set_calibrate_current(true); + self.motor_bl.set_calibrate_current(true); + self.motor_br.set_calibrate_current(true); + self.motor_fr.set_calibrate_current(true); - // 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()); + // Need to send one motion command to get telemetry started. + self.motor_fl.send_motion_command(); + self.motor_bl.send_motion_command(); + self.motor_br.send_motion_command(); + self.motor_fr.send_motion_command(); - /////////////////////////////////// - // send commands and telemetry // - /////////////////////////////////// + Timer::after_millis(10).await; - self.send_motor_commands_and_telemetry( - ctrl_seq_number, - &mut robot_controller, - cur_state, - ); + let mut gyro_movement_detected; + let mut gyro_stable_count = 0; + defmt::info!("Waiting for gyro data to be settled."); + loop { + while let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() { + self.last_gyro_x_rads = gyro_rads[0]; + self.last_gyro_y_rads = gyro_rads[1]; + self.last_gyro_z_rads = gyro_rads[2]; + } - // increment seq number - ctrl_seq_number = (ctrl_seq_number + 1) & 0x00FF; + gyro_movement_detected = self.last_gyro_x_rads.abs() > MAX_STATIONARY_GYRO_RADS || + self.last_gyro_y_rads.abs() > MAX_STATIONARY_GYRO_RADS || + self.last_gyro_z_rads.abs() > MAX_STATIONARY_GYRO_RADS; - 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!("control 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 gyro_movement_detected { + defmt::info!("Gyro movement detected! Stop that! Rads: {}, {}, {}", + self.last_gyro_x_rads, + self.last_gyro_y_rads, + self.last_gyro_z_rads); + gyro_stable_count = 0; + } + else { + gyro_stable_count += 1; + } + + if gyro_stable_count >= MIN_GYRO_STABLE_COUNT { + defmt::info!("Gyro data settled."); + break; + } + + loop_rate_ticker.next().await; } - last_loop_term_time = Instant::now(); + defmt::info!("Starting motor calibration."); + let mut motor_fl_current_offset: f32 = 0.0; + let mut motor_bl_current_offset: f32 = 0.0; + let mut motor_br_current_offset: f32 = 0.0; + let mut motor_fr_current_offset: f32 = 0.0; + let mut motor_average_count = 0; + loop { + self.motor_fl.process_packets(); + self.motor_bl.process_packets(); + self.motor_br.process_packets(); + self.motor_fr.process_packets(); + + defmt::info!("Current timestamp ms: {}, {}, {}, {}", + self.motor_fl.read_current_timestamp_ms(), + self.motor_bl.read_current_timestamp_ms(), + self.motor_br.read_current_timestamp_ms(), + self.motor_fr.read_current_timestamp_ms()); + + if self.motor_fl.read_rads().abs() > WHEEL_VELOCITY_STATIONARY_RADS_MAX || + self.motor_bl.read_rads().abs() > WHEEL_VELOCITY_STATIONARY_RADS_MAX || + self.motor_br.read_rads().abs() > WHEEL_VELOCITY_STATIONARY_RADS_MAX || + self.motor_fr.read_rads().abs() > WHEEL_VELOCITY_STATIONARY_RADS_MAX { + defmt::info!("One or more motors are moving, waiting for them to stop. Vel: FL: {}, BL: {}, BR: {}, FR: {}", + self.motor_fl.read_rads(), + self.motor_bl.read_rads(), + self.motor_br.read_rads(), + self.motor_fr.read_rads()); + } + else { + motor_fl_current_offset += self.motor_fl.read_current(); + motor_bl_current_offset += self.motor_bl.read_current(); + motor_br_current_offset += self.motor_br.read_current(); + motor_fr_current_offset += self.motor_fr.read_current(); + motor_average_count += 1; + + if motor_average_count >= CURRENT_CALIBRATION_SAMPLES { + motor_fl_current_offset /= motor_average_count as f32; + motor_bl_current_offset /= motor_average_count as f32; + motor_br_current_offset /= motor_average_count as f32; + motor_fr_current_offset /= motor_average_count as f32; + + defmt::info!("Calibration complete. FL: {}, BL: {}, BR: {}, FR: {}", + motor_fl_current_offset, + motor_bl_current_offset, + motor_br_current_offset, + motor_fr_current_offset); + + break; + } + } - loop_rate_ticker.next().await; - } - } + loop_rate_ticker.next().await; + } + + defmt::info!("Setting motor current offsets."); + let mut motor_constant_error = false; - async fn flash_motor_firmware(&mut self, debug: bool) { - defmt::info!("flashing firmware"); - let force_flash = debug; - if debug { - let mut had_motor_error = false; - if self - .motor_fl - .init_default_firmware_image(force_flash) - .await - .is_err() - { - defmt::error!("failed to flash FL"); - had_motor_error = true; + + if self.motor_fl.save_motor_current_constants(motor_fl_current_offset).await.is_err() { + defmt::error!("Failed to save FL motor current constants"); + motor_constant_error = true; } else { - defmt::info!("FL flashed"); + defmt::info!("FL motor current constants saved."); } - if self - .motor_bl - .init_default_firmware_image(force_flash) - .await - .is_err() - { - defmt::error!("failed to flash BL"); - had_motor_error = true; + if self.motor_bl.save_motor_current_constants(motor_bl_current_offset).await.is_err() { + defmt::error!("Failed to save BL motor current constants"); + motor_constant_error = true; } else { - defmt::info!("BL flashed"); + defmt::info!("BL motor current constants saved."); } - if self - .motor_br - .init_default_firmware_image(force_flash) - .await - .is_err() - { - defmt::error!("failed to flash BR"); - had_motor_error = true; + if self.motor_br.save_motor_current_constants(motor_br_current_offset).await.is_err() { + defmt::error!("Failed to save BR motor current constants"); + motor_constant_error = true; } else { - defmt::info!("BR flashed"); + defmt::info!("BR motor current constants saved."); } - if self - .motor_fr - .init_default_firmware_image(force_flash) - .await - .is_err() - { - defmt::error!("failed to flash FR"); - had_motor_error = true; + if self.motor_fr.save_motor_current_constants(motor_fr_current_offset).await.is_err() { + defmt::error!("Failed to save FR motor current constants"); + motor_constant_error = true; } else { - defmt::info!("FR flashed"); + defmt::info!("FR motor current constants saved."); } - if had_motor_error { - defmt::error!("one or more motors failed to flash.") + if motor_constant_error { + defmt::error!("One or more motors failed to save current constants. Run again!"); } else { - defmt::debug!("all motors flashed"); + defmt::info!("All motors current constants saved."); + } + + loop { + loop_rate_ticker.next().await; } - } else { - let res = embassy_futures::join::join4( - self.motor_fl.init_default_firmware_image(force_flash), - self.motor_bl.init_default_firmware_image(force_flash), - self.motor_br.init_default_firmware_image(force_flash), - self.motor_fr.init_default_firmware_image(force_flash), - ) - .await; - - let error_mask = res.0.is_err() as u8 - | ((res.1.is_err() as u8) & 0x01) << 1 - | ((res.2.is_err() as u8) & 0x01) << 2 - | ((res.3.is_err() as u8) & 0x01) << 3; - - self.shared_robot_state.set_wheels_inop(error_mask); - - if error_mask != 0 { - defmt::error!( - "failed to flash drive motor (FL, BL, BR, FR, DRIB): {}", - res - ); + } + + async fn flash_motor_firmware(&mut self, debug: bool) { + defmt::info!("flashing firmware"); + let force_flash = debug; + if debug { + let mut had_motor_error = false; + if self.motor_fl.init_default_firmware_image(force_flash).await.is_err() { + defmt::error!("failed to flash FL"); + had_motor_error = true; + } else { + defmt::info!("FL flashed"); + } + + if self.motor_bl.init_default_firmware_image(force_flash).await.is_err() { + defmt::error!("failed to flash BL"); + had_motor_error = true; + } else { + defmt::info!("BL flashed"); + } + + if self.motor_br.init_default_firmware_image(force_flash).await.is_err() { + defmt::error!("failed to flash BR"); + had_motor_error = true; + } else { + defmt::info!("BR flashed"); + } + + if self.motor_fr.init_default_firmware_image(force_flash).await.is_err() { + defmt::error!("failed to flash FR"); + had_motor_error = true; + } else { + defmt::info!("FR flashed"); + } + + if had_motor_error { + defmt::error!("one or more motors failed to flash.") + } else { + defmt::debug!("all motors flashed"); + } } else { - defmt::debug!("motor firmware flashed"); + let res = embassy_futures::join::join4( + self.motor_fl.init_default_firmware_image(force_flash), + self.motor_bl.init_default_firmware_image(force_flash), + self.motor_br.init_default_firmware_image(force_flash), + self.motor_fr.init_default_firmware_image(force_flash), + ) + .await; + + let error_mask = res.0.is_err() as u8 + | ((res.1.is_err() as u8) & 0x01) << 1 + | ((res.2.is_err() as u8) & 0x01) << 2 + | ((res.3.is_err() as u8) & 0x01) << 3; + + self.shared_robot_state.set_wheels_inop(error_mask); + + if error_mask != 0 { + defmt::error!("failed to flash drive motor (FL, BL, BR, FR, DRIB): {}", res); + } else { + defmt::debug!("motor firmware flashed"); + } } } - } - fn get_robot_model(&mut self) -> motion::robot_model::RobotModel { - let robot_model_constants: RobotConstants = RobotConstants { - wheel_angles_rad: Vector4::new( - WHEEL_ANGLES_DEG[0].to_radians(), - WHEEL_ANGLES_DEG[1].to_radians(), - WHEEL_ANGLES_DEG[2].to_radians(), - WHEEL_ANGLES_DEG[3].to_radians(), - ), - wheel_radius_m: Vector4::new( - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - WHEEL_RADIUS_M, - ), - wheel_dist_to_cent_m: Vector4::new( - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - WHEEL_DISTANCE_TO_ROBOT_CENTER_M, - ), - }; - - let robot_model: RobotModel = RobotModel::new(robot_model_constants); - - return robot_model; - } + fn get_robot_model(&mut self) -> motion::robot_model::RobotModel{ + let robot_model_constants: RobotConstants = RobotConstants { + wheel_angles_rad: Vector4::new( + WHEEL_ANGLES_DEG[0].to_radians(), + WHEEL_ANGLES_DEG[1].to_radians(), + WHEEL_ANGLES_DEG[2].to_radians(), + WHEEL_ANGLES_DEG[3].to_radians(), + ), + wheel_radius_m: Vector4::new( + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + WHEEL_RADIUS_M, + ), + wheel_dist_to_cent_m: Vector4::new( + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + WHEEL_DISTANCE_TO_ROBOT_CENTER_M, + ), + }; - fn stop_wheels(&self) -> bool { - // defmt::debug!("hco: {}, sd req: {}, estop: {}", self.last_power_telemetry.high_current_operations_allowed() == 0, self.shared_robot_state.shutdown_requested(), self.last_command.emergency_stop() != 0); + let robot_model: RobotModel = RobotModel::new(robot_model_constants); - // self.last_power_telemetry.high_current_operations_allowed() == 0 - self.shared_robot_state.shutdown_requested() || self.last_command.emergency_stop() != 0 - } + return robot_model; + } + + fn stop_wheels(&self) -> bool { + // defmt::debug!("hco: {}, sd req: {}, estop: {}", self.last_power_telemetry.high_current_operations_allowed() == 0, self.shared_robot_state.shutdown_requested(), self.last_command.emergency_stop() != 0); + + // self.last_power_telemetry.high_current_operations_allowed() == 0 + self.shared_robot_state.shutdown_requested() + || self.last_command.emergency_stop() != 0 + } } #[embassy_executor::task] @@ -622,6 +879,14 @@ async fn control_task_entry( } } +#[embassy_executor::task] +async fn motor_calibrate_task_entry(mut control_task: ControlTask) { + loop { + control_task.motor_calibrate_task_entry().await; + defmt::error!("Motor calibrate task returned"); + } +} + pub async fn start_control_task( control_task_spawner: Spawner, uart_queue_spawner: SendSpawner, @@ -660,8 +925,9 @@ pub async fn start_control_task( motor_fr_tx_dma: Peri<'static, MotorFRDmaTx>, motor_fr_boot0_pin: Peri<'static, MotorFRBootPin>, motor_fr_nrst_pin: Peri<'static, MotorFRResetPin>, + do_motor_calibrate: bool, ) { - let initial_motor_controller_uart_conifg = stm32_interface::get_bootloader_uart_config(); + let initial_motor_controller_uart_config = stm32_interface::get_bootloader_uart_config(); ////////////////////////// // create motor uarts // @@ -674,7 +940,7 @@ pub async fn start_control_task( SystemIrqs, motor_fl_tx_dma, motor_fl_rx_dma, - initial_motor_controller_uart_conifg, + initial_motor_controller_uart_config, ) .unwrap(); let bl_uart = Uart::new( @@ -684,7 +950,7 @@ pub async fn start_control_task( SystemIrqs, motor_bl_tx_dma, motor_bl_rx_dma, - initial_motor_controller_uart_conifg, + initial_motor_controller_uart_config, ) .unwrap(); let br_uart = Uart::new( @@ -694,7 +960,7 @@ pub async fn start_control_task( SystemIrqs, motor_br_tx_dma, motor_br_rx_dma, - initial_motor_controller_uart_conifg, + initial_motor_controller_uart_config, ) .unwrap(); let fr_uart = Uart::new( @@ -704,7 +970,7 @@ pub async fn start_control_task( SystemIrqs, motor_fr_tx_dma, motor_fr_rx_dma, - initial_motor_controller_uart_conifg, + initial_motor_controller_uart_config, ) .unwrap(); @@ -773,7 +1039,12 @@ pub async fn start_control_task( motor_fr, ); - control_task_spawner - .spawn(control_task_entry(control_task)) - .unwrap(); + if !do_motor_calibrate { + defmt::info!("Control task starting!"); + control_task_spawner.spawn(control_task_entry(control_task)).unwrap(); + } + else { + defmt::info!("Motor calibration starting!"); + control_task_spawner.spawn(motor_calibrate_task_entry(control_task)).unwrap(); + } } diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs index 9335be5f..5a7eb3c6 100644 --- a/control-board/src/tasks/radio_task.rs +++ b/control-board/src/tasks/radio_task.rs @@ -1,4 +1,4 @@ -use ateam_common_packets::{bindings::BasicTelemetry, radio::TelemetryPacket}; +use ateam_common_packets::{bindings::{BasicTelemetry, RadioPacket}, radio::TelemetryPacket}; use ateam_lib_stm32::{ idle_buffered_uart_read_task, idle_buffered_uart_write_task, static_idle_buffered_uart, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}, @@ -53,9 +53,9 @@ macro_rules! create_radio_task { pub const RADIO_LOOP_RATE_MS: u64 = 10; -pub const RADIO_MAX_TX_PACKET_SIZE: usize = 448; +pub const RADIO_MAX_TX_PACKET_SIZE: usize = core::mem::size_of::(); pub const RADIO_TX_BUF_DEPTH: usize = 4; -pub const RADIO_MAX_RX_PACKET_SIZE: usize = 256; +pub const RADIO_MAX_RX_PACKET_SIZE: usize = core::mem::size_of::(); pub const RADIO_RX_BUF_DEPTH: usize = 4; static_idle_buffered_uart!(RADIO, RADIO_MAX_RX_PACKET_SIZE, RADIO_RX_BUF_DEPTH, RADIO_MAX_TX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, DEBUG_RADIO_UART_QUEUES, #[link_section = ".axisram.buffers"]); diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs index ca697fe3..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."); @@ -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."); diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c index 6a3d74b4..41f511cc 100644 --- a/motor-controller/bin/dribbler/main.c +++ b/motor-controller/bin/dribbler/main.c @@ -59,8 +59,7 @@ 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); // initialize motor driver pwm6step_setup(); @@ -212,7 +211,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 = currsen_get_motor_current(); // TODO: recover current from voltage // TODO: estimate torque from current // TODO: filter? @@ -229,7 +228,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/dribbler/main.h b/motor-controller/bin/dribbler/main.h index 2e1149ac..4a8598da 100644 --- a/motor-controller/bin/dribbler/main.h +++ b/motor-controller/bin/dribbler/main.h @@ -1,12 +1,12 @@ /** * @file main.h * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-07-10 - * + * * @copyright Copyright (c) 2022 - * + * */ #pragma once @@ -15,10 +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 - /////////////////////////////////////////// // current sense amplification network // /////////////////////////////////////////// diff --git a/motor-controller/bin/dribbler/setup.c b/motor-controller/bin/dribbler/setup.c index d3fe35e3..0a0bb774 100644 --- a/motor-controller/bin/dribbler/setup.c +++ b/motor-controller/bin/dribbler/setup.c @@ -101,8 +101,8 @@ inline void setup_clocks() { // RTCSEL -> LSI (40Khz) RCC->BDCR |= RCC_BDCR_RTCSEL_LSI; - // HSI14 enable (for ADC) - // on by default?? + // ADC Clock Divider set in ADC setup. + // Based on PCLK (APB clock). // Setup TIM14 sysclk source RCC->CFGR |= RCC_CFGR_MCO_SYSCLK; diff --git a/motor-controller/bin/dribbler/system.h b/motor-controller/bin/dribbler/system.h index d0427c03..d7982023 100644 --- a/motor-controller/bin/dribbler/system.h +++ b/motor-controller/bin/dribbler/system.h @@ -16,21 +16,19 @@ /////////////////// #define UART_ENABLED -#define IOQ_BUF_LENGTH 64 +#define IOQ_BUF_LENGTH 80 #define IOQ_BUF_DEPTH 4 ////////////////// // ADC Config // ////////////////// -// #define ADC_MODE CS_MODE_DMA // #define ADC_NUM_CHANNELS 5 // #define ADC_CH_MASK (ADC_CHSELR_CHSEL3 | ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL5 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17) // #define ADC_SR_MASK (ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2) -#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_CH_MASK (ADC_CHSELR_CHSEL3) #define ADC_SR_MASK (ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2) //////////////////// diff --git a/motor-controller/bin/wheel-test/main.c b/motor-controller/bin/wheel-test/main.c index 45e78ae3..4da4ff1b 100644 --- a/motor-controller/bin/wheel-test/main.c +++ b/motor-controller/bin/wheel-test/main.c @@ -62,8 +62,7 @@ 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); // initialize motor driver pwm6step_setup(); @@ -108,7 +107,7 @@ int main() { // define the control points the loops use to interact float r_motor_board = 0.0f; - float control_setpoint_vel_duty = 0.0f; + float vel_computed_duty = 0.0f; float control_setpoint_vel_rads = 0.0f; float control_setpoint_vel_rads_prev = 0.0f; float u_torque_loop = 0.0f; @@ -116,7 +115,9 @@ int main() { // recovered velocities (helps torque recover direction) float enc_vel_rads = 0.0f; - float enc_rad_s_filt = 0.0f; + float enc_vel_rads_filt = 0.0f; + // The velocity loop target acceleration based off of commanded speed and measured encoder in rad/s^2 + float vel_target_accel_rads2 = 0.0f; // initialize the motor model for the Nanotec DF45 50W (this is our drive motor) MotorModel_t df45_model; @@ -132,6 +133,8 @@ int main() { df45_model.current_to_torque_linear_model_b = DF45_CURRENT_TO_TORQUE_LINEAR_B; df45_model.rads_to_dc_linear_map_m = DF45_RADS_TO_DC_LINEAR_M; df45_model.rads_to_dc_linear_map_b = DF45_RADS_TO_DC_LINEAR_B; + df45_model.line_resistance = DF45_LINE_RESISTANCE; + df45_model.back_emf_constant = DF45_BACK_EMF_CONSTANT; // setup the velocity PID PidConstants_t vel_pid_constants; @@ -157,9 +160,6 @@ int main() { turn_off_red_led(); turn_off_yellow_led(); - // For test image, always turn on green LED first - turn_on_green_led(); - // Initialize UART and logging status. #ifdef UART_ENABLED uart_initialize(); @@ -291,52 +291,61 @@ int main() { // run the torque loop if applicable 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); - // map current to torque using the motor model + // Get the current in amps from the current sense ADC. + // Should always be positive with the current electrical architecture. + float current_sense_I = currsen_get_motor_current(); + float vbus_voltage = currsen_get_vbus_voltage(); + // Map current to torque using the motor model float measured_torque_Nm = mm_current_to_torque(&df45_model, current_sense_I); - // filter torque + // Filter torque measured_torque_Nm = iir_filter_update(&torque_filter, measured_torque_Nm); - // TODO: add filter? - - // correct torque sign from recovered velocity - // TODO: this should probably be acceleration based not velocity - // e.g. F = ma - if (enc_vel_rads < 0.0f) { - measured_torque_Nm = -fabs(measured_torque_Nm); + // Correct torque sign based on previous duty cycle direction. + MotorDirection_t current_motor_direction = pwm6step_get_motor_direction(); + if (current_motor_direction == COUNTER_CLOCKWISE) { + // if the motor is spinning counter-clockwise, then the torque is negative. + measured_torque_Nm = -measured_torque_Nm; } // choose setpoint - float r_Nm; + float r_Nm = 0.0f; if (motion_control_type == TORQUE) { r_Nm = r_motor_board; - } else { - r_Nm = control_setpoint_vel_duty; + } + else if (motion_control_type == VELOCITY_W_TORQUE) { + // This assumes that the controller will keep + // velocity at the setpoint if we hit acceleration = 0. + // Inspired by SKUBA https://arxiv.org/pdf/1708.02271 + r_Nm = vel_target_accel_rads2; } - // calculate PID on the torque in Nm - float torque_setpoint_Nm = pid_calculate(&torque_pid, r_Nm, measured_torque_Nm, TORQUE_LOOP_RATE_S); + // Calculate the torque setpoint in Nm + float torque_setpoint_Nm = 0.0f; + if (motion_control_type == TORQUE || + motion_control_type == VELOCITY_W_TORQUE) { - // convert desired torque to desired current - float current_setpoint = mm_torque_to_current(&df45_model, fabs(torque_setpoint_Nm)); - // convert desired current to desired duty cycle - u_torque_loop = mm_pos_current_to_pos_dc(&df45_model, current_setpoint); + torque_setpoint_Nm = pid_calculate(&torque_pid, r_Nm, measured_torque_Nm, TORQUE_LOOP_RATE_S); - if (torque_setpoint_Nm < 0.0f) { - u_torque_loop = -fabs(u_torque_loop); + // Convert desired torque to desired current + float current_setpoint = mm_torque_to_current(&df45_model, fabs(torque_setpoint_Nm)); + // Clamp the current setpoint to zero and the maximum current. + current_setpoint = fmax(fmin(current_setpoint, 0.0f), DF45_MAX_CURRENT); + + // Convert desired current to desired duty cycle. + u_torque_loop = mm_pos_current_to_pos_dc(&df45_model, current_setpoint, vbus_voltage, enc_vel_rads_filt); + // Match the sign of the torque setpoint to the duty cycle. + u_torque_loop = copysignf(u_torque_loop, torque_setpoint_Nm); } // load data frame // torque control data response_packet.data.motion.torque_setpoint = r_Nm; + response_packet.data.motion.vbus_voltage = vbus_voltage; 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; + response_packet.data.motion.torque_computed_duty = u_torque_loop; } // run velocity loop if applicable @@ -347,10 +356,13 @@ int main() { enc_vel_rads = discrete_time_derivative(rads_delta, VELOCITY_LOOP_RATE_S); // filter the recovered velocity - enc_rad_s_filt = iir_filter_update(&encoder_filter, enc_vel_rads); + enc_vel_rads_filt = iir_filter_update(&encoder_filter, enc_vel_rads); + + // Vel_desired - Vel_measured + vel_target_accel_rads2 = (r_motor_board - enc_vel_rads_filt) / VELOCITY_LOOP_RATE_S; // compute the velocity PID - control_setpoint_vel_rads = pid_calculate(&vel_pid, r_motor_board, enc_rad_s_filt, VELOCITY_LOOP_RATE_S); + control_setpoint_vel_rads = pid_calculate(&vel_pid, r_motor_board, enc_vel_rads_filt, VELOCITY_LOOP_RATE_S); // Clamp setpoint acceleration float setpoint_accel_rads_2 = (control_setpoint_vel_rads - control_setpoint_vel_rads_prev)/VELOCITY_LOOP_RATE_S; @@ -364,22 +376,34 @@ int main() { control_setpoint_vel_rads_prev = control_setpoint_vel_rads; // back convert rads to duty cycle - control_setpoint_vel_duty = mm_rads_to_dc(&df45_model, control_setpoint_vel_rads); + vel_computed_duty = mm_rads_to_dc(&df45_model, control_setpoint_vel_rads); // 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_enc_estimate = enc_vel_rads_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 = vel_computed_duty; } if (run_torque_loop || run_vel_loop) { // detect if the encoder is not pulling the detect pin down - bool encoder_disconnected = (GPIOA->IDR & GPIO_IDR_5) != 0; - - pwm6step_set_duty_cycle_f(0.10); + // bool encoder_disconnected = (GPIOA->IDR & GPIO_IDR_5) != 0; + bool encoder_disconnected = false; + + // set the motor duty cycle + 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_duty = r_motor; + pwm6step_set_duty_cycle_f(r_motor); + } else if (motion_control_type == VELOCITY) { + pwm6step_set_duty_cycle_f(vel_computed_duty); + } else { + // For torque control + velocity with torque control + pwm6step_set_duty_cycle_f(u_torque_loop); + } // load system state for transmit @@ -458,7 +482,6 @@ 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.data.params.vel_p = vel_pid_constants.kP; response_packet.data.params.vel_i = vel_pid_constants.kI; @@ -512,5 +535,26 @@ int main() { } else { 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. + if (!telemetry_enabled) { + turn_off_green_led(); + } else { + turn_on_green_led(); + } + + #ifdef COMP_MODE + if (telemetry_enabled && + 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 + // the system when in COMP_MODE. + // This is a safety feature to prevent the robot from + // running away if the upstream controller locks up. + while (true); + } + #endif } } diff --git a/motor-controller/bin/wheel-test/main.h b/motor-controller/bin/wheel-test/main.h index c89e19cf..515aaa2d 100644 --- a/motor-controller/bin/wheel-test/main.h +++ b/motor-controller/bin/wheel-test/main.h @@ -1,12 +1,12 @@ /** * @file main.h * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-07-10 - * + * * @copyright Copyright (c) 2022 - * + * */ #include @@ -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 @@ -48,11 +47,14 @@ #define DF45_CURRENT_TO_TORQUE_LINEAR_B 0.00242f #define DF45_RADS_TO_DC_LINEAR_M 0.00144873f #define DF45_RADS_TO_DC_LINEAR_B 0.0057431f +#define DF45_LINE_RESISTANCE 0.4f // 0.8 ohm is line to line resistance, so 0.4 ohm is line to neutral +#define DF45_BACK_EMF_CONSTANT 0.0335f // V*s/rad, based on https://www.nanotec.com/us/en/products/1789-df45m024053-a2 +#define DF45_MAX_CURRENT 4.0f // A. This is around the middle of rated current and peak current for the motor. //////////////////////// // FILTERING/TUNING // //////////////////////// #define ENCODER_IIR_TF_MS 0.20f -#define TORQUE_IIR_TF_MS 0.40f +#define TORQUE_IIR_TF_MS 40.0f // #define DC_IIR_TF_MS 0.20f \ No newline at end of file diff --git a/motor-controller/bin/wheel-test/system.h b/motor-controller/bin/wheel-test/system.h index 0d5d9a17..da3df585 100644 --- a/motor-controller/bin/wheel-test/system.h +++ b/motor-controller/bin/wheel-test/system.h @@ -18,16 +18,15 @@ /////////////////// #define UART_ENABLED -#define IOQ_BUF_LENGTH 64 +#define IOQ_BUF_LENGTH 80 #define IOQ_BUF_DEPTH 4 ////////////////// // ADC Config // ////////////////// -#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_CH_MASK (ADC_CHSELR_CHSEL3) #define ADC_SR_MASK (ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2) //////////////////// diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c index 03d58be3..47e1735f 100644 --- a/motor-controller/bin/wheel/main.c +++ b/motor-controller/bin/wheel/main.c @@ -13,6 +13,7 @@ #include #include #include +#include #include "ateam-common-packets/include/stspin.h" @@ -50,14 +51,36 @@ int main() { turn_off_yellow_led(); turn_off_green_led(); - // turn on Red/Yellow LED - turn_on_red_led(); + // turn on Yellow LED turn_on_yellow_led(); // Setups clocks setup(); - // start watchdog + // setup the loop rate regulators + SyncTimer_t vel_loop_timer; + time_sync_init(&vel_loop_timer, VELOCITY_LOOP_RATE_MS); + SyncTimer_t torque_loop_timer; + time_sync_init(&torque_loop_timer, TORQUE_LOOP_RATE_MS); + SyncTimer_t telemetry_timer; + time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); + + uint32_t ticks_since_last_command_packet = 0; + bool telemetry_enabled = false; + bool motion_enabled = false; + bool calibrate_current = false; + + // 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 before 6step set up. 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 @@ -65,13 +88,6 @@ int main() { 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 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(); pwm6step_set_duty_cycle_f(0.0f); @@ -79,10 +95,6 @@ int main() { // enable ADC hardware trigger (tied to 6step timer) currsen_enable_ht(); - // setup encoder - quadenc_setup(); - quadenc_reset_encoder_delta(); - // Initialized response_packet here to capture the reset method. MotorResponse response_packet; memset(&response_packet, 0, sizeof(MotorResponse)); @@ -95,14 +107,6 @@ int main() { bool params_return_packet_requested = false; - // setup the loop rate regulators - SyncTimer_t vel_loop_timer; - time_sync_init(&vel_loop_timer, VELOCITY_LOOP_RATE_MS); - SyncTimer_t torque_loop_timer; - time_sync_init(&torque_loop_timer, TORQUE_LOOP_RATE_MS); - SyncTimer_t telemetry_timer; - time_sync_init(&telemetry_timer, TELEMETRY_LOOP_RATE_MS); - // setup the velocity filter IIRFilter_t encoder_filter; iir_filter_init(&encoder_filter, iir_filter_alpha_from_Tf(ENCODER_IIR_TF_MS, VELOCITY_LOOP_RATE_MS)); @@ -115,7 +119,7 @@ int main() { // define the control points the loops use to interact float r_motor_board = 0.0f; - float control_setpoint_vel_duty = 0.0f; + float vel_computed_duty = 0.0f; float control_setpoint_vel_rads = 0.0f; float control_setpoint_vel_rads_prev = 0.0f; float u_torque_loop = 0.0f; @@ -123,7 +127,9 @@ int main() { // recovered velocities (helps torque recover direction) float enc_vel_rads = 0.0f; - float enc_rad_s_filt = 0.0f; + float enc_vel_rads_filt = 0.0f; + // The velocity loop target acceleration based off of commanded speed and measured encoder in rad/s^2 + float vel_target_accel_rads2 = 0.0f; // initialize the motor model for the Nanotec DF45 50W (this is our drive motor) MotorModel_t df45_model; @@ -139,6 +145,8 @@ int main() { df45_model.current_to_torque_linear_model_b = DF45_CURRENT_TO_TORQUE_LINEAR_B; df45_model.rads_to_dc_linear_map_m = DF45_RADS_TO_DC_LINEAR_M; df45_model.rads_to_dc_linear_map_b = DF45_RADS_TO_DC_LINEAR_B; + df45_model.line_resistance = DF45_LINE_RESISTANCE; + df45_model.back_emf_constant = DF45_BACK_EMF_CONSTANT; // setup the velocity PID // PidConstants_t vel_pid_constants; @@ -190,6 +198,10 @@ int main() { pid_initialize(&torque_pid, &torque_pid_constants); torque_pid_constants.kP = 1.0f; + torque_pid_constants.kI = 0.0f; + torque_pid_constants.kD = 0.0f; + torque_pid_constants.kI_max = DF45_MAX_CURRENT; + torque_pid_constants.kI_min = -DF45_MAX_CURRENT; // Turn off Red/Yellow LED after booting. turn_off_red_led(); @@ -202,6 +214,8 @@ int main() { uart_logging_status_tx_t uart_logging_status_send; #endif + uint32_t green_led_ticker = 0; + // toggle J1-1 while (true) { IWDG->KR = 0x0000AAAA; // feed the watchdog @@ -251,6 +265,8 @@ int main() { } telemetry_enabled = motor_command_packet.data.motion.enable_telemetry; + motion_enabled = motor_command_packet.data.motion.enable_motion; + calibrate_current = motor_command_packet.data.motion.calibrate_current; r_motor_board = motor_command_packet.data.motion.setpoint; motion_control_type = motor_command_packet.data.motion.motion_control_type; } else if (motor_command_packet.type == MCP_PARAMS) { @@ -307,11 +323,12 @@ int main() { #endif // Coast motor based on failure states. - if (!telemetry_enabled || + if (!motion_enabled || + !telemetry_enabled || ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS || response_packet.data.motion.master_error || uart_logging_status_receive != UART_LOGGING_OK) { - // If telemetry is disabled, that means the upstream + // If telemetry or motion is disabled, that means the upstream // isn't ready. // If we haven't received a command packet in a while, // that means the upstream isn't ready or locked up. @@ -328,63 +345,89 @@ int main() { // run the torque loop if applicable 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); - // map current to torque using the motor model + // Get the current in amps from the current sense ADC. + // Should always be positive with the current electrical architecture. + float current_sense_I = 0.0f; + if (calibrate_current) { + // Just return the current with no offset applied. + current_sense_I = currsen_get_motor_current(); + } + else { + current_sense_I = currsen_get_motor_current_with_offset(); + } + + float vbus_voltage = currsen_get_vbus_voltage(); + // Map current to torque using the motor model float measured_torque_Nm = mm_current_to_torque(&df45_model, current_sense_I); - // filter torque + // Filter torque measured_torque_Nm = iir_filter_update(&torque_filter, measured_torque_Nm); - // TODO: add filter? - - // correct torque sign from recovered velocity - // TODO: this should probably be acceleration based not velocity - // e.g. F = ma - if (enc_vel_rads < 0.0f) { - measured_torque_Nm = -fabs(measured_torque_Nm); + // Correct torque sign based on previous duty cycle direction. + MotorDirection_t current_motor_direction = pwm6step_get_motor_direction(); + if (current_motor_direction == COUNTER_CLOCKWISE) { + // if the motor is spinning counter-clockwise, then the torque is negative. + measured_torque_Nm = -measured_torque_Nm; } // choose setpoint - float r_Nm; + float r_Nm = 0.0f; if (motion_control_type == TORQUE) { r_Nm = r_motor_board; - } else { - r_Nm = control_setpoint_vel_duty; + } + else if (motion_control_type == OPEN_LOOP || + motion_control_type == VELOCITY_W_TORQUE || + motion_control_type == VELOCITY) { + // Input from motor board is in rad/s + // This assumes that the controller will keep + // velocity at the setpoint if we hit acceleration = 0. + // Inspired by SKUBA https://arxiv.org/pdf/1708.02271 + r_Nm = vel_target_accel_rads2; } - // calculate PID on the torque in Nm - float torque_setpoint_Nm = pid_calculate(&torque_pid, r_Nm, measured_torque_Nm, TORQUE_LOOP_RATE_S); + // Calculate the torque setpoint in Nm + float torque_setpoint_Nm = 0.0f; - // convert desired torque to desired current - float current_setpoint = mm_torque_to_current(&df45_model, fabs(torque_setpoint_Nm)); - // convert desired current to desired duty cycle - u_torque_loop = mm_pos_current_to_pos_dc(&df45_model, current_setpoint); + // TODO remove OPEN_LOOP case after testing + if (motion_control_type == OPEN_LOOP || + motion_control_type == TORQUE || + motion_control_type == VELOCITY_W_TORQUE) { - if (torque_setpoint_Nm < 0.0f) { - u_torque_loop = -fabs(u_torque_loop); + torque_setpoint_Nm = pid_calculate(&torque_pid, r_Nm, measured_torque_Nm, TORQUE_LOOP_RATE_S); + + // Convert desired torque to desired current + float current_setpoint = mm_torque_to_current(&df45_model, fabs(torque_setpoint_Nm)); + // Clamp the current setpoint to zero and the maximum current. + current_setpoint = fmax(fmin(current_setpoint, 0.0f), DF45_MAX_CURRENT); + + // Convert desired current to desired duty cycle. + u_torque_loop = mm_pos_current_to_pos_dc(&df45_model, current_setpoint, vbus_voltage, enc_vel_rads_filt); + // Match the sign of the torque setpoint to the duty cycle. + u_torque_loop = copysignf(u_torque_loop, torque_setpoint_Nm); } // load data frame // torque control data response_packet.data.motion.torque_setpoint = r_Nm; + // TODO Temp undo + response_packet.data.motion.vbus_voltage = currsen_get_motor_current_offset(); 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; + response_packet.data.motion.torque_computed_duty = u_torque_loop; } // run velocity loop if applicable if (run_vel_loop) { int32_t enc_delta = quadenc_get_encoder_delta(); float rads_delta = mm_enc_ticks_to_rad(&df45_model, enc_delta); - // float enc_vel_rad_s = quadenc_delta_to_w(enc_delta, VELOCITY_LOOP_RATE_S); enc_vel_rads = discrete_time_derivative(rads_delta, VELOCITY_LOOP_RATE_S); // filter the recovered velocity - enc_rad_s_filt = iir_filter_update(&encoder_filter, enc_vel_rads); + enc_vel_rads_filt = iir_filter_update(&encoder_filter, enc_vel_rads); + + // Vel_desired - Vel_measured + vel_target_accel_rads2 = (r_motor_board - enc_vel_rads_filt) / VELOCITY_LOOP_RATE_S; // reset integrator when commanded to stop if(fabsf(r_motor_board) < 1e-3f) { @@ -392,7 +435,7 @@ int main() { } // compute the velocity PID - control_setpoint_vel_rads = gspid_calculate(&vel_pid, r_motor_board, enc_rad_s_filt, VELOCITY_LOOP_RATE_S); + control_setpoint_vel_rads = gspid_calculate(&vel_pid, r_motor_board, enc_vel_rads_filt, VELOCITY_LOOP_RATE_S); // Clamp setpoint acceleration float setpoint_accel_rads_2 = (control_setpoint_vel_rads - control_setpoint_vel_rads_prev)/VELOCITY_LOOP_RATE_S; @@ -407,18 +450,17 @@ int main() { // back convert rads to duty cycle if(r_motor_board == 0.0f) { - control_setpoint_vel_duty = 0.0f; + vel_computed_duty = 0.0f; } else { - control_setpoint_vel_duty = mm_rads_to_dc(&df45_model, control_setpoint_vel_rads); + vel_computed_duty = mm_rads_to_dc(&df45_model, control_setpoint_vel_rads); } // 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.encoder_delta = enc_delta; - response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt; + response_packet.data.motion.vel_enc_estimate = enc_vel_rads_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_rads = control_setpoint_vel_rads; + response_packet.data.motion.vel_computed_duty = vel_computed_duty; } if (run_torque_loop || run_vel_loop) { @@ -430,10 +472,10 @@ 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_computed_duty = r_motor; pwm6step_set_duty_cycle_f(r_motor); } else if (motion_control_type == VELOCITY) { - pwm6step_set_duty_cycle_f(control_setpoint_vel_duty); + pwm6step_set_duty_cycle_f(vel_computed_duty); } else { pwm6step_set_duty_cycle_f(u_torque_loop); } @@ -444,6 +486,7 @@ int main() { const MotorErrors_t reported_motor_errors = pwm6step_get_motor_errors(); response_packet.type = MRP_MOTION; + response_packet.timestamp = time_get_uptime_ms(); // bldc errors response_packet.data.motion.hall_power_error = reported_motor_errors.hall_power; @@ -517,7 +560,6 @@ 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(); // TODO parameter updates are off for gain scheduled PID // response_packet.data.params.vel_p = vel_pid_constants.kP; @@ -562,7 +604,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)) { + (motion_enabled && ticks_since_last_command_packet > COMMAND_PACKET_TIMEOUT_TICKS)) { turn_on_red_led(); } @@ -578,13 +620,21 @@ int main() { // Green LED means we are able to send telemetry upstream. // This means the upstream sent a packet downstream with telemetry enabled. if (!telemetry_enabled) { - turn_off_green_led(); - } else { turn_on_green_led(); + } else { + if (green_led_ticker > 2000) { + green_led_ticker = 0; + } else if (green_led_ticker > 1000) { + turn_off_green_led(); + } else { + turn_on_green_led(); + } + + green_led_ticker++; } #ifdef COMP_MODE - if (telemetry_enabled && + if (motion_enabled && 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 @@ -596,4 +646,4 @@ int main() { } #endif } -} +} \ No newline at end of file diff --git a/motor-controller/bin/wheel/main.h b/motor-controller/bin/wheel/main.h index 63baec1e..b25263cf 100644 --- a/motor-controller/bin/wheel/main.h +++ b/motor-controller/bin/wheel/main.h @@ -1,12 +1,12 @@ /** * @file main.h * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-07-10 - * + * * @copyright Copyright (c) 2022 - * + * */ #include @@ -21,9 +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 /////////////////////////////////////////// // current sense amplification network // @@ -42,12 +39,16 @@ #define DF45_MAX_MOTOR_ROT_PER_S ((float) DF45_MAX_MOTOR_ROT_PER_M / 60.0f) #define DF45_MAX_MOTOR_RAD_PER_S (DF45_MAX_MOTOR_ROT_PER_S * 2.0f * (float) M_PI) #define DF45_RATED_CURRENT 2.36f +// These are based on https://www.nanotec.com/us/en/products/1789-df45m024053-a2 #define DF45_TORQUE_TO_CURRENT_LINEAR_M 28.690f #define DF45_TORQUE_TO_CURRENT_LINEAR_B (-0.0558f) #define DF45_CURRENT_TO_TORQUE_LINEAR_M 0.03477f #define DF45_CURRENT_TO_TORQUE_LINEAR_B 0.00242f #define DF45_RADS_TO_DC_LINEAR_M 0.00144873f #define DF45_RADS_TO_DC_LINEAR_B 0.0057431f +#define DF45_LINE_RESISTANCE 0.8f // 0.8 ohm is line to line resistance +#define DF45_BACK_EMF_CONSTANT 0.0335f // V*s/rad, based on https://www.nanotec.com/us/en/products/1789-df45m024053-a2 +#define DF45_MAX_CURRENT 4.0f // A. This is around the middle of rated current and peak current for the motor. //////////////////////// // FILTERING/TUNING // diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h index 4c6c1c1c..683cf4f2 100644 --- a/motor-controller/bin/wheel/system.h +++ b/motor-controller/bin/wheel/system.h @@ -18,17 +18,15 @@ /////////////////// #define UART_ENABLED -#define IOQ_BUF_LENGTH 64 +#define IOQ_BUF_LENGTH 80 #define IOQ_BUF_DEPTH 4 ////////////////// // ADC Config // ////////////////// -#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) +// ADC Motor Current Unfiltered (Ch 3), Motor Current Filtered (Ch 4) +#define ADC_CH_MASK (ADC_CHSELR_CHSEL3) //////////////////// // TIME KEEPING // diff --git a/motor-controller/common/6step.c b/motor-controller/common/6step.c index 98d4ee6c..86c1b551 100644 --- a/motor-controller/common/6step.c +++ b/motor-controller/common/6step.c @@ -4,16 +4,16 @@ * @brief core code for 6step commutation of the bldc * @version 0.1 * @date 2022-05-22 - * + * * @copyright Copyright (c) 2022 - * + * * The most readable file ever! Jk. Its not too bad but you'll definitely * need stm32f031c6 and rm0091 in hand (or memorized LOL) before continuing. - * + * * Documents you will need: * Understading of 6step PWM commutation: * https://www.youtube.com/user/jtlee1108 - * Motor Phase/Hall Relationship: + * Motor Phase/Hall Relationship: * https://us.nanotec.com/fileadmin/files/Datenblaetter/BLDC/DF45/DF45M024053-A2.pdf * Schematic relating header to the pins: * STEVAL-SPIN3204 Data Brief @@ -32,11 +32,6 @@ #include "system.h" #include "time.h" -typedef enum MotorDirection { - CLOCKWISE, - COUNTER_CLOCKWISE -} MotorDirection_t; - typedef enum CommutationValuesType { NORMAL, MOMENTUM_DETECTED, @@ -102,7 +97,7 @@ static int hall_transition_error_count = 0; #endif #ifdef BRAKE_ON_HALL_ERROR - #define HALL_ERROR_COMMUTATION BRAKE_COMMUTATION + #define HALL_ERROR_COMMUTATION BRAKE_COMMUTATION #else #define HALL_ERROR_COMMUTATION COAST_COMMUTATION #endif @@ -113,21 +108,21 @@ static const int COAST_COMMUTATION_INDEX = 9; /** * @brief clockwise transition table - * + * * Hall sensors should produce a gray-coded 3-bit value, meaning * 0 (0'b000) and 7 (0'b111) are invalid. The signal wires have * pull up resistors so 7 probably means one or more wires is unplugged, * and 0 probably means there's a power issue. - * + * * Clockwise (human readable order) - * D H3 H2 H1 -> P1 P2 P3 -> W1L W1H W2L W2H W3L W3H + * D H3 H2 H1 -> P1 P2 P3 -> W1L W1H W2L W2H W3L W3H * 1 0 0 1 V H G 0 1 0 0 1 0 * 3 0 1 1 H V G 0 0 0 1 1 0 * 2 0 1 0 G V H 1 0 0 1 0 0 * 6 1 1 0 G H V 1 0 0 0 0 1 * 4 1 0 0 H G V 0 0 1 0 0 1 * 5 1 0 1 V G H 0 1 1 0 0 0 - * + * * Clockwise (direct hall index order) * 1 0 0 1 V H G 0 1 0 0 1 0 * 2 0 1 0 G V H 1 0 0 1 0 0 @@ -135,10 +130,10 @@ static const int COAST_COMMUTATION_INDEX = 9; * 4 1 0 0 H G V 0 0 1 0 0 1 * 5 1 0 1 V G H 0 1 1 0 0 0 * 6 1 1 0 G H V 1 0 0 0 0 1 - * + * */ static bool cw_commutation_table[10][6] = { - HALL_ERROR_COMMUTATION, + HALL_ERROR_COMMUTATION, {false, true, false, false, true, false}, {true, false, false, true, false, false}, {false, false, false, true, true, false}, @@ -163,12 +158,12 @@ static uint8_t cw_expected_hall_transition_table[8] = { /** * @brief counter clockwise transition table - * + * * Hall sensors should produce a gray-coded 3-bit value, meaning * 0 (0'b000) and 7 (0'b111) are invalid. The signal wires have * pull up resistors so 7 probably means one or more wires is unplugged, * and 0 probably means there's a power issue. - * + * * Counter Clockwise (human readable order) * D H3 H2 H1 -> P1 P2 P3 -> W1L W1H W2L W2H W3L W3H * 1 0 0 1 V H G 1 0 0 0 0 1 @@ -177,7 +172,7 @@ static uint8_t cw_expected_hall_transition_table[8] = { * 6 1 1 0 G H V 0 1 0 0 1 0 * 2 0 1 0 G V H 0 1 1 0 0 0 * 3 0 1 1 H V G 0 0 1 0 0 1 - * + * * Counter Clockwise (direct hall index order) * 1 0 0 1 V H G 1 0 0 0 0 1 * 2 0 1 0 G V H 0 1 1 0 0 0 @@ -229,7 +224,7 @@ static void pwm6step_set_direct(uint16_t, MotorDirection_t); /** * @brief sets up the hall sensor timer - * + * * The hall sensor timer is TIM2. The hall timer can be used to * trigger a TIM1 COM event which will call back it's interrupt handler */ @@ -254,7 +249,7 @@ 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 = 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 @@ -285,7 +280,7 @@ static void pwm6step_setup_hall_timer() { // 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 + // enable master slave mode TIM2->SMCR &= ~TIM_SMCR_MSM; TIM2->SMCR |= (TIM_SMCR_MSM); @@ -385,20 +380,20 @@ static void pwm6step_setup_hall_timer() { #define CCER_TIM4_ADC_TRIG (TIM_CCER_CC4E) /** - * @brief - * - * @param pwm_freq_hz + * @brief + * + * @param pwm_freq_hz */ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) { - //////////////////////////// - // TIM1 Setup IO // - // - PA8 TIM1_CH1 // - // - PA9 TIM1_CH2 // - // - PA10 TIM1_CH3 // - // - PB13 TIM1_CH1N // - // - PB14 TIM1_CH2N // - // - PB15 TIM1-CH3N // - //////////////////////////// + //////////////////////////////// + // TIM1 Setup IO // + // - PA8 TIM1_CH1 HS1 // + // - PA9 TIM1_CH2 HS2 // + // - PA10 TIM1_CH3 HS3 // + // - PB13 TIM1_CH1N LS1 // + // - PB14 TIM1_CH2N LS2 // + // - PB15 TIM1-CH3N LS3 // + //////////////////////////////// // set OC_SEL = 0, OC signal visible only to MCU, no action on gate driver GPIOA->MODER |= (GPIO_MODER_MODER11_0); // output (def push pull) @@ -414,6 +409,7 @@ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) { GPIOA->MODER |= (GPIO_MODER_MODER8_1 | GPIO_MODER_MODER9_1 | GPIO_MODER_MODER10_1); GPIOB->MODER |= (GPIO_MODER_MODER13_1 | GPIO_MODER_MODER14_1 | GPIO_MODER_MODER15_1); + // Pull downs for all pins. GPIOA->PUPDR |= (GPIO_PUPDR_PUPDR8_1 | GPIO_PUPDR_PUPDR9_1 | GPIO_PUPDR_PUPDR10_1); GPIOB->PUPDR |= (GPIO_PUPDR_PUPDR13_1 | GPIO_PUPDR_PUPDR14_1 | GPIO_PUPDR_PUPDR15_1); @@ -436,12 +432,20 @@ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) { 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; + // Set the Repetition Counter to 0 + TIM1->RCR = 0; + // Preload ARR, Center-aligned mode 3 (up/down counting and output compare both up and down). TIM1->CR1 = (TIM_CR1_ARPE | TIM_CR1_CMS); + // CCxE, CCxNE and OCxM bits are preloaded, after having been written, they are updated + // only when a communication event (COM) occurs TIM1->CR2 = (TIM_CR2_CCPC); TIM1->CCMR1 = CCMR1_PHASE1_OFF | CCMR1_PHASE2_OFF; TIM1->CCMR2 = CCMR2_PHASE3_OFF; + // ADC trigger delay (TIM1->CCR4) will be set on each commutation to happen + // at the same period as the PWM. + TIM1->CCR4 = 20; // set channel 4 PWM mode 1, affects OC4REF as input to ADC // should be co triggered with ch1-3 commutation TIM1->CCMR2 |= CCMR2_TIM4_ADC_TRIG; @@ -451,6 +455,7 @@ 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); + // Counter Enable TIM1->CR1 |= TIM_CR1_CEN; TIM1->BDTR |= TIM_BDTR_MOE; @@ -497,18 +502,18 @@ void TIM16_IRQHandler() { } /** - * @brief - * + * @brief + * * keep this interrupt short so we can minimize the delay between the CC1 * transition event and delayed required on CC2 to fire the COM event automatically - * + * * handle most functionality on the interrupt callback for CC2 which fires *after* * the COM event - * + * * force apply O0 as we'll need to count the instructions here + ctxswitch to make * sure this function completes before the other interrupt fires. Maybe that happens * anyway since read of CCR1 clear the IF - * + * */ __attribute__((optimize("O0"))) static void TIM2_IRQHandler_HallTransition() { @@ -529,8 +534,8 @@ static void TIM2_IRQHandler_HallTransition() { } /** - * @brief - * + * @brief + * */ static void perform_commutation_cycle() { // mask off Hall lines PA0-PA2 (already the LSBs) @@ -616,16 +621,16 @@ static void perform_commutation_cycle() { trigger_commutation(); return; - } - + } + set_commutation_for_hall(hall_recorded_state_on_transition, false); trigger_commutation(); } /** * @brief reads the hall value from the pins - * - * @return uint8_t + * + * @return uint8_t */ static uint8_t read_hall() { uint8_t hall_value = (GPIOA->IDR & (GPIO_IDR_2 | GPIO_IDR_1 | GPIO_IDR_0)); @@ -642,7 +647,7 @@ static uint8_t read_hall() { /** * @brief stages com values for estop - * + * */ static void set_commutation_estop() { set_commutation_for_hall(0, true); @@ -650,8 +655,8 @@ static void set_commutation_estop() { /** * @brief sets channel duty cycles, and stages enables for COM event - * - * @param hall_state + * + * @param hall_state */ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { bool *commutation_values; @@ -677,15 +682,16 @@ 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); - 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; + // Set the ADC trigger value to the same as the PWM duty cycle + // so it's right TODO determine what it is right after. TIM1->CCR4 = (current_duty_cycle == NUM_RAW_DC_STEPS) ? NUM_RAW_DC_STEPS - MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW : current_duty_cycle; + + // Set the duty cycle and capture control configuration for each phase if (phase1_low) { if (command_brake) { TIM1->CCR1 = current_duty_cycle; @@ -710,7 +716,7 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { if (command_brake) { TIM1->CCR2 = current_duty_cycle; ccmr1 |= CCMR1_PHASE2_PWM_BRAKE; - ccer |= CCER_PHASE2_PWM_BRAKE; + ccer |= CCER_PHASE2_PWM_BRAKE; } else { TIM1->CCR2 = 0; ccmr1 |= CCMR1_PHASE2_LOW; @@ -753,7 +759,7 @@ static void set_commutation_for_hall(uint8_t hall_state, bool estop) { /** * @brief trigger a hardware commutation in TIM1 - * + * */ static void trigger_commutation() { TIM1->EGR |= TIM_EGR_COMG; @@ -769,15 +775,15 @@ void TIM1_CC_IRQHandler() { } /** - * @brief - * - * @param duty_cycle - * @param motor_direction + * @brief + * + * @param duty_cycle + * @param motor_direction */ static void pwm6step_set_direct(uint16_t duty_cycle, MotorDirection_t motor_direction) { uint16_t scaled_dc = MAP_UINT16_TO_RAW_DC(duty_cycle); - if (scaled_dc >= MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW - && motor_direction != commanded_motor_direction) { + if (scaled_dc >= MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW && + motor_direction != commanded_motor_direction) { direction_change_commanded = true; } @@ -795,8 +801,8 @@ static void pwm6step_set_direct(uint16_t duty_cycle, MotorDirection_t motor_dire //////////////////////// /** - * @brief sets up the pins and timer peripherials associated with the pins - * + * @brief sets up the pins and timer peripherials associated with the pins + * */ void pwm6step_setup() { @@ -806,9 +812,9 @@ void pwm6step_setup() { } /** - * @brief - * - * @param duty_cycle + * @brief + * + * @param duty_cycle */ void pwm6step_set_duty_cycle(int32_t duty_cycle) { MotorDirection_t motor_direction; @@ -847,7 +853,7 @@ void pwm6step_brake(uint16_t braking_force) { } void pwm6step_brake_f(float braking_force_pct) { - pwm6step_brake((uint16_t) (braking_force_pct * (float) UINT16_MAX)); + pwm6step_brake((uint16_t) (braking_force_pct * (float) UINT16_MAX)); } void pwm6step_stop() { @@ -869,7 +875,7 @@ bool pwm6step_is_direction_inverted() { const MotorErrors_t pwm6step_get_motor_errors() { return motor_errors; -} +} bool pwm6step_hall_rps_estimate_valid() { @@ -879,4 +885,6 @@ int pwm6step_hall_get_rps_estimate() { } - +MotorDirection_t pwm6step_get_motor_direction() { + return commanded_motor_direction; +} \ No newline at end of file diff --git a/motor-controller/common/6step.h b/motor-controller/common/6step.h index e32acce9..878785f7 100644 --- a/motor-controller/common/6step.h +++ b/motor-controller/common/6step.h @@ -1,12 +1,12 @@ /** * @file 6step.h * @author Will Stuckey - * @brief + * @brief * @version 0.1 * @date 2022-05-22 - * + * * @copyright Copyright (c) 2022 - * + * */ #pragma once @@ -38,6 +38,11 @@ #define HALL_TRANSITION_ERROR_THRESHOLD 3 +typedef enum MotorDirection { + CLOCKWISE, + COUNTER_CLOCKWISE +} MotorDirection_t; + typedef struct MotorErrors { bool hall_power; bool hall_disconnected; @@ -78,4 +83,5 @@ void pwm6step_invert_direction(bool invert); bool pwm6step_is_direction_inverted(); const MotorErrors_t pwm6step_get_motor_errors(); bool pwm6step_hall_rps_estimate_valid(); -int pwm6step_hall_get_rps_estimate(); \ No newline at end of file +int pwm6step_hall_get_rps_estimate(); +MotorDirection_t pwm6step_get_motor_direction(); \ No newline at end of file diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c index afbf0c22..44811226 100644 --- a/motor-controller/common/current_sensing.c +++ b/motor-controller/common/current_sensing.c @@ -1,12 +1,12 @@ /** * @file main.c * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-04-10 - * + * * @copyright Copyright (c) 2022 - * + * */ #include @@ -15,156 +15,187 @@ #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; + + // 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 +206,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_10B - | ADC_DATA_ALIGN_LEFT - | ADC_LP_MODE_NONE - ); - - // Set ADC clock - // PCLK DIV 2, Latency is deterministic (no jitter) and equal to - // 2.75 ADC clock cycles - MODIFY_REG(ADC1->CFGR2, - ADC_CFGR2_CKMODE - , - ADC_CFGR2_CKMODE_0 - ); - - 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 +245,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 +265,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 +302,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 +343,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 +359,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 0dc4ca3c..b17ef172 100644 --- a/motor-controller/common/current_sensing.h +++ b/motor-controller/common/current_sensing.h @@ -1,28 +1,76 @@ /** * @file setup.h * @author your name (you@domain.com) - * @brief + * @brief * @version 0.1 * @date 2022-04-10 - * + * * @copyright Copyright (c) 2022 - * + * */ #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)) @@ -82,31 +130,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_filt; - uint16_t I_motor; - 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(); diff --git a/motor-controller/common/motor_model.c b/motor-controller/common/motor_model.c index c3dd1ad8..29f11016 100644 --- a/motor-controller/common/motor_model.c +++ b/motor-controller/common/motor_model.c @@ -17,6 +17,8 @@ void mm_initialize(MotorModel_t *mm) { mm->current_to_torque_linear_model_b = 0.0f; mm->rads_to_dc_linear_map_m = 0.0f; mm->rads_to_dc_linear_map_b = 0.0f; + mm->line_resistance = 0.0f; + mm->back_emf_constant = 0.0f; } float mm_rads_to_dc(MotorModel_t *mm, float avel_rads) { @@ -49,13 +51,33 @@ float mm_voltage_to_current(MotorModel_t *mm, float voltage) { } float mm_current_to_torque(MotorModel_t *mm, float current) { + if (fabs(current) < fabs(mm->current_to_torque_linear_model_b)) { + // Prevent sign flipping at very low values. + return 0.0f; + } + return fmax(mm->current_to_torque_linear_model_m * current + mm->current_to_torque_linear_model_b, 0.0f); } float mm_torque_to_current(MotorModel_t *mm, float torque) { + if (fabs(torque) < fabs(mm->torque_to_current_linear_model_b)) { + // Prevent sign flipping at very low values. + return 0.0f; + } + return fmax(mm->torque_to_current_linear_model_m * torque + mm->torque_to_current_linear_model_b, 0.0f); } -float mm_pos_current_to_pos_dc(MotorModel_t *mm, float current) { - return fmax(fmin(current / mm->rated_current, 1.0f), 0.0f); +float mm_pos_current_to_pos_dc(MotorModel_t *mm, float current, float vbus_voltage, float vel_rads) { + // V_required = I_motor * R_motor + L * dI/dt + V_bemf + // We think L * dI/dt is negligible for our purposes, so we can ignore it (something something steady state current). + // V_bemf = K_e * vel_rads + float v_required = (current * mm->line_resistance) + (mm->back_emf_constant * vel_rads); + if (vbus_voltage == 0.0f) { + return 0.0f; // avoid division by zero + } + + // bound DC [0, 1] + // duty_cycle = V_required / V_bus + return fmax(fmin(v_required / vbus_voltage, 1.0f), 0.0f); } \ No newline at end of file diff --git a/motor-controller/common/motor_model.h b/motor-controller/common/motor_model.h index 90bf66bf..b7323b8c 100644 --- a/motor-controller/common/motor_model.h +++ b/motor-controller/common/motor_model.h @@ -13,6 +13,8 @@ typedef struct MotorModel { float current_to_torque_linear_model_b; float rads_to_dc_linear_map_m; float rads_to_dc_linear_map_b; + float line_resistance; // ohms + float back_emf_constant; // V*s/rad } MotorModel_t; void mm_initialize(MotorModel_t *mm); @@ -22,4 +24,4 @@ float mm_enc_ticks_to_rad(MotorModel_t *mm, float enc_ticks); float mm_voltage_to_current(MotorModel_t *mm, float voltage); float mm_current_to_torque(MotorModel_t *mm, float current); float mm_torque_to_current(MotorModel_t *mm, float torque); -float mm_pos_current_to_pos_dc(MotorModel_t *mm, float current); \ No newline at end of file +float mm_pos_current_to_pos_dc(MotorModel_t *mm, float current, float vbus_voltage, float vel_rads); \ No newline at end of file diff --git a/motor-controller/common/pid.c b/motor-controller/common/pid.c index 43056c26..5d92ca59 100644 --- a/motor-controller/common/pid.c +++ b/motor-controller/common/pid.c @@ -38,7 +38,7 @@ float pid_calculate(Pid_t *pid, float r, float y, float dt) { } float termI = pid->eI * pid->pid_constants->kI; - float termD = ((err - pid->prev_err) / dt) * pid->pid_constants->kD; // flip err and prev_err??? + float termD = ((err - pid->prev_err) / dt) * pid->pid_constants->kD; pid->prev_err = err; float u = r + (termP + termI + termD); diff --git a/motor-controller/common/stm32f031xx.ld b/motor-controller/common/stm32f031xx.ld index 0b4a15a1..9528dbdf 100644 --- a/motor-controller/common/stm32f031xx.ld +++ b/motor-controller/common/stm32f031xx.ld @@ -11,7 +11,8 @@ _Min_Stack_Size = 0xC00 ; /* required amount of stack */ MEMORY { RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 4K - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 31K + /* Total flash is 32K but last 1K is used for per board constants. */ } /* Sections */ 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