From 9bd02643bff82e9cbef6949b5f9a068344291892 Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Thu, 6 Mar 2025 17:33:00 -0800
Subject: [PATCH 01/20] Initial current sensing reworking

---
 motor-controller/bin/dribbler/setup.c     |  17 +-
 motor-controller/bin/wheel/main.c         |   2 +-
 motor-controller/bin/wheel/system.h       |   4 +-
 motor-controller/common/current_sensing.c | 322 +++++++++++++---------
 motor-controller/common/current_sensing.h |  38 +--
 5 files changed, 220 insertions(+), 163 deletions(-)

diff --git a/motor-controller/bin/dribbler/setup.c b/motor-controller/bin/dribbler/setup.c
index 42acf1e3..18b9f37d 100644
--- a/motor-controller/bin/dribbler/setup.c
+++ b/motor-controller/bin/dribbler/setup.c
@@ -78,13 +78,14 @@ inline void setup_clocks() {
         asm volatile("nop");
     }
 
-    // Set AHB clock freq to sysclk (48MHz)
-    // HPRE -> 1
-    RCC->CFGR |= RCC_CFGR_HPRE_0;
+    // Set AHB clock (HCLK) freq to sysclk (48 MHz)
+    // HPRE 0xxx -> SYSCLK not divided.
+    RCC->CFGR &= ~RCC_CFGR_HPRE_3;
+
+    // Set APB clock (PCLK) freq to AHB clk (48 MHz)
+    // PPRE 0xxx -> HCLK not divided.
+    RCC->CFGR &= ~RCC_CFGR_PPRE_3;
 
-    // Set APB clock freq to AHB clk (48MHz)
-    // PPRE div -> 1
-    RCC->CFGR |= RCC_CFGR_PPRE_0;
     // PPRE mul -> 1
     // ??? dont see this cfg
     // USART1SW src -> PCLK
@@ -101,8 +102,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/wheel/main.c b/motor-controller/bin/wheel/main.c
index 098cafeb..d6fc4c40 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -63,7 +63,7 @@ int main() {
 
     // 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_MODE, &res, ADC_NUM_CHANNELS, ADC_CH_MASK, ADC_SR_MASK);
 
     // initialize motor driver
     pwm6step_setup();
diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h
index bc8584ed..4400efca 100644
--- a/motor-controller/bin/wheel/system.h
+++ b/motor-controller/bin/wheel/system.h
@@ -27,8 +27,10 @@
 
 #define ADC_MODE CS_MODE_DMA
 #define ADC_NUM_CHANNELS 4
+// ADC Motor Current Filtered (Ch 3), Motor Current Unfiltered (Ch 4), Temperature Sensor (Ch 16), Vrefint (Ch 17)
 #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)
+// 110 = 71.5 ADC clock cycles for t_SMPR
+#define ADC_SR_MASK (ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1)
 
 ////////////////////
 //  TIME KEEPING  //
diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c
index afbf0c22..061449b4 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 <stm32f031x6.h>
@@ -20,25 +20,24 @@
 /// ADC Channel 4 -> PA4 ///
 ////////////////////////////
 
-static CS_Mode_t cs_mode;
+static CS_Mode_t m_cs_mode;
+static ADC_Result_t m_adc_result;
 
 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)
@@ -51,7 +50,6 @@ void currsen_read(ADC_Result_t *res)
     res->status = CS_OK;
     while (1)
     {
-
         for (int i = 0; i < NUM_CHANNELS; i++)
         {
             // Start ADC conversion
@@ -93,78 +91,160 @@ void currsen_read(ADC_Result_t *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(CS_Mode_t mode, uint8_t motor_adc_ch)
 {
-    cs_mode = mode;
+    m_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) {
+    if (m_cs_mode == CS_MODE_DMA || m_cs_mode == CS_MODE_TIMER_DMA) {
+        // 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;
+        // 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) 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);
+        // Set DMA Channel 1 Number of Data to Transfer to the number of channels.
+        // Temperature and motor so 2.
+        // Since in circular mode, this will reset.
+        DMA1_Channel1->CNDTR = 2;
+        // Enable DMA1 Channel 1.
+        // TODO: Need to find DMA Enable in ST source.
         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();
+    // TODO Add ADC watchdog?
+
+    // 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_LEFT);
+
+    // 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 fall of the PWM is closer to center
+    // than the rising edge. Probably doesn't actually matter because
+    // we aren't making 48 kHz control adjustments (today).
+    // 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);
+
+    // 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).
+    // Will go with SMP = 110 (71.5 ADC clock cycles) for now as motor should be low pass filtered.
+    // 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.
+
+    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;
 
     // Calibration
     status = currsen_adc_cal();
     if (status != CS_OK)
         return status;
 
-
     // 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);
@@ -180,83 +260,9 @@ void ADC1_IRQHandler() {
     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;
-}
-
 /**
  * @brief calibrate the ADC
- * 
+ *
  * @return CS_Status_t status of the operation
  */
 CS_Status_t currsen_adc_cal()
@@ -284,9 +290,9 @@ CS_Status_t currsen_adc_cal()
     }
 
     // Clear DMAEN
-    ADC1->CFGR1 &= ~ADC_CFGR1_DMAEN; 
-    // Launch the calibration by setting ADCAL 
-    ADC1->CR |= ADC_CR_ADCAL; 
+    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;
@@ -309,15 +315,15 @@ CS_Status_t currsen_adc_cal()
 
 /**
  * @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;
     }
 
@@ -326,7 +332,7 @@ CS_Status_t currsen_adc_en()
     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)
@@ -346,16 +352,24 @@ CS_Status_t currsen_adc_en()
 }
 
 /**
- * @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 +386,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 +402,38 @@ CS_Status_t currsen_adc_dis()
     }
 
     return CS_OK;
-}
\ No newline at end of file
+}
+
+/**
+ * @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()
+{
+    // TODO need more for motor current scaling
+    return V_ADC_SCALE * m_adc_result.Motor_current_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_temp()
+{
+    // From A.7.16 of RM0091
+    int32_t temperature; /* will contain the temperature in degrees Celsius */
+    // 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;
+}
diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h
index 0dc4ca3c..7d5ac21b 100644
--- a/motor-controller/common/current_sensing.h
+++ b/motor-controller/common/current_sensing.h
@@ -1,23 +1,33 @@
 /**
  * @file setup.h
  * @author your name (you@domain.com)
- * @brief 
+ * @brief
  * @version 0.1
  * @date 2022-04-10
- * 
+ *
  * @copyright Copyright (c) 2022
- * 
+ *
  */
 
 #pragma once
 
+#define V_DDA 3300 // mV
+#define V_ADC_SCALE V_DDA / 4095
+
+// 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/10))
+
 typedef enum {
   CS_MODE_POLLING,
   CS_MODE_DMA,
   CS_MODE_TIMER_DMA
 } CS_Mode_t;
 
-typedef enum 
+typedef enum
 {
   CS_OK       = 0x00U,
   CS_ERROR    = 0x01U,
@@ -83,30 +93,26 @@ typedef enum
 #define ADC_STP_TIMEOUT 5 //ms
 
 
-// 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    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(CS_Mode_t mode, uint8_t motor_adc_ch);
 CS_Status_t currsen_adc_cal();
 CS_Status_t currsen_adc_en();
 CS_Status_t currsen_adc_dis();
 
-
+float currsen_get_motor_current();
+float currsen_get_temp();

From c0c9c7bb1169f8a21104a455ecb0582b6cea11fc Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Sun, 9 Mar 2025 19:48:56 -0700
Subject: [PATCH 02/20] WIP gets building and testable

---
 motor-controller/bin/dribbler/main.c      |  5 ++---
 motor-controller/bin/dribbler/main.h      |  8 +++-----
 motor-controller/bin/dribbler/setup.c     |  4 ++--
 motor-controller/bin/dribbler/system.h    |  2 +-
 motor-controller/bin/wheel/main.c         |  6 +++---
 motor-controller/bin/wheel/main.h         |  8 +++-----
 motor-controller/bin/wheel/setup.c        | 13 +++++++------
 motor-controller/bin/wheel/system.h       |  2 +-
 motor-controller/common/current_sensing.c | 11 ++++++-----
 motor-controller/common/current_sensing.h |  4 ++--
 10 files changed, 30 insertions(+), 33 deletions(-)

diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c
index 28ac9a52..e788985d 100644
--- a/motor-controller/bin/dribbler/main.c
+++ b/motor-controller/bin/dribbler/main.c
@@ -52,8 +52,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_MODE, ADC_CH_MASK);
 
     // initialize motor driver
     pwm6step_setup();
@@ -202,7 +201,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?
diff --git a/motor-controller/bin/dribbler/main.h b/motor-controller/bin/dribbler/main.h
index 2e1149ac..e3ec1128 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
@@ -17,8 +17,6 @@
 
 #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 18b9f37d..d8be07f4 100644
--- a/motor-controller/bin/dribbler/setup.c
+++ b/motor-controller/bin/dribbler/setup.c
@@ -83,8 +83,8 @@ inline void setup_clocks() {
     RCC->CFGR &= ~RCC_CFGR_HPRE_3;
 
     // Set APB clock (PCLK) freq to AHB clk (48 MHz)
-    // PPRE 0xxx -> HCLK not divided.
-    RCC->CFGR &= ~RCC_CFGR_PPRE_3;
+    // PPRE 0xx -> HCLK not divided.
+    RCC->CFGR &= ~RCC_CFGR_PPRE_2;
 
     // PPRE mul -> 1
     // ??? dont see this cfg
diff --git a/motor-controller/bin/dribbler/system.h b/motor-controller/bin/dribbler/system.h
index d0427c03..53008618 100644
--- a/motor-controller/bin/dribbler/system.h
+++ b/motor-controller/bin/dribbler/system.h
@@ -30,7 +30,7 @@
 
 #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 d6fc4c40..50b4e144 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -62,8 +62,7 @@ int main() {
     bool telemetry_enabled = false;
 
     // initialize current sensing setup
-    ADC_Result_t res;
-    CS_Status_t cs_status = currsen_setup(ADC_MODE, &res, ADC_NUM_CHANNELS, ADC_CH_MASK, ADC_SR_MASK);
+    CS_Status_t cs_status = currsen_setup(ADC_MODE, ADC_CH_MASK);
 
     // initialize motor driver
     pwm6step_setup();
@@ -288,7 +287,7 @@ int main() {
         if (run_torque_loop) {
             // recover torque using the shunt voltage drop, amplification network model and motor model
             // pct of voltage range 0-3.3V
-            float current_sense_shunt_v = ((float) res.I_motor_filt / (float) UINT16_MAX) * AVDD_V;
+            float current_sense_shunt_v = currsen_get_motor_current();
             // 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
@@ -332,6 +331,7 @@ int main() {
             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.temperature = currsen_get_temperature();
         }
 
         // run velocity loop if applicable
diff --git a/motor-controller/bin/wheel/main.h b/motor-controller/bin/wheel/main.h
index c89e19cf..928dc238 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 <math.h>
@@ -23,8 +23,6 @@
 #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/wheel/setup.c b/motor-controller/bin/wheel/setup.c
index f91717fe..9238f54c 100644
--- a/motor-controller/bin/wheel/setup.c
+++ b/motor-controller/bin/wheel/setup.c
@@ -78,13 +78,14 @@ inline void setup_clocks() {
         asm volatile("nop");
     }
 
-    // Set AHB clock freq to sysclk (48MHz)
-    // HPRE -> 1
-    RCC->CFGR |= RCC_CFGR_HPRE_0;
+    // Set AHB clock (HCLK) freq to sysclk (48 MHz)
+    // HPRE 0xxx -> SYSCLK not divided.
+    RCC->CFGR &= ~RCC_CFGR_HPRE_3;
+
+    // Set APB clock (PCLK) freq to AHB clk (48 MHz)
+    // PPRE 0xx -> HCLK not divided.
+    RCC->CFGR &= ~RCC_CFGR_PPRE_2;
 
-    // Set APB clock freq to AHB clk (48MHz)
-    // PPRE div -> 1
-    RCC->CFGR |= RCC_CFGR_PPRE_0;
     // PPRE mul -> 1
     // ??? dont see this cfg
     // USART1SW src -> PCLK
diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h
index 4400efca..1b98af25 100644
--- a/motor-controller/bin/wheel/system.h
+++ b/motor-controller/bin/wheel/system.h
@@ -28,7 +28,7 @@
 #define ADC_MODE CS_MODE_DMA
 #define ADC_NUM_CHANNELS 4
 // ADC Motor Current Filtered (Ch 3), Motor Current Unfiltered (Ch 4), Temperature Sensor (Ch 16), Vrefint (Ch 17)
-#define ADC_CH_MASK (ADC_CHSELR_CHSEL3 | ADC_CHSELR_CHSEL4 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17)
+#define ADC_CH_MASK (ADC_CHSELR_CHSEL3)
 // 110 = 71.5 ADC clock cycles for t_SMPR
 #define ADC_SR_MASK (ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1)
 
diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c
index 061449b4..37eaae71 100644
--- a/motor-controller/common/current_sensing.c
+++ b/motor-controller/common/current_sensing.c
@@ -40,7 +40,7 @@ void DMA1_Channel1_IRQHandler() {
  *
  * @return ADC_Result result of the ADC operation
  */
-void currsen_read(ADC_Result_t *res)
+/*void currsen_read(ADC_Result_t *res)
 {
     const int NUM_CHANNELS = 4;
 
@@ -92,6 +92,7 @@ void currsen_read(ADC_Result_t *res)
         }
     }
 }
+*/
 
 /**
  * @brief configure, calibrate, and enable the ADC
@@ -102,7 +103,7 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
 {
     m_cs_mode = mode;
 
-    memset(m_adc_result, 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;
@@ -131,7 +132,7 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
         // 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) res;
+        DMA1_Channel1->CMAR = (uint32_t) (&m_adc_result);
         // Set DMA Channel 1 Number of Data to Transfer to the number of channels.
         // Temperature and motor so 2.
         // Since in circular mode, this will reset.
@@ -329,7 +330,7 @@ CS_Status_t currsen_adc_en()
 
     // Enable the ADC
     ADC1->CR |= ADC_CR_ADEN;
-    if (cs_mode == CS_MODE_DMA || cs_mode == CS_MODE_TIMER_DMA) {
+    if (m_cs_mode == CS_MODE_DMA || m_cs_mode == CS_MODE_TIMER_DMA) {
         ADC1->CFGR1 |= ADC_CFGR1_DMAEN;
     }
 
@@ -423,7 +424,7 @@ float currsen_get_motor_current()
  * @return int32_t temperature in C
  */
 
-int32_t currsen_get_temp()
+int32_t currsen_get_temperature()
 {
     // From A.7.16 of RM0091
     int32_t temperature; /* will contain the temperature in degrees Celsius */
diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h
index 7d5ac21b..381befee 100644
--- a/motor-controller/common/current_sensing.h
+++ b/motor-controller/common/current_sensing.h
@@ -12,7 +12,7 @@
 #pragma once
 
 #define V_DDA 3300 // mV
-#define V_ADC_SCALE V_DDA / 4095
+#define V_ADC_SCALE V_DDA / 4095.0f
 
 // From A.7.16 of RM0091
 #define TEMP110_CAL_ADDR ((uint16_t*) ((uint32_t) 0x1FFFF7C2))
@@ -115,4 +115,4 @@ CS_Status_t currsen_adc_en();
 CS_Status_t currsen_adc_dis();
 
 float currsen_get_motor_current();
-float currsen_get_temp();
+int32_t currsen_get_temperature();

From 080b6a9e9418c8ed6945c4f2884b72ee485f88f2 Mon Sep 17 00:00:00 2001
From: Will Stuckey <guyfleeman@gmail.com>
Date: Tue, 11 Mar 2025 23:05:36 -0400
Subject: [PATCH 03/20] local adc testing tweaks

---
 control-board/src/bin/hwtest-motor/main.rs |  2 +-
 control-board/src/tasks/control_task.rs    |  4 +-
 motor-controller/bin/wheel-test/main.c     | 47 +++++++++++++++++-----
 motor-controller/bin/wheel/system.h        |  2 +-
 motor-controller/common/6step.c            |  2 +
 motor-controller/common/current_sensing.c  |  9 +++--
 motor-controller/common/current_sensing.h  |  2 +-
 7 files changed, 51 insertions(+), 17 deletions(-)

diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs
index 25d4bd59..fd9e7d5e 100644
--- a/control-board/src/bin/hwtest-motor/main.rs
+++ b/control-board/src/bin/hwtest-motor/main.rs
@@ -102,7 +102,7 @@ async fn main(main_spawner: embassy_executor::Spawner) {
 
 
     loop {
-        Timer::after_millis(100).await;
+        Timer::after_millis(10).await;
 
         test_command_publisher.publish(DataPacket::BasicControl(BasicControl {
             vel_x_linear: 1.0,
diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs
index 851c7076..469cc5ab 100644
--- a/control-board/src/tasks/control_task.rs
+++ b/control-board/src/tasks/control_task.rs
@@ -292,7 +292,7 @@ impl <
                     self.last_accel_y_ms = accel_ms[1];
                 }
                 
-                let controls_enabled = true;
+                let controls_enabled = false;
 
                 // let kill_vel = self.shared_robot_state.get_battery_low() || self.shared_robot_state.get_battery_crit() || self.shared_robot_state.shutdown_requested();
                 let kill_vel = false;
@@ -309,7 +309,7 @@ impl <
                 self.motor_br.set_setpoint(wheel_vels[2]);
                 self.motor_fr.set_setpoint(wheel_vels[3]);
 
-                // defmt::info!("wheel vels: {} {} {} {}", self.motor_fl.read_rads(), self.motor_bl.read_rads(), self.motor_br.read_rads(), self.motor_fr.read_rads());
+                defmt::info!("wheel vels: {} {} {} {}", self.motor_fl.read_current() as u32, self.motor_bl.read_current() as u32, self.motor_br.read_current() as u32, self.motor_fr.read_current() as u32);
 
                 self.send_motor_commands_and_telemetry(
                     &mut robot_controller, self.last_battery_v);
diff --git a/motor-controller/bin/wheel-test/main.c b/motor-controller/bin/wheel-test/main.c
index ee12e8c0..b0fe0d98 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_MODE, ADC_CH_MASK);
 
     // initialize motor driver
     pwm6step_setup();
@@ -157,9 +156,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.
     uart_initialize();
     uart_logging_status_rx_t uart_logging_status_receive;
@@ -291,7 +287,7 @@ int main() {
         if (run_torque_loop) {
             // recover torque using the shunt voltage drop, amplification network model and motor model
             // pct of voltage range 0-3.3V
-            float current_sense_shunt_v = ((float) res.I_motor_filt / (float) UINT16_MAX) * AVDD_V;
+            float current_sense_shunt_v = currsen_get_motor_current();
             // 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
@@ -335,6 +331,7 @@ int main() {
             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.temperature = currsen_get_temperature();
         }
 
         // run velocity loop if applicable
@@ -375,9 +372,20 @@ int main() {
 
         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_setpoint = r_motor;
+                pwm6step_set_duty_cycle_f(r_motor);
+            } else if (motion_control_type == VELOCITY) {
+                pwm6step_set_duty_cycle_f(control_setpoint_vel_duty);
+            } else {
+                pwm6step_set_duty_cycle_f(u_torque_loop);
+            }
 
             // load system state for transmit
 
@@ -510,5 +518,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/system.h b/motor-controller/bin/wheel/system.h
index 1b98af25..cafd2fbc 100644
--- a/motor-controller/bin/wheel/system.h
+++ b/motor-controller/bin/wheel/system.h
@@ -28,7 +28,7 @@
 #define ADC_MODE CS_MODE_DMA
 #define ADC_NUM_CHANNELS 4
 // ADC Motor Current Filtered (Ch 3), Motor Current Unfiltered (Ch 4), Temperature Sensor (Ch 16), Vrefint (Ch 17)
-#define ADC_CH_MASK (ADC_CHSELR_CHSEL3)
+#define ADC_CH_MASK (ADC_CHSELR_CHSEL4)
 // 110 = 71.5 ADC clock cycles for t_SMPR
 #define ADC_SR_MASK (ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1)
 
diff --git a/motor-controller/common/6step.c b/motor-controller/common/6step.c
index 98d4ee6c..bc1538a5 100644
--- a/motor-controller/common/6step.c
+++ b/motor-controller/common/6step.c
@@ -442,6 +442,8 @@ static void pwm6step_setup_commutation_timer(uint16_t pwm_freq_hz) {
     TIM1->CCMR1 = CCMR1_PHASE1_OFF | CCMR1_PHASE2_OFF;
     TIM1->CCMR2 = CCMR2_PHASE3_OFF;
 
+    // ADC trigger delay
+    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;
diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c
index 37eaae71..2d899038 100644
--- a/motor-controller/common/current_sensing.c
+++ b/motor-controller/common/current_sensing.c
@@ -120,6 +120,8 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
         DMA1_Channel1->CCR &= ~DMA_CCR_PSIZE_1;
         // Set DMA to circular mode
         DMA1_Channel1->CCR |= DMA_CCR_CIRC;
+        // Incrememnt 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.
@@ -158,7 +160,7 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
         ADC_CFGR1_RES |
         ADC_CFGR1_ALIGN,
         ADC_RESOLUTION_12B |
-        ADC_DATA_ALIGN_LEFT);
+        ADC_DATA_ALIGN_RIGHT);
 
     // Set ADC low power mode - None
     MODIFY_REG(
@@ -414,10 +416,11 @@ CS_Status_t currsen_adc_dis()
 float currsen_get_motor_current()
 {
     // TODO need more for motor current scaling
-    return V_ADC_SCALE * m_adc_result.Motor_current_raw;
+    // return ((float) m_adc_result.Motor_current_raw);
+    return V_ADC_SCALE * ((float) m_adc_result.Motor_current_raw);
 }
 
-/**
+/**Motor_current_raw
  * @brief gets the temperature. Translate from raw ADC value to
  *       to scaled raw ADC value to temperature.
  *
diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h
index 381befee..f496374c 100644
--- a/motor-controller/common/current_sensing.h
+++ b/motor-controller/common/current_sensing.h
@@ -11,7 +11,7 @@
 
 #pragma once
 
-#define V_DDA 3300 // mV
+#define V_DDA 3000.0f // mV
 #define V_ADC_SCALE V_DDA / 4095.0f
 
 // From A.7.16 of RM0091

From 85d49ed06749ba7b2bc1ab1346c1a632a2fb36e7 Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Mon, 5 May 2025 20:36:40 -0700
Subject: [PATCH 04/20] WIP Current control fixes

---
 control-board/src/stspin_motor.rs         |  18 ++-
 control-board/src/tasks/control_task.rs   |  63 ++++++----
 control-board/src/tasks/radio_task.rs     |  30 ++---
 motor-controller/bin/wheel/main.c         |   2 +-
 motor-controller/bin/wheel/system.h       |   7 +-
 motor-controller/common/6step.c           | 133 ++++++++++++----------
 motor-controller/common/current_sensing.c |  57 +++++++---
 motor-controller/common/current_sensing.h |  45 +++++++-
 software-communication                    |   2 +-
 9 files changed, 224 insertions(+), 133 deletions(-)

diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs
index b658b30a..97e8ec10 100644
--- a/control-board/src/stspin_motor.rs
+++ b/control-board/src/stspin_motor.rs
@@ -190,7 +190,7 @@ impl<
 
                 // copy receieved uart bytes into packet
                 let state = &mut mrp as *mut _ as *mut u8;
-                for i in 0..core::mem::size_of::<MotorResponse_Motion_Packet>() {
+                for i in 0..core::mem::size_of::<MotorResponsePacket>() {
                     *state.offset(i as isize) = buf[i];
                 }
 
@@ -323,6 +323,22 @@ impl<
     pub fn read_vel_computed_setpoint(&self) -> f32 {
         return self.current_state.vel_computed_setpoint;
     }
+
+    pub fn read_torque_computed_error(&self) -> f32 {
+        return self.current_state.torque_computed_error;
+    }
+
+    pub fn read_torque_computed_setpoint(&self) -> f32 {
+        return self.current_state.torque_computed_setpoint;
+    }
+
+    pub fn read_torque_estimate(&self) -> f32 {
+        return self.current_state.torque_estimate;
+    }
+
+    pub fn read_vbus_voltage(&self) -> f32 {
+        return self.current_state.vbus_voltage;
+    }
 }
 
 pub struct DribblerMotor<
diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs
index 469cc5ab..59034825 100644
--- a/control-board/src/tasks/control_task.rs
+++ b/control-board/src/tasks/control_task.rs
@@ -61,7 +61,7 @@ pub struct ControlTask<
     last_accel_x_ms: f32,
     last_accel_y_ms: f32,
     telemetry_publisher: TelemetryPublisher,
-    
+
     motor_fl: WheelMotor<'static, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>,
     motor_bl: WheelMotor<'static, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>,
     motor_br: WheelMotor<'static, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>,
@@ -72,8 +72,8 @@ impl <
     const MAX_RX_PACKET_SIZE: usize,
     const MAX_TX_PACKET_SIZE: usize,
     const RX_BUF_DEPTH: usize,
-    const TX_BUF_DEPTH: usize> 
-    ControlTask<MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH> 
+    const TX_BUF_DEPTH: usize>
+    ControlTask<MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>
     {
 
         pub fn new(robot_state: &'static SharedRobotState,
@@ -98,14 +98,14 @@ impl <
                 last_gyro_rads: 0.0,
                 last_accel_x_ms: 0.0,
                 last_accel_y_ms: 0.0,
-                motor_fl: motor_fl, 
+                motor_fl: motor_fl,
                 motor_bl: motor_bl,
                 motor_br: motor_br,
                 motor_fr: motor_fr,
             }
         }
 
-        fn do_control_update(&mut self, 
+        fn do_control_update(&mut self,
             robot_controller: &mut BodyVelocityController,
             cmd_vel: Vector3<f32>,
             gyro_rads: f32,
@@ -115,7 +115,7 @@ impl <
             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(),
@@ -132,16 +132,16 @@ impl <
                 self.motor_br.read_current(),
                 self.motor_fr.read_current()
             );
-        
+
             // TODO read from channel or something
 
-            robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads, controls_enabled); 
-            robot_controller.get_wheel_velocities()   
+            robot_controller.control_update(&cmd_vel, &wheel_vels, &wheel_torques, gyro_rads, controls_enabled);
+            robot_controller.get_wheel_velocities()
         }
 
         fn send_motor_commands_and_telemetry(&mut self,
                                             robot_controller: &mut BodyVelocityController,
-                                            battery_voltage: f32) 
+                                            battery_voltage: f32)
         {
             self.motor_fl.send_motion_command();
             self.motor_bl.send_motion_command();
@@ -190,7 +190,7 @@ impl <
             let control_debug_telem = TelemetryPacket::Control(control_debug_telem);
             self.telemetry_publisher.publish_immediate(control_debug_telem);
         }
-    
+
         async fn control_task_entry(&mut self) {
             defmt::info!("control task init.");
 
@@ -198,10 +198,10 @@ impl <
             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;
-             
+
             embassy_futures::join::join4(
                 self.motor_fl.leave_reset(),
                 self.motor_bl.leave_reset(),
@@ -225,7 +225,7 @@ impl <
 
             let robot_model = self.get_robot_model();
             let mut robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model);
-    
+
             let mut loop_rate_ticker = Ticker::every(Duration::from_millis(10));
 
             let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0);
@@ -272,7 +272,7 @@ impl <
                         },
                     }
                 }
-                
+
                 if ticks_since_control_packet >= TICKS_WITHOUT_PACKET_STOP {
                     cmd_vel = Vector3::new(0., 0., 0.);
                     //defmt::warn!("ticks since packet lockout");
@@ -291,7 +291,7 @@ impl <
                     self.last_accel_x_ms = accel_ms[0];
                     self.last_accel_y_ms = accel_ms[1];
                 }
-                
+
                 let controls_enabled = false;
 
                 // let kill_vel = self.shared_robot_state.get_battery_low() || self.shared_robot_state.get_battery_crit() || self.shared_robot_state.shutdown_requested();
@@ -309,8 +309,21 @@ impl <
                 self.motor_br.set_setpoint(wheel_vels[2]);
                 self.motor_fr.set_setpoint(wheel_vels[3]);
 
-                defmt::info!("wheel vels: {} {} {} {}", self.motor_fl.read_current() as u32, self.motor_bl.read_current() as u32, self.motor_br.read_current() as u32, self.motor_fr.read_current() as u32);
-
+                defmt::info!("CURRENT: {} {} {} {}, SP: {} {} {} {}, VBUS: {} {} {} {}",
+                    self.motor_fl.read_current() as u32,
+                    self.motor_bl.read_current() as u32,
+                    self.motor_br.read_current() as u32,
+                    self.motor_fr.read_current() as u32,
+                    self.motor_fl.read_vel_computed_setpoint() as u32,
+                    self.motor_bl.read_vel_computed_setpoint() as u32,
+                    self.motor_br.read_vel_computed_setpoint() as u32,
+                    self.motor_fr.read_vel_computed_setpoint() as u32,
+                    (self.motor_fl.read_vbus_voltage() * 100.0) as u32,
+                    (self.motor_bl.read_vbus_voltage() * 100.0) as u32,
+                    (self.motor_br.read_vbus_voltage() * 100.0) as u32,
+                    (self.motor_fr.read_vbus_voltage() * 100.0) as u32);
+
+                //defmt::info!("stspin temp: {} {} {} {}", self.motor_fl.read_mcu_temperature(), self.motor_bl.read_mcu_temperature(), self.motor_br.read_mcu_temperature(), self.motor_fr.read_mcu_temperature());
                 self.send_motor_commands_and_telemetry(
                     &mut robot_controller, self.last_battery_v);
 
@@ -363,9 +376,9 @@ impl <
                     self.motor_fr.load_default_firmware_image(),
                 )
                 .await;
-                
-                let error_mask = res.0.is_err() as u8 
-                        | ((res.1.is_err() as u8) & 0x01) << 1 
+
+                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;
 
@@ -402,11 +415,11 @@ impl <
             };
 
             let robot_model: RobotModel = RobotModel::new(robot_model_constants);
-            
-            return robot_model; 
+
+            return robot_model;
         }
     }
-    
+
 #[embassy_executor::task]
 async fn control_task_entry(mut control_task: ControlTask<MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>) {
     loop {
@@ -458,7 +471,7 @@ pub async fn start_control_task(
     ////////////////////////////////
     //  create motor controllers  //
     ////////////////////////////////
-    
+
     let motor_fl = WheelMotor::new_from_pins(&FRONT_LEFT_IDLE_BUFFERED_UART, FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(),  FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(),  motor_fl_boot0_pin, motor_fl_nrst_pin, WHEEL_FW_IMG);
     let motor_bl = WheelMotor::new_from_pins(&BACK_LEFT_IDLE_BUFFERED_UART, BACK_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), BACK_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(),  motor_bl_boot0_pin, motor_bl_nrst_pin, WHEEL_FW_IMG);
     let motor_br = WheelMotor::new_from_pins(&BACK_RIGHT_IDLE_BUFFERED_UART,  BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_read_queue(), BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_write_queue(), motor_br_boot0_pin, motor_br_nrst_pin, WHEEL_FW_IMG);
diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs
index d585aa21..c2763edd 100644
--- a/control-board/src/tasks/radio_task.rs
+++ b/control-board/src/tasks/radio_task.rs
@@ -1,5 +1,5 @@
 
-use ateam_common_packets::{bindings::BasicTelemetry, radio::TelemetryPacket};
+use ateam_common_packets::{bindings::BasicTelemetry, bindings::RadioPacket, radio::TelemetryPacket};
 use ateam_lib_stm32::{idle_buffered_uart_read_task, idle_buffered_uart_write_task, static_idle_buffered_uart, static_idle_buffered_uart_nl, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}};
 use credentials::WifiCredential;
 use embassy_executor::{SendSpawner, Spawner};
@@ -18,21 +18,21 @@ macro_rules! create_radio_task {
         $radio_command_publisher:ident, $radio_telemetry_subscriber:ident,
         $wifi_credentials:ident, $p:ident) => {
         ateam_control_board::tasks::radio_task::start_radio_task(
-            $main_spawner, $rx_uart_queue_spawner, $tx_uart_queue_spawner, 
+            $main_spawner, $rx_uart_queue_spawner, $tx_uart_queue_spawner,
             $robot_state,
             $radio_command_publisher, $radio_telemetry_subscriber,
             &$wifi_credentials,
             $p.USART2, $p.PD6, $p.PD5, $p.PD3, $p.PD4,
             $p.DMA2_CH1, $p.DMA2_CH0,
-            $p.PD7, $p.PA15); 
+            $p.PD7, $p.PA15);
     };
 }
 
 pub const RADIO_LOOP_RATE_MS: u64 = 10;
 
-pub const RADIO_MAX_TX_PACKET_SIZE: usize = 320;
+pub const RADIO_MAX_TX_PACKET_SIZE: usize = core::mem::size_of::<RadioPacket>();
 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::<RadioPacket>();
 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, #[link_section = ".axisram.buffers"]);
@@ -69,7 +69,7 @@ impl<
         const RADIO_MAX_TX_PACKET_SIZE: usize,
         const RADIO_MAX_RX_PACKET_SIZE: usize,
         const RADIO_TX_BUF_DEPTH: usize,
-        const RADIO_RX_BUF_DEPTH: usize> 
+        const RADIO_RX_BUF_DEPTH: usize>
     RadioTask<RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH> {
     // pub type TaskRobotRadio = RobotRadio<'static, RADIO_MAX_TX_PACKET_SIZE, RADIO_MAX_RX_PACKET_SIZE, RADIO_TX_BUF_DEPTH, RADIO_RX_BUF_DEPTH>;
 
@@ -150,7 +150,7 @@ impl<
             if cur_robot_state != last_robot_state {
                 // we are connected to software and robot id or team was changed on hardware
                 if self.connection_state == RadioConnectionState::Connected &&
-                        (cur_robot_state.hw_robot_id != last_robot_state.hw_robot_id || 
+                        (cur_robot_state.hw_robot_id != last_robot_state.hw_robot_id ||
                         cur_robot_state.hw_robot_team_is_blue != last_robot_state.hw_robot_team_is_blue) {
                     // Enter connected network state (disconnecting)
                     self.connection_state = RadioConnectionState::ConnectedNetwork;
@@ -158,7 +158,7 @@ impl<
                 }
 
                 // We are at least connected on UART and the wifi network id changed
-                if self.connection_state >= RadioConnectionState::ConnectedUart && 
+                if self.connection_state >= RadioConnectionState::ConnectedUart &&
                     cur_robot_state.hw_wifi_network_index != last_robot_state.hw_wifi_network_index {
                     defmt::info!("dip state change triggering wifi network change");
 
@@ -196,7 +196,7 @@ impl<
                 RadioConnectionState::ConnectedPhys => {
                     if self.connect_uart().await.is_err() {
                         radio_inop_flag_local = true;
-                        // If the pin is unconnected, will be overridden out of the state. 
+                        // If the pin is unconnected, will be overridden out of the state.
                         // So just check UART again.
                         self.connection_state = RadioConnectionState::ConnectedPhys;
                     } else {
@@ -242,17 +242,17 @@ impl<
                     let _ = self.process_packets().await;
                     // if we're stably connected, process packets at 100Hz
 
-                    // If timeout have elapsed since we last got a packet, 
+                    // If timeout have elapsed since we last got a packet,
                     // reboot the robot (unless we had a shutdown request).
                     let cur_time = Instant::now();
-                    if !cur_robot_state.shutdown_requested && 
-                        Instant::checked_duration_since(&cur_time, self.last_software_packet).unwrap().as_millis() > Self::RESPONSE_FROM_PC_TIMEOUT_MS {                        
+                    if !cur_robot_state.shutdown_requested &&
+                        Instant::checked_duration_since(&cur_time, self.last_software_packet).unwrap().as_millis() > Self::RESPONSE_FROM_PC_TIMEOUT_MS {
                         cortex_m::peripheral::SCB::sys_reset();
                     }
                 },
             }
 
-            // set global radio connected flag           
+            // set global radio connected flag
             self.shared_robot_state.set_radio_network_ok(self.connection_state >= RadioConnectionState::ConnectedNetwork);
             self.shared_robot_state.set_radio_bridge_ok(self.connection_state == RadioConnectionState::Connected);
             self.shared_robot_state.set_radio_inop(radio_inop_flag_local);
@@ -298,7 +298,7 @@ impl<
             return Err(());
         }
         defmt::info!("radio connected");
-    
+
         let res = self.radio.open_multicast().await;
         if res.is_err() {
             defmt::error!("failed to establish multicast socket to network.");
@@ -322,7 +322,7 @@ impl<
             defmt::error!("send hello failed.");
             return Err(());
         }
-        
+
         let hello = self.radio.wait_hello(Duration::from_millis(Self::RESPONSE_FROM_PC_TIMEOUT_MS)).await;
         match hello {
             Ok(hello) => {
diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c
index b0fe0d98..1bc9bd05 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -331,7 +331,7 @@ int main() {
             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.temperature = currsen_get_temperature();
+            response_packet.data.motion.vbus_voltage = currsen_get_vbus_voltage();
         }
 
         // run velocity loop if applicable
diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h
index cafd2fbc..de511127 100644
--- a/motor-controller/bin/wheel/system.h
+++ b/motor-controller/bin/wheel/system.h
@@ -26,11 +26,8 @@
 //////////////////
 
 #define ADC_MODE CS_MODE_DMA
-#define ADC_NUM_CHANNELS 4
-// ADC Motor Current Filtered (Ch 3), Motor Current Unfiltered (Ch 4), Temperature Sensor (Ch 16), Vrefint (Ch 17)
-#define ADC_CH_MASK (ADC_CHSELR_CHSEL4)
-// 110 = 71.5 ADC clock cycles for t_SMPR
-#define ADC_SR_MASK (ADC_SMPR_SMP_2 | ADC_SMPR_SMP_1)
+// 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 bc1538a5..4fbbf58d 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
@@ -102,7 +102,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 +113,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 +135,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 +163,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 +177,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 +229,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 +254,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 +285,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 +385,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 +414,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,13 +437,19 @@ 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
+    // 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
@@ -453,6 +460,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;
 
@@ -499,18 +507,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() {
@@ -531,8 +539,8 @@ static void TIM2_IRQHandler_HallTransition() {
 }
 
 /**
- * @brief 
- * 
+ * @brief
+ *
  */
 static void perform_commutation_cycle() {
     // mask off Hall lines PA0-PA2 (already the LSBs)
@@ -618,16 +626,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));
@@ -644,7 +652,7 @@ static uint8_t read_hall() {
 
 /**
  * @brief stages com values for estop
- * 
+ *
  */
 static void set_commutation_estop() {
     set_commutation_for_hall(0, true);
@@ -652,8 +660,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;
@@ -679,15 +687,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;
@@ -712,7 +721,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;
@@ -755,7 +764,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;
@@ -771,14 +780,14 @@ 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 
+    if (scaled_dc >= MINIMUM_EFFECTIVE_DUTY_CYCLE_RAW
             && motor_direction != commanded_motor_direction) {
         direction_change_commanded = true;
     }
@@ -797,8 +806,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() {
 
@@ -808,9 +817,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;
@@ -849,7 +858,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() {
@@ -871,7 +880,7 @@ bool pwm6step_is_direction_inverted() {
 
 const MotorErrors_t pwm6step_get_motor_errors() {
     return motor_errors;
-} 
+}
 
 bool pwm6step_hall_rps_estimate_valid() {
 
@@ -880,5 +889,3 @@ bool pwm6step_hall_rps_estimate_valid() {
 int pwm6step_hall_get_rps_estimate() {
 
 }
-
-
diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c
index 2d899038..48960fd2 100644
--- a/motor-controller/common/current_sensing.c
+++ b/motor-controller/common/current_sensing.c
@@ -135,8 +135,8 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
         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 channels.
-        // Temperature and motor so 2.
+        // 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.
@@ -149,6 +149,11 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
     if (status != CS_OK)
         return status;
 
+    // Calibration (should happen before DMAEN in CFGR1)
+    status = currsen_adc_cal();
+    if (status != CS_OK)
+        return status;
+
     // TODO Add ADC watchdog?
 
     // Set ADC data resolution
@@ -208,6 +213,11 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
         | 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
@@ -225,23 +235,25 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
     // 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).
-    // Will go with SMP = 110 (71.5 ADC clock cycles) for now as motor should be low pass filtered.
+    // 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;
+    */
 
-    // Calibration
-    status = currsen_adc_cal();
-    if (status != CS_OK)
-        return status;
+    // For now, 1.5 ADC clock cycles (0.125 us) is the minimum sampling time.
+    ADC1->SMPR = 0;
+
+    // Set ADC channel selection. Ch9 is the Vbus.
+    ADC1->CHSELR = motor_adc_ch | ADC_CHSELR_CHSEL9;
 
     // Enable
     status = currsen_adc_en();
@@ -258,10 +270,10 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
     return status;
 }
 
-void ADC1_IRQHandler() {
-    GPIOB->BSRR |= GPIO_BSRR_BR_9;
-    ADC1->ISR |= (ADC_ISR_EOSEQ);
-}
+//void ADC1_IRQHandler() {
+//    GPIOB->BSRR |= GPIO_BSRR_BR_9;
+//    ADC1->ISR |= (ADC_ISR_EOSEQ);
+//}
 
 /**
  * @brief calibrate the ADC
@@ -415,12 +427,21 @@ CS_Status_t currsen_adc_dis()
  */
 float currsen_get_motor_current()
 {
-    // TODO need more for motor current scaling
-    // return ((float) m_adc_result.Motor_current_raw);
-    return V_ADC_SCALE * ((float) m_adc_result.Motor_current_raw);
+    return I_MOTOR_SCALE * ((float) m_adc_result.Motor_current_raw);
 }
 
-/**Motor_current_raw
+/**
+ * @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.
  *
@@ -430,10 +451,10 @@ float currsen_get_motor_current()
 int32_t currsen_get_temperature()
 {
     // From A.7.16 of RM0091
-    int32_t temperature; /* will contain the temperature in degrees Celsius */
+    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);
+    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);
diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h
index f496374c..f755421c 100644
--- a/motor-controller/common/current_sensing.h
+++ b/motor-controller/common/current_sensing.h
@@ -11,15 +11,51 @@
 
 #pragma once
 
-#define V_DDA 3000.0f // mV
-#define V_ADC_SCALE V_DDA / 4095.0f
+#define V_DDA_MV 3000.0f // mV
+#define V_DDA_V 3.0f // V
+#define V_ADC_SCALE_MV (V_DDA_MV / 4095.0f) // mV
+#define 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/10))
+#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
+// Rb = 2.37k
+// Ra = 140k
+// R1 = 10.0k
+// R2 = 40.2k
+// 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
+// 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
+
+// 1. I_motor = Vs / Rs
+// 2. Vs * G_real = V_adc
+// 3. V_adc = V_adc_raw * V_ADC_SCALE
+// I_motor = ((V_adc_raw * V_ADC_SCALE) / G_real) / Rs
+// I_MOTOR_SCALE = V_ADC_SCALE / (G_real * Rs)
+// I_motor = V_adc_raw * I_MOTOR_SCALE
+#define I_MOTOR_SCALE 0.00296599486f // A/V
+// TODO Add timing offset to the ADC for settling time.
+
+// 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.
+// Vbus = V_adc_raw * VBUS_SCALE
+#define VBUS_SCALE 0.0091575f
 
 typedef enum {
   CS_MODE_POLLING,
@@ -92,7 +128,6 @@ typedef enum
 // TODO tune timing
 #define ADC_STP_TIMEOUT 5 //ms
 
-
 // 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
@@ -103,6 +138,7 @@ typedef enum
 typedef struct
 __attribute__((__packed__)) ADC_Result {
     uint16_t    Motor_current_raw;
+    uint16_t    Vbus_raw;
     uint16_t    Spin_temperature_raw;
 } ADC_Result_t;
 
@@ -115,4 +151,5 @@ CS_Status_t currsen_adc_en();
 CS_Status_t currsen_adc_dis();
 
 float currsen_get_motor_current();
+float currsen_get_vbus_voltage();
 int32_t currsen_get_temperature();
diff --git a/software-communication b/software-communication
index 5c06b8ac..b3ac11bd 160000
--- a/software-communication
+++ b/software-communication
@@ -1 +1 @@
-Subproject commit 5c06b8ac380e254188fa2cb25310c4b09e1d262c
+Subproject commit b3ac11bdf3704e4e3d4bbba585bf8e249d2e5d85

From 8197671568ddbfa7d6585205e69052947e414f9f Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Mon, 5 May 2025 20:38:31 -0700
Subject: [PATCH 05/20] WIP update software communication

---
 software-communication | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/software-communication b/software-communication
index b3ac11bd..38c668a2 160000
--- a/software-communication
+++ b/software-communication
@@ -1 +1 @@
-Subproject commit b3ac11bdf3704e4e3d4bbba585bf8e249d2e5d85
+Subproject commit 38c668a2ee5800784a9406f7e4a26c03735467ff

From 4d4db0609e3acbeddb0be1fdc115b09a861ccc92 Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Tue, 27 May 2025 20:21:06 -0700
Subject: [PATCH 06/20] Add timing register, redo functions

---
 control-board/src/tasks/control_task.rs   | 8 ++++----
 motor-controller/bin/wheel/main.c         | 7 ++-----
 motor-controller/common/current_sensing.c | 5 +++--
 3 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs
index 59034825..cbddfeee 100644
--- a/control-board/src/tasks/control_task.rs
+++ b/control-board/src/tasks/control_task.rs
@@ -310,10 +310,10 @@ impl <
                 self.motor_fr.set_setpoint(wheel_vels[3]);
 
                 defmt::info!("CURRENT: {} {} {} {}, SP: {} {} {} {}, VBUS: {} {} {} {}",
-                    self.motor_fl.read_current() as u32,
-                    self.motor_bl.read_current() as u32,
-                    self.motor_br.read_current() as u32,
-                    self.motor_fr.read_current() as u32,
+                    (self.motor_fl.read_current() * 100.0) as u32,
+                    (self.motor_bl.read_current() * 100.0) as u32,
+                    (self.motor_br.read_current() * 100.0) as u32,
+                    (self.motor_fr.read_current() * 100.0) as u32,
                     self.motor_fl.read_vel_computed_setpoint() as u32,
                     self.motor_bl.read_vel_computed_setpoint() as u32,
                     self.motor_br.read_vel_computed_setpoint() as u32,
diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c
index 1bc9bd05..dd4bb288 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -285,11 +285,8 @@ 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 = currsen_get_motor_current();
-            // map voltage given by the amp network to current
-            float current_sense_I = mm_voltage_to_current(&df45_model, current_sense_shunt_v);
+            // Get the current in amps from the current sense ADC.
+            float current_sense_I = currsen_get_motor_current();
             // map current to torque using the motor model
             float measured_torque_Nm = mm_current_to_torque(&df45_model, current_sense_I);
             // filter torque
diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c
index 48960fd2..58ef804b 100644
--- a/motor-controller/common/current_sensing.c
+++ b/motor-controller/common/current_sensing.c
@@ -249,8 +249,9 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
     ADC1->CHSELR = motor_adc_ch | ADC_CHSELR_CHSEL16;
     */
 
-    // For now, 1.5 ADC clock cycles (0.125 us) is the minimum sampling time.
-    ADC1->SMPR = 0;
+    // 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;

From 024bbc2a2b408238752a230210d20afa2bbb2408 Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Fri, 30 May 2025 09:08:23 -0700
Subject: [PATCH 07/20] First pass of current control

---
 control-board/src/stspin_motor.rs         | 24 ++++++++------
 control-board/src/tasks/control_task.rs   | 26 +++++++--------
 motor-controller/bin/dribbler/main.c      |  2 +-
 motor-controller/bin/wheel-test/main.c    | 40 +++++++++++------------
 motor-controller/bin/wheel-test/main.h    |  9 ++---
 motor-controller/bin/wheel/main.c         | 35 ++++++++++----------
 motor-controller/bin/wheel/main.h         |  3 +-
 motor-controller/common/6step.c           | 13 ++++----
 motor-controller/common/6step.h           | 14 +++++---
 motor-controller/common/current_sensing.h |  1 -
 motor-controller/common/iir.c             |  2 +-
 motor-controller/common/motor_model.c     | 13 ++++++--
 motor-controller/common/motor_model.h     |  3 +-
 motor-controller/common/pid.c             |  2 +-
 14 files changed, 103 insertions(+), 84 deletions(-)

diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs
index 97e8ec10..b33e2bd1 100644
--- a/control-board/src/stspin_motor.rs
+++ b/control-board/src/stspin_motor.rs
@@ -300,10 +300,6 @@ impl<
         return self.current_state.hall_power_error() != 0 || self.current_state.hall_disconnected_error() != 0 || 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;
     }
@@ -320,20 +316,28 @@ 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_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_setpoint(&self) -> f32 {
-        return self.current_state.torque_computed_setpoint;
+    pub fn read_torque_computed_nm(&self) -> f32 {
+        return self.current_state.torque_computed_nm;
     }
 
-    pub fn read_torque_estimate(&self) -> f32 {
-        return self.current_state.torque_estimate;
+    pub fn read_torque_computed_duty(&self) -> f32 {
+        return self.current_state.torque_computed_duty;
     }
 
     pub fn read_vbus_voltage(&self) -> f32 {
diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs
index cbddfeee..3d5670e8 100644
--- a/control-board/src/tasks/control_task.rs
+++ b/control-board/src/tasks/control_task.rs
@@ -309,19 +309,19 @@ impl <
                 self.motor_br.set_setpoint(wheel_vels[2]);
                 self.motor_fr.set_setpoint(wheel_vels[3]);
 
-                defmt::info!("CURRENT: {} {} {} {}, SP: {} {} {} {}, VBUS: {} {} {} {}",
-                    (self.motor_fl.read_current() * 100.0) as u32,
-                    (self.motor_bl.read_current() * 100.0) as u32,
-                    (self.motor_br.read_current() * 100.0) as u32,
-                    (self.motor_fr.read_current() * 100.0) as u32,
-                    self.motor_fl.read_vel_computed_setpoint() as u32,
-                    self.motor_bl.read_vel_computed_setpoint() as u32,
-                    self.motor_br.read_vel_computed_setpoint() as u32,
-                    self.motor_fr.read_vel_computed_setpoint() as u32,
-                    (self.motor_fl.read_vbus_voltage() * 100.0) as u32,
-                    (self.motor_bl.read_vbus_voltage() * 100.0) as u32,
-                    (self.motor_br.read_vbus_voltage() * 100.0) as u32,
-                    (self.motor_fr.read_vbus_voltage() * 100.0) as u32);
+                defmt::info!("TORQUE: {} {} {} {}, TORQUE_DC: {} {} {} {}, VEL_DC: {} {} {} {}",
+                    (self.motor_fl.read_torque_estimate() * 1000.0) as i32,
+                    (self.motor_bl.read_torque_estimate() * 1000.0) as i32,
+                    (self.motor_br.read_torque_estimate() * 1000.0) as i32,
+                    (self.motor_fr.read_torque_estimate() * 1000.0) as i32,
+                    (self.motor_fl.read_torque_computed_duty() * 1000.0) as i32,
+                    (self.motor_bl.read_torque_computed_duty() * 1000.0) as i32,
+                    (self.motor_br.read_torque_computed_duty() * 1000.0) as i32,
+                    (self.motor_fr.read_torque_computed_duty() * 1000.0) as i32,
+                    (self.motor_fl.read_vel_computed_duty() * 1000.0) as i32,
+                    (self.motor_bl.read_vel_computed_duty() * 1000.0) as i32,
+                    (self.motor_br.read_vel_computed_duty() * 1000.0) as i32,
+                    (self.motor_fr.read_vel_computed_duty() * 1000.0) as i32);
 
                 //defmt::info!("stspin temp: {} {} {} {}", self.motor_fl.read_mcu_temperature(), self.motor_bl.read_mcu_temperature(), self.motor_br.read_mcu_temperature(), self.motor_fr.read_mcu_temperature());
                 self.send_motor_commands_and_telemetry(
diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c
index e788985d..7d85ff34 100644
--- a/motor-controller/bin/dribbler/main.c
+++ b/motor-controller/bin/dribbler/main.c
@@ -218,7 +218,7 @@ int main() {
             response_packet.data.motion.torque_setpoint = r;
             response_packet.data.motion.torque_estimate = cur_measurement;
             response_packet.data.motion.torque_computed_error = torque_pid.prev_err;
-            response_packet.data.motion.torque_computed_setpoint = torque_setpoint;
+            response_packet.data.motion.torque_computed_nm = torque_setpoint;
         }
 
         if (run_torque_loop) {
diff --git a/motor-controller/bin/wheel-test/main.c b/motor-controller/bin/wheel-test/main.c
index b0fe0d98..c8de4d8d 100644
--- a/motor-controller/bin/wheel-test/main.c
+++ b/motor-controller/bin/wheel-test/main.c
@@ -107,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;
@@ -131,6 +131,7 @@ 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;
 
     // setup the velocity PID
     PidConstants_t vel_pid_constants;
@@ -285,22 +286,18 @@ 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 = currsen_get_motor_current();
-            // map voltage given by the amp network to current
-            float current_sense_I = mm_voltage_to_current(&df45_model, current_sense_shunt_v);
+            // Get the current in amps from the current sense ADC.
+            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
             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) {
+            // Correct torque sign based on 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 = -fabs(measured_torque_Nm);
             }
 
@@ -309,7 +306,7 @@ int main() {
             if (motion_control_type == TORQUE) {
                 r_Nm = r_motor_board;
             } else {
-                r_Nm = control_setpoint_vel_duty;
+                r_Nm = vel_computed_duty;
             }
 
             // calculate PID on the torque in Nm
@@ -318,7 +315,7 @@ int main() {
             // 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);
+            u_torque_loop = mm_pos_current_to_pos_dc(&df45_model, current_setpoint, vbus_voltage);
 
             if (torque_setpoint_Nm < 0.0f) {
                 u_torque_loop = -fabs(u_torque_loop);
@@ -330,8 +327,9 @@ int main() {
             response_packet.data.motion.current_estimate = current_sense_I;
             response_packet.data.motion.torque_estimate = measured_torque_Nm;
             response_packet.data.motion.torque_computed_error = torque_pid.prev_err;
-            response_packet.data.motion.torque_computed_setpoint = torque_setpoint_Nm;
-            //response_packet.data.motion.temperature = currsen_get_temperature();
+            response_packet.data.motion.torque_computed_nm = torque_setpoint_Nm;
+            response_packet.data.motion.torque_computed_duty = u_torque_loop;
+            response_packet.data.motion.vbus_voltage = vbus_voltage;
         }
 
         // run velocity loop if applicable
@@ -359,15 +357,15 @@ 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_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) {
@@ -379,10 +377,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);
             }
diff --git a/motor-controller/bin/wheel-test/main.h b/motor-controller/bin/wheel-test/main.h
index c89e19cf..aa262342 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 <math.h>
@@ -48,11 +48,12 @@
 #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
 
 ////////////////////////
 //  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/main.c b/motor-controller/bin/wheel/main.c
index dd4bb288..95b3b6d2 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -107,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;
@@ -131,6 +131,7 @@ 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;
 
     // setup the velocity PID
     PidConstants_t vel_pid_constants;
@@ -287,17 +288,16 @@ int main() {
         if (run_torque_loop) {
             // Get the current in amps from the current sense ADC.
             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
             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) {
+            // Correct torque sign based on 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 = -fabs(measured_torque_Nm);
             }
 
@@ -306,7 +306,7 @@ int main() {
             if (motion_control_type == TORQUE) {
                 r_Nm = r_motor_board;
             } else {
-                r_Nm = control_setpoint_vel_duty;
+                r_Nm = vel_computed_duty;
             }
 
             // calculate PID on the torque in Nm
@@ -315,20 +315,21 @@ int main() {
             // 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);
+            u_torque_loop = mm_pos_current_to_pos_dc(&df45_model, current_setpoint, vbus_voltage);
 
-            if (torque_setpoint_Nm < 0.0f) {
+            if (torque_setpoint_Nm > 0.0f) {
                 u_torque_loop = -fabs(u_torque_loop);
             }
 
             // 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.vbus_voltage = currsen_get_vbus_voltage();
+            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
@@ -356,15 +357,15 @@ 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.encoder_delta = enc_delta;
             response_packet.data.motion.vel_enc_estimate = enc_rad_s_filt;
             response_packet.data.motion.vel_computed_error = vel_pid.prev_err;
-            response_packet.data.motion.vel_computed_setpoint = control_setpoint_vel_duty;
+            response_packet.data.motion.vel_computed_rads = control_setpoint_vel_rads;
+            response_packet.data.motion.vel_computed_duty = vel_computed_duty;
         }
 
         if (run_torque_loop || run_vel_loop) {
@@ -376,10 +377,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);
             }
diff --git a/motor-controller/bin/wheel/main.h b/motor-controller/bin/wheel/main.h
index 928dc238..7c34f80d 100644
--- a/motor-controller/bin/wheel/main.h
+++ b/motor-controller/bin/wheel/main.h
@@ -46,11 +46,12 @@
 #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
 
 ////////////////////////
 //  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/common/6step.c b/motor-controller/common/6step.c
index 4fbbf58d..86c1b551 100644
--- a/motor-controller/common/6step.c
+++ b/motor-controller/common/6step.c
@@ -32,11 +32,6 @@
 #include "system.h"
 #include "time.h"
 
-typedef enum MotorDirection {
-    CLOCKWISE,
-    COUNTER_CLOCKWISE
-} MotorDirection_t;
-
 typedef enum CommutationValuesType {
     NORMAL,
     MOMENTUM_DETECTED,
@@ -787,8 +782,8 @@ void TIM1_CC_IRQHandler() {
  */
 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;
     }
 
@@ -889,3 +884,7 @@ bool pwm6step_hall_rps_estimate_valid() {
 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.h b/motor-controller/common/current_sensing.h
index f755421c..2dcfe208 100644
--- a/motor-controller/common/current_sensing.h
+++ b/motor-controller/common/current_sensing.h
@@ -47,7 +47,6 @@
 // I_MOTOR_SCALE = V_ADC_SCALE / (G_real * Rs)
 // I_motor = V_adc_raw * I_MOTOR_SCALE
 #define I_MOTOR_SCALE 0.00296599486f // A/V
-// TODO Add timing offset to the ADC for settling time.
 
 // VBUS voltage scaling
 // 11.5k / 1k resistor divider -> 12.5 scaling
diff --git a/motor-controller/common/iir.c b/motor-controller/common/iir.c
index b6c0912d..8fc2d617 100644
--- a/motor-controller/common/iir.c
+++ b/motor-controller/common/iir.c
@@ -12,6 +12,6 @@ void iir_filter_init(IIRFilter_t *iir_filter, float alpha) {
 
 float iir_filter_update(IIRFilter_t *iir_filter, float cur_val) {
     float Vf = (iir_filter->alpha * iir_filter->previous_value) + ((1.0f - iir_filter->alpha) * cur_val);
-    iir_filter->previous_value = cur_val;
+    iir_filter->previous_value = Vf;
     return Vf;
 }
\ No newline at end of file
diff --git a/motor-controller/common/motor_model.c b/motor-controller/common/motor_model.c
index c3dd1ad8..28c5037d 100644
--- a/motor-controller/common/motor_model.c
+++ b/motor-controller/common/motor_model.c
@@ -17,6 +17,7 @@ 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;
 }
 
 float mm_rads_to_dc(MotorModel_t *mm, float avel_rads) {
@@ -56,6 +57,14 @@ float mm_torque_to_current(MotorModel_t *mm, float torque) {
     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) {
+    // I_motor = V_motor / R_motor
+    // V_motor = duty_cycle * V_bus
+    // duty_cycle = I_motor * (R_motor / V_bus)
+    if (vbus_voltage == 0.0f) {
+        return 0.0f; // avoid division by zero
+    }
+
+    // bound DC [0, 1]
+    return fmax(fmin(current * (mm->line_resistance / 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..dbd7848a 100644
--- a/motor-controller/common/motor_model.h
+++ b/motor-controller/common/motor_model.h
@@ -13,6 +13,7 @@ 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
 } MotorModel_t;
 
 void mm_initialize(MotorModel_t *mm);
@@ -22,4 +23,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);
\ No newline at end of file
diff --git a/motor-controller/common/pid.c b/motor-controller/common/pid.c
index 82e26983..bc0a947e 100644
--- a/motor-controller/common/pid.c
+++ b/motor-controller/common/pid.c
@@ -42,7 +42,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) * 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);

From 3558bbc4c67da0e69c5547502a061707de41ee29 Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Tue, 3 Jun 2025 22:29:01 -0700
Subject: [PATCH 08/20] Cleanup and adds opamp offset

---
 control-board/src/bin/hwtest-motor/main.rs |   8 +-
 control-board/src/stspin_motor.rs          |   4 +
 control-board/src/tasks/control_task.rs    |  28 ++--
 motor-controller/bin/dribbler/main.c       |   2 +-
 motor-controller/bin/dribbler/system.h     |   2 -
 motor-controller/bin/wheel-test/main.c     |  62 +++++---
 motor-controller/bin/wheel-test/main.h     |   2 +
 motor-controller/bin/wheel-test/system.h   |   3 +-
 motor-controller/bin/wheel/main.c          | 115 +++++++++------
 motor-controller/bin/wheel/main.h          |   7 +-
 motor-controller/bin/wheel/system.h        |   1 -
 motor-controller/common/current_sensing.c  | 163 +++++++--------------
 motor-controller/common/current_sensing.h  |  57 +++----
 motor-controller/common/motor_model.c      |  23 ++-
 motor-controller/common/motor_model.h      |   3 +-
 software-communication                     |   2 +-
 16 files changed, 249 insertions(+), 233 deletions(-)

diff --git a/control-board/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs
index fd9e7d5e..4f6c2bf9 100644
--- a/control-board/src/bin/hwtest-motor/main.rs
+++ b/control-board/src/bin/hwtest-motor/main.rs
@@ -93,8 +93,8 @@ async fn main(main_spawner: embassy_executor::Spawner) {
         imu_gyro_data_publisher, imu_accel_data_publisher,
         p);
 
-    create_control_task!(main_spawner, uart_queue_spawner, 
-        robot_state, 
+    create_control_task!(main_spawner, uart_queue_spawner,
+        robot_state,
         control_command_subscriber, control_telemetry_publisher,
         battery_volt_subscriber,
         control_gyro_data_subscriber, control_accel_data_subscriber,
@@ -105,9 +105,9 @@ async fn main(main_spawner: embassy_executor::Spawner) {
         Timer::after_millis(10).await;
 
         test_command_publisher.publish(DataPacket::BasicControl(BasicControl {
-            vel_x_linear: 1.0,
+            vel_x_linear: 0.0,
             vel_y_linear: 0.0,
-            vel_z_angular: 0.0,
+            vel_z_angular: 5.0,
             kick_vel: 0.0,
             dribbler_speed: 10.0,
             kick_request: KickRequest::KR_DISABLE,
diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs
index b33e2bd1..1cd0a696 100644
--- a/control-board/src/stspin_motor.rs
+++ b/control-board/src/stspin_motor.rs
@@ -324,6 +324,10 @@ impl<
         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;
     }
diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs
index 3d5670e8..2abfc002 100644
--- a/control-board/src/tasks/control_task.rs
+++ b/control-board/src/tasks/control_task.rs
@@ -309,19 +309,21 @@ impl <
                 self.motor_br.set_setpoint(wheel_vels[2]);
                 self.motor_fr.set_setpoint(wheel_vels[3]);
 
-                defmt::info!("TORQUE: {} {} {} {}, TORQUE_DC: {} {} {} {}, VEL_DC: {} {} {} {}",
-                    (self.motor_fl.read_torque_estimate() * 1000.0) as i32,
-                    (self.motor_bl.read_torque_estimate() * 1000.0) as i32,
-                    (self.motor_br.read_torque_estimate() * 1000.0) as i32,
-                    (self.motor_fr.read_torque_estimate() * 1000.0) as i32,
-                    (self.motor_fl.read_torque_computed_duty() * 1000.0) as i32,
-                    (self.motor_bl.read_torque_computed_duty() * 1000.0) as i32,
-                    (self.motor_br.read_torque_computed_duty() * 1000.0) as i32,
-                    (self.motor_fr.read_torque_computed_duty() * 1000.0) as i32,
-                    (self.motor_fl.read_vel_computed_duty() * 1000.0) as i32,
-                    (self.motor_bl.read_vel_computed_duty() * 1000.0) as i32,
-                    (self.motor_br.read_vel_computed_duty() * 1000.0) as i32,
-                    (self.motor_fr.read_vel_computed_duty() * 1000.0) as i32);
+                defmt::info!("MEASURED_CUR: FL: {}, BL: {}, BR: {}, FR: {}",
+                    (self.motor_fl.read_current() * 1000.0) as i32,
+                    (self.motor_bl.read_current() * 1000.0) as i32,
+                    (self.motor_br.read_current() * 1000.0) as i32,
+                    (self.motor_fr.read_current() * 1000.0) as i32);
+
+                //defmt::info!("TARGET_VEL: {}, MEASURED_VEL: {}, MEASURED_CUR: {}, TARGET_TOR: {}, MEASURED_TOR: {}, COMP_TOR: {}, DC_TOR: {}, DC_VEL: {}",
+                //    (self.motor_br.read_vel_setpoint() * 1000.0) as i32,
+                //    (self.motor_br.read_rads() * 1000.0) as i32,
+                //    (self.motor_br.read_current() * 1000.0) as i32,
+                //    (self.motor_br.read_torque_setpoint() * 1000.0) as i32,
+                //    (self.motor_br.read_torque_estimate() * 1000.0) as i32,
+                //    (self.motor_br.read_torque_computed_nm() * 1000.0) as i32,
+                //    (self.motor_br.read_torque_computed_duty() * 1000.0) as i32,
+                //    (self.motor_br.read_vel_computed_duty() * 1000.0) as i32);
 
                 //defmt::info!("stspin temp: {} {} {} {}", self.motor_fl.read_mcu_temperature(), self.motor_bl.read_mcu_temperature(), self.motor_br.read_mcu_temperature(), self.motor_fr.read_mcu_temperature());
                 self.send_motor_commands_and_telemetry(
diff --git a/motor-controller/bin/dribbler/main.c b/motor-controller/bin/dribbler/main.c
index 7d85ff34..649521fe 100644
--- a/motor-controller/bin/dribbler/main.c
+++ b/motor-controller/bin/dribbler/main.c
@@ -52,7 +52,7 @@ int main() {
     bool telemetry_enabled = false;
 
     // initialize current sensing setup
-    CS_Status_t cs_status = currsen_setup(ADC_MODE, ADC_CH_MASK);
+    CS_Status_t cs_status = currsen_setup(ADC_CH_MASK);
 
     // initialize motor driver
     pwm6step_setup();
diff --git a/motor-controller/bin/dribbler/system.h b/motor-controller/bin/dribbler/system.h
index 53008618..b8835c02 100644
--- a/motor-controller/bin/dribbler/system.h
+++ b/motor-controller/bin/dribbler/system.h
@@ -23,12 +23,10 @@
 //  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)
 #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 c8de4d8d..2ff0fe49 100644
--- a/motor-controller/bin/wheel-test/main.c
+++ b/motor-controller/bin/wheel-test/main.c
@@ -62,7 +62,7 @@ int main() {
     bool telemetry_enabled = false;
 
     // initialize current sensing setup
-    CS_Status_t cs_status = currsen_setup(ADC_MODE, ADC_CH_MASK);
+    CS_Status_t cs_status = currsen_setup(ADC_CH_MASK);
 
     // initialize motor driver
     pwm6step_setup();
@@ -115,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 +134,7 @@ int main() {
     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;
@@ -287,49 +290,60 @@ int main() {
         // run the torque loop if applicable
         if (run_torque_loop) {
             // 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
+            // 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);
 
-            // Correct torque sign based on duty cycle direction
+            // 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 = -fabs(measured_torque_Nm);
+                // 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 = vel_computed_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 the torque setpoint in Nm
+            float torque_setpoint_Nm = 0.0f;
+            if (motion_control_type == TORQUE ||
+                motion_control_type == VELOCITY_W_TORQUE) {
 
-            // calculate PID on the torque in Nm
-            float torque_setpoint_Nm = pid_calculate(&torque_pid, r_Nm, measured_torque_Nm, TORQUE_LOOP_RATE_S);
+                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));
-            // convert desired current to desired duty cycle
-            u_torque_loop = mm_pos_current_to_pos_dc(&df45_model, current_setpoint, vbus_voltage);
+                // 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);
 
-            if (torque_setpoint_Nm < 0.0f) {
-                u_torque_loop = -fabs(u_torque_loop);
+                // 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_nm = torque_setpoint_Nm;
             response_packet.data.motion.torque_computed_duty = u_torque_loop;
-            response_packet.data.motion.vbus_voltage = vbus_voltage;
         }
 
         // run velocity loop if applicable
@@ -340,10 +354,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;
@@ -363,7 +380,7 @@ int main() {
             response_packet.data.motion.vel_setpoint = r_motor_board;
             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_duty = vel_computed_duty;
         }
@@ -382,6 +399,7 @@ int main() {
             } 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);
             }
 
diff --git a/motor-controller/bin/wheel-test/main.h b/motor-controller/bin/wheel-test/main.h
index aa262342..6006d9db 100644
--- a/motor-controller/bin/wheel-test/main.h
+++ b/motor-controller/bin/wheel-test/main.h
@@ -49,6 +49,8 @@
 #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  //
diff --git a/motor-controller/bin/wheel-test/system.h b/motor-controller/bin/wheel-test/system.h
index 0d5d9a17..9ac68795 100644
--- a/motor-controller/bin/wheel-test/system.h
+++ b/motor-controller/bin/wheel-test/system.h
@@ -25,9 +25,8 @@
 //  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 95b3b6d2..a88cec4e 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -13,6 +13,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <math.h>
 
 #include "ateam-common-packets/include/stspin.h"
 
@@ -43,14 +44,34 @@ 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;
+
+    // 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
@@ -58,12 +79,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
-    CS_Status_t cs_status = currsen_setup(ADC_MODE, ADC_CH_MASK);
-
     // initialize motor driver
     pwm6step_setup();
     pwm6step_set_duty_cycle_f(0.0f);
@@ -71,10 +86,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.
     MotorResponsePacket response_packet;
     memset(&response_packet, 0, sizeof(MotorResponsePacket));
@@ -87,14 +98,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 +118,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 +137,7 @@ int main() {
     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;
@@ -152,6 +158,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();
@@ -287,38 +297,55 @@ int main() {
         // run the torque loop if applicable
         if (run_torque_loop) {
             // 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
+            // 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);
 
-            // Correct torque sign based on duty cycle direction
+            // 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 = -fabs(measured_torque_Nm);
+                // 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 = vel_computed_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 the torque setpoint in Nm
+            float torque_setpoint_Nm = 0.0f;
 
-            // calculate PID on the torque in Nm
-            float torque_setpoint_Nm = pid_calculate(&torque_pid, r_Nm, measured_torque_Nm, TORQUE_LOOP_RATE_S);
+            // TODO remove OPEN_LOOP case after testing
+            if (motion_control_type == OPEN_LOOP ||
+                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, vbus_voltage);
+                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
@@ -336,14 +363,16 @@ int main() {
         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;
 
             // 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;
@@ -362,7 +391,7 @@ int main() {
             // velocity control data
             response_packet.data.motion.vel_setpoint = r_motor_board;
             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_rads = control_setpoint_vel_rads;
             response_packet.data.motion.vel_computed_duty = vel_computed_duty;
@@ -538,4 +567,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 7c34f80d..9c8c79bc 100644
--- a/motor-controller/bin/wheel/main.h
+++ b/motor-controller/bin/wheel/main.h
@@ -40,18 +40,21 @@
 #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.4f // 0.8 ohm is line to line resistance, so 0.4 ohm is line to neutral
+#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  //
 ////////////////////////
 
-#define ENCODER_IIR_TF_MS 0.20f
+#define ENCODER_IIR_TF_MS 40.0f
 #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/system.h b/motor-controller/bin/wheel/system.h
index de511127..47c4a7cf 100644
--- a/motor-controller/bin/wheel/system.h
+++ b/motor-controller/bin/wheel/system.h
@@ -25,7 +25,6 @@
 //  ADC Config  //
 //////////////////
 
-#define ADC_MODE CS_MODE_DMA
 // ADC Motor Current Unfiltered (Ch 3), Motor Current Filtered (Ch 4)
 #define ADC_CH_MASK (ADC_CHSELR_CHSEL3)
 
diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c
index 58ef804b..a7fe3713 100644
--- a/motor-controller/common/current_sensing.c
+++ b/motor-controller/common/current_sensing.c
@@ -15,13 +15,13 @@
 #include "current_sensing.h"
 #include "time.h"
 
-
 ////////////////////////////
 /// ADC Channel 4 -> PA4 ///
 ////////////////////////////
 
 static CS_Mode_t m_cs_mode;
 static ADC_Result_t m_adc_result;
+static bool m_adc_calibrated = false;
 
 void currsen_enable_ht() {
     ADC1->CR |= ADC_CR_ADSTART;
@@ -35,127 +35,61 @@ void DMA1_Channel1_IRQHandler() {
     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, uint8_t motor_adc_ch)
+CS_Status_t currsen_setup(uint8_t motor_adc_ch)
 {
-    m_cs_mode = mode;
-
     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 (m_cs_mode == CS_MODE_DMA || m_cs_mode == CS_MODE_TIMER_DMA) {
-        // 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;
-        // Incrememnt 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.
-        // TODO: Need to find DMA Enable in ST source.
-        DMA1_Channel1->CCR |= DMA_CCR_EN;
-    }
-
     // Disable ADC before configuration.
     status = currsen_adc_dis();
     if (status != CS_OK)
         return status;
 
+    // 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;
 
-    // TODO Add ADC watchdog?
-
     // 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).
@@ -191,9 +125,8 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
     // TIM1 Ch4 Falling Edge trigger
     // TIM1 -> Triggering 48 kHz
     // 1 / 48 kHz = 20.833 us period
-    // Falling edge is used because the fall of the PWM is closer to center
-    // than the rising edge. Probably doesn't actually matter because
-    // we aren't making 48 kHz control adjustments (today).
+    // 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.
@@ -283,6 +216,11 @@ CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch)
  */
 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)
     {
@@ -305,7 +243,7 @@ CS_Status_t currsen_adc_cal()
         }
     }
 
-    // Clear DMAEN
+    // Clear DMAEN just to be safe.
     ADC1->CFGR1 &= ~ADC_CFGR1_DMAEN;
     // Launch the calibration by setting ADCAL
     ADC1->CR |= ADC_CR_ADCAL;
@@ -325,10 +263,11 @@ CS_Status_t currsen_adc_cal()
         }
     }
 
+    m_adc_calibrated = true;
+
     return CS_OK;
 }
 
-
 /**
  * @brief enables the ADC
  *
@@ -345,9 +284,6 @@ CS_Status_t currsen_adc_en()
 
     // Enable the ADC
     ADC1->CR |= ADC_CR_ADEN;
-    if (m_cs_mode == CS_MODE_DMA || m_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;
@@ -364,6 +300,9 @@ CS_Status_t currsen_adc_en()
         }
     }
 
+    // Enable the DMA after the ADC is powered up.
+    ADC1->CFGR1 |= ADC_CFGR1_DMAEN;
+
     return CS_OK;
 }
 
@@ -428,7 +367,11 @@ CS_Status_t currsen_adc_dis()
  */
 float currsen_get_motor_current()
 {
-    return I_MOTOR_SCALE * ((float) m_adc_result.Motor_current_raw);
+    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;
 }
 
 /**
diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h
index 2dcfe208..148e8d6c 100644
--- a/motor-controller/common/current_sensing.h
+++ b/motor-controller/common/current_sensing.h
@@ -11,10 +11,13 @@
 
 #pragma once
 
-#define V_DDA_MV 3000.0f // mV
-#define V_DDA_V 3.0f // V
-#define V_ADC_SCALE_MV (V_DDA_MV / 4095.0f) // mV
-#define V_ADC_SCALE_V (V_DDA_V / 4095.0f) // V
+#include <math.h>
+#include <stdbool.h>
+
+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))
@@ -26,48 +29,44 @@
 // Motor Current scaling
 // In v1.0 motor-controller board, schematic based off of AN5397.
 // Rs = 2x 0.1 ohm in parallel -> 0.05 ohm
-// Rb = 2.37k
-// Ra = 140k
-// R1 = 10.0k
-// R2 = 40.2k
+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
 
-// 1. I_motor = Vs / Rs
-// 2. Vs * G_real = V_adc
-// 3. V_adc = V_adc_raw * V_ADC_SCALE
-// I_motor = ((V_adc_raw * V_ADC_SCALE) / G_real) / Rs
-// I_MOTOR_SCALE = V_ADC_SCALE / (G_real * Rs)
-// I_motor = V_adc_raw * I_MOTOR_SCALE
-#define I_MOTOR_SCALE 0.00296599486f // A/V
+// 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.
-// Vbus = V_adc_raw * VBUS_SCALE
-#define VBUS_SCALE 0.0091575f
+// V_bus = V_adc_raw * VBUS_SCALE
+static const float VBUS_SCALE = 0.0091575f;
 
 typedef enum {
-  CS_MODE_POLLING,
-  CS_MODE_DMA,
-  CS_MODE_TIMER_DMA
+    CS_MODE_PWM_DMA,
+    CS_MODE_SOFTWARE
 } CS_Mode_t;
 
 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))
@@ -127,6 +126,10 @@ 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.
 // ADC->DR reads are two bytes, DMA will do half word transfers
 // rm0091 tells us the 16->32 bit port mapping packing scheme
@@ -143,12 +146,14 @@ __attribute__((__packed__)) ADC_Result {
 
 void currsen_enable_ht();
 void currsen_read_dma();
-void currsen_read(ADC_Result_t *res);
-CS_Status_t currsen_setup(CS_Mode_t mode, uint8_t motor_adc_ch);
+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_raw();
 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 28c5037d..29f11016 100644
--- a/motor-controller/common/motor_model.c
+++ b/motor-controller/common/motor_model.c
@@ -18,6 +18,7 @@ void mm_initialize(MotorModel_t *mm) {
     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) {
@@ -50,21 +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, float vbus_voltage) {
-    // I_motor = V_motor / R_motor
-    // V_motor = duty_cycle * V_bus
-    // duty_cycle = I_motor * (R_motor / V_bus)
+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]
-    return fmax(fmin(current * (mm->line_resistance / vbus_voltage), 1.0f), 0.0f);
+    // 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 dbd7848a..b7323b8c 100644
--- a/motor-controller/common/motor_model.h
+++ b/motor-controller/common/motor_model.h
@@ -14,6 +14,7 @@ typedef struct MotorModel {
     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);
@@ -23,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, float vbus_voltage);
\ 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/software-communication b/software-communication
index 38c668a2..f420a7d6 160000
--- a/software-communication
+++ b/software-communication
@@ -1 +1 @@
-Subproject commit 38c668a2ee5800784a9406f7e4a26c03735467ff
+Subproject commit f420a7d6f523762dbe393bd0b2fc3f8ad799c3e6

From 35da67b1637a615ee834c5f1fd4f1af6dbf17151 Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Sun, 15 Jun 2025 15:47:13 -0700
Subject: [PATCH 09/20] WIP adding ADC current calibration

---
 control-board/Cargo.toml                      |   8 +-
 control-board/src/bin/calibrate-motor/main.rs | 105 ++++++
 control-board/src/bin/hwtest-motor/main.rs    |   2 +-
 control-board/src/robot_state.rs              |  10 +-
 control-board/src/stm32_interface.rs          | 126 ++++++-
 control-board/src/stspin_motor.rs             |  17 +-
 control-board/src/tasks/control_task.rs       | 308 ++++++++++++++++--
 control-board/src/tasks/imu_task.rs           |  11 +-
 motor-controller/bin/wheel-test/main.c        |   1 -
 motor-controller/bin/wheel/main.c             |  13 +-
 motor-controller/common/stm32f031xx.ld        |   3 +-
 11 files changed, 552 insertions(+), 52 deletions(-)
 create mode 100644 control-board/src/bin/calibrate-motor/main.rs

diff --git a/control-board/Cargo.toml b/control-board/Cargo.toml
index 6f8668da..ea12baa8 100644
--- a/control-board/Cargo.toml
+++ b/control-board/Cargo.toml
@@ -27,7 +27,7 @@ embassy-time = { version = "0.4.0", features = [
 embassy-stm32 = { version = "0.2.0", features = [
     "defmt",
     "stm32h723zg",
-    "unstable-pac", 
+    "unstable-pac",
     "time-driver-tim1",
     "exti",
     "chrono"
@@ -115,6 +115,11 @@ name = "hwtest-radio"
 test = false
 harness = false
 
+[[bin]]
+name = "calibrate-motor"
+test = false
+harness = false
+
 [[test]]
 name = "basic-test"
 harness = false
@@ -131,4 +136,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..377e7559
--- /dev/null
+++ b/control-board/src/bin/calibrate-motor/main.rs
@@ -0,0 +1,105 @@
+#![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, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState};
+
+use embassy_time::Timer;
+// provide embedded panic probe
+use panic_probe as _;
+use static_cell::ConstStaticCell;
+
+static ROBOT_STATE: ConstStaticCell<SharedRobotState> = ConstStaticCell::new(SharedRobotState::new());
+
+static RADIO_C2_CHANNEL: CommandsPubSub = PubSubChannel::new();
+static RADIO_TELEMETRY_CHANNEL: TelemetryPubSub = PubSubChannel::new();
+static GYRO_DATA_CHANNEL: GyroDataPubSub = PubSubChannel::new();
+static ACCEL_DATA_CHANNEL: AccelDataPubSub = PubSubChannel::new();
+static BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = PubSubChannel::new();
+
+static RADIO_UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new();
+static UART_QUEUE_EXECUTOR: InterruptExecutor = InterruptExecutor::new();
+
+#[interrupt]
+unsafe fn CEC() {
+    UART_QUEUE_EXECUTOR.on_interrupt();
+}
+
+#[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 test_command_publisher = RADIO_C2_CHANNEL.publisher().unwrap();
+
+    // telemetry channel
+    let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap();
+
+    // Battery Channel
+    let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap();
+    let battery_volt_subscriber = BATTERY_VOLT_CHANNEL.subscriber().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_accel_data_publisher = ACCEL_DATA_CHANNEL.publisher().unwrap();
+    let control_accel_data_subscriber = ACCEL_DATA_CHANNEL.subscriber().unwrap();
+
+    ///////////////////
+    //  start tasks  //
+    ///////////////////
+
+    create_io_task!(main_spawner,
+        robot_state,
+        battery_volt_publisher,
+        p);
+
+    create_imu_task!(main_spawner,
+        robot_state,
+        imu_gyro_data_publisher, imu_accel_data_publisher,
+        p);
+
+    create_motor_calibrate_task!(main_spawner, uart_queue_spawner,
+        robot_state,
+        control_command_subscriber, control_telemetry_publisher,
+        battery_volt_subscriber,
+        control_gyro_data_subscriber, control_accel_data_subscriber,
+        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 4f6c2bf9..d4146c49 100644
--- a/control-board/src/bin/hwtest-motor/main.rs
+++ b/control-board/src/bin/hwtest-motor/main.rs
@@ -10,7 +10,7 @@ use embassy_sync::pubsub::PubSubChannel;
 
 use defmt_rtt as _;
 
-use ateam_control_board::{create_control_task, create_imu_task, create_io_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState, tasks::control_task::start_control_task};
+use ateam_control_board::{create_control_task, create_imu_task, create_io_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState};
 
 use embassy_time::Timer;
 // provide embedded panic probe
diff --git a/control-board/src/robot_state.rs b/control-board/src/robot_state.rs
index ec309783..e13ec918 100644
--- a/control-board/src/robot_state.rs
+++ b/control-board/src/robot_state.rs
@@ -44,11 +44,11 @@ impl SharedRobotState {
             radio_inop: AtomicBool::new(true),
             imu_inop: AtomicBool::new(true),
             kicker_inop: AtomicBool::new(true),
-            wheels_inop: AtomicU8::new(0x0F), 
+            wheels_inop: AtomicU8::new(0x0F),
             dribbler_inop: AtomicBool::new(true),
             last_packet_receive_time_ticks_ms: AtomicU32::new(0),
-            radio_network_ok: AtomicBool::new(false), 
-            radio_bridge_ok: AtomicBool::new(false), 
+            radio_network_ok: AtomicBool::new(false),
+            radio_bridge_ok: AtomicBool::new(false),
             battery_low: AtomicBool::new(false),
             battery_crit: AtomicBool::new(false),
             robot_tipped: AtomicBool::new(false),
@@ -71,7 +71,7 @@ impl SharedRobotState {
             kicker_inop: self.get_kicker_inop(),
             wheels_inop: self.get_wheels_inop(),
             dribbler_inop: self.get_dribbler_inop(),
-        
+
             last_packet_receive_time_ticks_ms: 0,
             radio_network_ok: self.get_radio_network_ok(),
             radio_bridge_ok: self.get_radio_bridge_ok(),
@@ -118,7 +118,7 @@ impl SharedRobotState {
     pub fn set_hw_network_index(&self, ind: u8) {
         self.hw_wifi_network_index.store(ind, Ordering::Relaxed);
     }
- 
+
     pub fn hw_in_debug_mode(&self) -> bool {
         self.hw_debug_mode.load(Ordering::Relaxed)
     }
diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs
index bf62d274..17740134 100644
--- a/control-board/src/stm32_interface.rs
+++ b/control-board/src/stm32_interface.rs
@@ -332,8 +332,109 @@ 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;
+            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
     }
 
     async fn write_device_memory_chunk(&self, data: &[u8], write_base_addr: u32) -> Result<(), ()> {
@@ -536,6 +637,27 @@ impl<
             }
         };
 
+        let mut read_data = [0u8; 63];
+        if self.read_device_memory(&mut read_data, 0x0800_7C00).await.is_err() {
+            return Err(());
+        }
+        defmt::info!("read device memory: {:x}", read_data);
+
+        // TODO Implement a chunk erase.
+        //let data_write: [u8; 7] = [69, 42, 00, 1, 2, 3, 4];
+        //if self.write_device_memory_chunk(&data_write, 0x0800_7C00).await.is_err() {
+        //    return Err(());
+        //}
+        //defmt::info!("wrote device memory: {:x}", data_write);
+//
+        //// Read back the data to verify it was written correctly
+        //let mut read_data_2 = [0u8; 63];
+        //if self.read_device_memory(&mut read_data_2, 0x0800_7C00).await.is_err() {
+        //    return Err(());
+        //}
+        //defmt::info!("read back device memory: {:x}", read_data_2);
+
+
         // erase part
         if let Err(err) = self.erase_flash_memory().await {
             return Err(err);
diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs
index 1cd0a696..b2c82bdb 100644
--- a/control-board/src/stspin_motor.rs
+++ b/control-board/src/stspin_motor.rs
@@ -37,6 +37,7 @@ pub struct WheelMotor<
     >,
     firmware_image: &'a [u8],
 
+    current_timestamp_ms: u32,
     current_state: MotorResponse_Motion_Packet,
     current_params_state: MotorResponse_Params_Packet,
 
@@ -53,6 +54,7 @@ pub struct WheelMotor<
     motion_type: MotorCommand_MotionType::Type,
     reset_flagged: bool,
     telemetry_enabled: bool,
+    motion_enabled: bool,
 }
 
 impl<
@@ -87,6 +89,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),
@@ -99,6 +102,7 @@ impl<
             motion_type: OPEN_LOOP,
             reset_flagged: false,
             telemetry_enabled: false,
+            motion_enabled: false,
         }
     }
 
@@ -127,6 +131,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),
@@ -139,6 +144,7 @@ impl<
             motion_type: OPEN_LOOP,
             reset_flagged: false,
             telemetry_enabled: false,
+            motion_enabled: false,
         }
     }
 
@@ -194,7 +200,7 @@ impl<
                     *state.offset(i as isize) = buf[i];
                 }
 
-
+                self.current_timestamp_ms = mrp.timestamp;
                 // TODO probably do some checksum stuff eventually
 
                 // decode union type, and reinterpret subtype
@@ -257,6 +263,7 @@ impl<
             cmd.crc32 = 0;
             cmd.data.motion.set_reset(self.reset_flagged as u32);
             cmd.data.motion.set_enable_telemetry(self.telemetry_enabled as u32);
+            cmd.data.motion.set_enable_motion(self.motion_enabled as u32);
             cmd.data.motion.motion_control_type = self.motion_type;
             cmd.data.motion.setpoint = self.setpoint;
             //info!("setpoint: {:?}", cmd.data.motion.setpoint);
@@ -292,6 +299,14 @@ impl<
         self.telemetry_enabled = telemetry_enabled;
     }
 
+    pub fn set_motion_enabled(&mut self, enabled: bool) {
+        self.motion_enabled = enabled;
+    }
+
+    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;
     }
diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs
index 2abfc002..8a216bbe 100644
--- a/control-board/src/tasks/control_task.rs
+++ b/control-board/src/tasks/control_task.rs
@@ -24,13 +24,28 @@ static_idle_buffered_uart!(FRONT_RIGHT, MAX_RX_PACKET_SIZE, RX_BUF_DEPTH, MAX_TX
 const TICKS_WITHOUT_PACKET_STOP: usize = 20;
 const BATTERY_MIN_VOLTAGE: f32 = 18.0;
 
+// 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: MotorCommand_MotionType::Type = MotorCommand_MotionType::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,
         $battery_volt_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, $uart_queue_spawner,
@@ -42,10 +57,51 @@ macro_rules! create_control_task {
             $p.USART10, $p.PE2, $p.PE3, $p.DMA1_CH3, $p.DMA1_CH2, $p.PE5, $p.PE4,
             $p.USART6, $p.PC7, $p.PC6, $p.DMA1_CH5, $p.DMA1_CH4, $p.PG7, $p.PG8,
             $p.USART3, $p.PD9, $p.PD8, $p.DMA1_CH7, $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,
+        $battery_volt_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,
+            $battery_volt_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,
+        $battery_volt_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,
+            $battery_volt_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,
@@ -57,9 +113,12 @@ pub struct ControlTask<
     last_battery_v: f32,
     gyro_subscriber: GyroDataSubscriber,
     accel_subscriber: AccelDataSubscriber,
-    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,
     telemetry_publisher: TelemetryPublisher,
 
     motor_fl: WheelMotor<'static, MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>,
@@ -95,9 +154,12 @@ impl <
                 last_battery_v: 0.0,
                 gyro_subscriber: gyro_subscriber,
                 accel_subscriber: accel_subscriber,
-                last_gyro_rads: 0.0,
+                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,
                 motor_fl: motor_fl,
                 motor_bl: motor_bl,
                 motor_br: motor_br,
@@ -209,27 +271,46 @@ impl <
                 self.motor_fr.leave_reset(),
             ).await;
 
+            let robot_model = self.get_robot_model();
+            let mut robot_controller = BodyVelocityController::new_from_global_params(CONTROL_LOOP_RATE_S, robot_model);
+
+            let mut loop_rate_ticker = Ticker::every(Duration::from_millis(CONTROL_LOOP_RATE_MS));
+
+            let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0);
+            let mut ticks_since_control_packet = 0;
+
+            while self.shared_robot_state.get_imu_inop() {
+                defmt::info!("Waiting for IMU to be ready.");
+                loop_rate_ticker.next().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);
 
-            self.motor_fl.set_motion_type(MotorCommand_MotionType::OPEN_LOOP);
-            self.motor_bl.set_motion_type(MotorCommand_MotionType::OPEN_LOOP);
-            self.motor_br.set_motion_type(MotorCommand_MotionType::OPEN_LOOP);
-            self.motor_fr.set_motion_type(MotorCommand_MotionType::OPEN_LOOP);
-
+            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);
 
-            Timer::after_millis(10).await;
+            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);
 
-            let robot_model = self.get_robot_model();
-            let mut robot_controller = BodyVelocityController::new_from_global_params(1.0 / 100.0, robot_model);
+            // 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 loop_rate_ticker = Ticker::every(Duration::from_millis(10));
+            Timer::after_millis(10).await;
 
-            let mut cmd_vel = Vector3::new(0.0, 0.0, 0.0);
-            let mut ticks_since_control_packet = 0;
+            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();
 
             loop {
                 self.motor_fl.process_packets();
@@ -237,12 +318,38 @@ impl <
                 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;
+
                 // 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 {
@@ -284,12 +391,15 @@ impl <
                 }
 
                 while let Some(gyro_rads) = self.gyro_subscriber.try_next_message_pure() {
-                    self.last_gyro_rads = gyro_rads[2];
+                    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];
+                    self.last_accel_z_ms = accel_ms[2];
                 }
 
                 let controls_enabled = false;
@@ -297,7 +407,7 @@ impl <
                 // let kill_vel = self.shared_robot_state.get_battery_low() || self.shared_robot_state.get_battery_crit() || self.shared_robot_state.shutdown_requested();
                 let kill_vel = false;
                 let wheel_vels = if !kill_vel {
-                    self.do_control_update(&mut robot_controller, cmd_vel, self.last_gyro_rads, controls_enabled)
+                    self.do_control_update(&mut robot_controller, cmd_vel, self.last_gyro_z_rads, controls_enabled)
                 } else {
                     // Battery is too low, set velocity to zero
                     defmt::warn!("CT - low battery / shutting down command lockout");
@@ -333,6 +443,133 @@ impl <
             }
         }
 
+        async fn motor_calibrate_task_entry(&mut self) {
+            defmt::info!("Motor Calibrate task init.");
+
+            // 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;
+
+            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;
+            }
+
+            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);
+
+            // 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();
+
+            Timer::after_millis(10).await;
+
+            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];
+                }
+
+                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;
+
+                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;
+            }
+
+            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 {
+                        defmt::info!("Calibration complete. FL: {}, BL: {}, BR: {}, FR: {}",
+                            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);
+
+                        break;
+                    }
+                }
+
+                loop_rate_ticker.next().await;
+            }
+
+            defmt::info!("Setting motor current offsets.");
+            loop {
+                // TODO
+                loop_rate_ticker.next().await;
+            }
+        }
+
         async fn flash_motor_firmware(&mut self, debug: bool) {
             defmt::info!("flashing firmware");
             if debug {
@@ -430,6 +667,14 @@ async fn control_task_entry(mut control_task: ControlTask<MAX_RX_PACKET_SIZE, MA
     }
 }
 
+#[embassy_executor::task]
+async fn motor_calibrate_task_entry(mut control_task: ControlTask<MAX_RX_PACKET_SIZE, MAX_TX_PACKET_SIZE, RX_BUF_DEPTH, TX_BUF_DEPTH>) {
+    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,
@@ -443,18 +688,18 @@ pub async fn start_control_task(
     motor_bl_uart: MotorBLUart, motor_bl_rx_pin: MotorBLUartRxPin, motor_bl_tx_pin: MotorBLUartTxPin, motor_bl_rx_dma: MotorBLDmaRx, motor_bl_tx_dma: MotorBLDmaTx, motor_bl_boot0_pin: MotorBLBootPin, motor_bl_nrst_pin: MotorBLResetPin,
     motor_br_uart: MotorBRUart, motor_br_rx_pin: MotorBRUartRxPin, motor_br_tx_pin: MotorBRUartTxPin, motor_br_rx_dma: MotorBRDmaRx, motor_br_tx_dma: MotorBRDmaTx, motor_br_boot0_pin: MotorBRBootPin, motor_br_nrst_pin: MotorBRResetPin,
     motor_fr_uart: MotorFRUart, motor_fr_rx_pin: MotorFRUartRxPin, motor_fr_tx_pin: MotorFRUartTxPin, motor_fr_rx_dma: MotorFRDmaRx, motor_fr_tx_dma: MotorFRDmaTx, motor_fr_boot0_pin: MotorFRBootPin, motor_fr_nrst_pin: 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  //
     //////////////////////////
 
-    let fl_uart = Uart::new(motor_fl_uart, motor_fl_rx_pin, motor_fl_tx_pin, SystemIrqs, motor_fl_tx_dma, motor_fl_rx_dma, initial_motor_controller_uart_conifg).unwrap();
-    let bl_uart = Uart::new(motor_bl_uart, motor_bl_rx_pin, motor_bl_tx_pin, SystemIrqs, motor_bl_tx_dma, motor_bl_rx_dma, initial_motor_controller_uart_conifg).unwrap();
-    let br_uart = Uart::new(motor_br_uart, motor_br_rx_pin, motor_br_tx_pin, SystemIrqs, motor_br_tx_dma, motor_br_rx_dma, initial_motor_controller_uart_conifg).unwrap();
-    let fr_uart = Uart::new(motor_fr_uart, motor_fr_rx_pin, motor_fr_tx_pin, SystemIrqs, motor_fr_tx_dma, motor_fr_rx_dma, initial_motor_controller_uart_conifg).unwrap();
+    let fl_uart = Uart::new(motor_fl_uart, motor_fl_rx_pin, motor_fl_tx_pin, SystemIrqs, motor_fl_tx_dma, motor_fl_rx_dma, initial_motor_controller_uart_config).unwrap();
+    let bl_uart = Uart::new(motor_bl_uart, motor_bl_rx_pin, motor_bl_tx_pin, SystemIrqs, motor_bl_tx_dma, motor_bl_rx_dma, initial_motor_controller_uart_config).unwrap();
+    let br_uart = Uart::new(motor_br_uart, motor_br_rx_pin, motor_br_tx_pin, SystemIrqs, motor_br_tx_dma, motor_br_rx_dma, initial_motor_controller_uart_config).unwrap();
+    let fr_uart = Uart::new(motor_fr_uart, motor_fr_rx_pin, motor_fr_tx_pin, SystemIrqs, motor_fr_tx_dma, motor_fr_rx_dma, initial_motor_controller_uart_config).unwrap();
 
     //////////////////////////////////////////////
     //  register motor queues and DMA hardware  //
@@ -474,8 +719,8 @@ pub async fn start_control_task(
     //  create motor controllers  //
     ////////////////////////////////
 
-    let motor_fl = WheelMotor::new_from_pins(&FRONT_LEFT_IDLE_BUFFERED_UART, FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(),  FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(),  motor_fl_boot0_pin, motor_fl_nrst_pin, WHEEL_FW_IMG);
-    let motor_bl = WheelMotor::new_from_pins(&BACK_LEFT_IDLE_BUFFERED_UART, BACK_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), BACK_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(),  motor_bl_boot0_pin, motor_bl_nrst_pin, WHEEL_FW_IMG);
+    let motor_fl = WheelMotor::new_from_pins(&FRONT_LEFT_IDLE_BUFFERED_UART, FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), FRONT_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(), motor_fl_boot0_pin, motor_fl_nrst_pin, WHEEL_FW_IMG);
+    let motor_bl = WheelMotor::new_from_pins(&BACK_LEFT_IDLE_BUFFERED_UART, BACK_LEFT_IDLE_BUFFERED_UART.get_uart_read_queue(), BACK_LEFT_IDLE_BUFFERED_UART.get_uart_write_queue(), motor_bl_boot0_pin, motor_bl_nrst_pin, WHEEL_FW_IMG);
     let motor_br = WheelMotor::new_from_pins(&BACK_RIGHT_IDLE_BUFFERED_UART,  BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_read_queue(), BACK_RIGHT_IDLE_BUFFERED_UART.get_uart_write_queue(), motor_br_boot0_pin, motor_br_nrst_pin, WHEEL_FW_IMG);
     let motor_fr = WheelMotor::new_from_pins(&FRONT_RIGHT_IDLE_BUFFERED_UART, FRONT_RIGHT_IDLE_BUFFERED_UART.get_uart_read_queue(), FRONT_RIGHT_IDLE_BUFFERED_UART.get_uart_write_queue(), motor_fr_boot0_pin, motor_fr_nrst_pin, WHEEL_FW_IMG);
 
@@ -483,5 +728,12 @@ pub async fn start_control_task(
         robot_state, command_subscriber, telemetry_publisher, battery_subscriber,
         gyro_subscriber, accel_subscriber, motor_fl, motor_bl, motor_br, 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();
+    }
 }
\ No newline at end of file
diff --git a/control-board/src/tasks/imu_task.rs b/control-board/src/tasks/imu_task.rs
index e652ba57..3ff680c2 100644
--- a/control-board/src/tasks/imu_task.rs
+++ b/control-board/src/tasks/imu_task.rs
@@ -43,7 +43,7 @@ async fn imu_task_entry(
         mut gyro_int: ExtiInput<'static>) {
 
     defmt::info!("imu start startup.");
-    let mut first_tipped_check_time = Instant::now(); 
+    let mut first_tipped_check_time = Instant::now();
     let mut first_tipped_seen = false;
 
     'imu_configuration_loop:
@@ -57,7 +57,7 @@ async fn imu_task_entry(
             Timer::after_millis(1000).await;
             continue 'imu_configuration_loop;
         }
-    
+
         // configure the gyro, map int to int pin 2
         let gyro_config_res = imu.set_gyro_config(GyroMode::ContinuousHighPerformance,
             GyroRange::PlusMinus2000DegPerSec,
@@ -69,7 +69,7 @@ async fn imu_task_entry(
         if gyro_config_res.is_err() {
             defmt::error!("gyro configration failed.");
         }
-        
+
         // configure the gyro, map int to int pin 1
         let acc_config_res = imu.set_accel_config(AccelMode::ContinuousHighPerformance,
             AccelRange::Range2g,
@@ -100,7 +100,7 @@ async fn imu_task_entry(
                     // read gyro data
                     let imu_data = imu.gyro_get_data_rads().await;
                     gyro_pub.publish_immediate(Vector3::new(imu_data[0], imu_data[1], imu_data[2]));
-            
+
                     // read accel data
                     // TODO: don't use raw data, impl conversion
                     let accel_data = imu.accel_get_data_mps().await;
@@ -123,7 +123,7 @@ async fn imu_task_entry(
                             }
                         }
                     } else {
-                        // Not tipped so clear everything. 
+                        // Not tipped so clear everything.
                         first_tipped_seen = false;
                         robot_state.set_robot_tipped(false);
                     }
@@ -174,4 +174,3 @@ pub fn start_imu_task(
 
     imu_task_spawner.spawn(imu_task_entry(robot_state, gyro_data_publisher, accel_data_publisher, imu, accel_int, gyro_int)).unwrap();
 }
-
diff --git a/motor-controller/bin/wheel-test/main.c b/motor-controller/bin/wheel-test/main.c
index 2ff0fe49..129875f3 100644
--- a/motor-controller/bin/wheel-test/main.c
+++ b/motor-controller/bin/wheel-test/main.c
@@ -480,7 +480,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;
diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c
index a88cec4e..86a29db1 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -60,6 +60,7 @@ int main() {
 
     uint32_t ticks_since_last_command_packet = 0;
     bool telemetry_enabled = false;
+    bool motion_enabled = false;
 
     // Setup encoder.
     quadenc_setup();
@@ -221,6 +222,7 @@ int main() {
                 }
 
                 telemetry_enabled = motor_command_packet.data.motion.enable_telemetry;
+                motion_enabled = motor_command_packet.data.motion.enable_motion;
                 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) {
@@ -275,11 +277,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.
@@ -420,6 +423,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;
@@ -491,7 +495,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;
@@ -533,7 +536,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();
         }
 
@@ -555,7 +558,7 @@ int main() {
         }
 
         #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
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 */

From f553d4f1dfd3abfd680076506f30afdea1d8ef8f Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Sun, 15 Jun 2025 15:48:49 -0700
Subject: [PATCH 10/20] Updated software-communication

---
 software-communication | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/software-communication b/software-communication
index f420a7d6..538834c3 160000
--- a/software-communication
+++ b/software-communication
@@ -1 +1 @@
-Subproject commit f420a7d6f523762dbe393bd0b2fc3f8ad799c3e6
+Subproject commit 538834c35029d9e4e2f76f20e04387bd04e784c6

From f3b943d65fe020273cb5f69e8c58c2afa16458e2 Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Tue, 17 Jun 2025 22:32:34 -0700
Subject: [PATCH 11/20] Confirmed page erase works

---
 control-board/src/stm32_interface.rs | 166 ++++++++++++++++++++++-----
 1 file changed, 140 insertions(+), 26 deletions(-)

diff --git a/control-board/src/stm32_interface.rs b/control-board/src/stm32_interface.rs
index 17740134..79f127cd 100644
--- a/control-board/src/stm32_interface.rs
+++ b/control-board/src/stm32_interface.rs
@@ -350,6 +350,7 @@ impl<
         .write(|buf| {
             buf[0] = STM32_BOOTLOADER_CMD_READ_MEM;
             buf[1] = !STM32_BOOTLOADER_CMD_READ_MEM;
+            //defmt::info!("send buffer {:?}", buf);
             2
         })
         .await?;
@@ -437,6 +438,73 @@ impl<
         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 single page of device memory.
+    pub async fn erase_device_memory_single(&self, erase_page: u8) -> Result<(), ()> {
+        if !self.in_bootloader {
+            defmt::error!("Called bootloader operation when not in bootloader context.");
+            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");
+                }
+            }
+        }).await?;
+
+        if res.is_err() {
+            return res;
+        }
+
+        // defmt::debug!("sending the page number...");
+        self.writer
+        .write(|buf| {
+            // Quantity is N + 1, so 0x0000 for single page erase, MSB
+            buf[0] = 0x00;
+            buf[1] = 0x00;
+            // Page number is the page to erase, but never greater than a byte, MSB
+            buf[2] = 0x00;
+            buf[3] = erase_page;
+            // Checksum for all previous bytes is just the erase page number
+            buf[4] = erase_page;
+            // defmt::debug!("send buffer {:?}", buf);
+            5
+        })
+        .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
+    }
+
     async fn write_device_memory_chunk(&self, data: &[u8], write_base_addr: u32) -> Result<(), ()> {
         if !self.in_bootloader {
             defmt::error!("called bootloader operation when not in bootloader context.");
@@ -610,6 +678,23 @@ impl<
         Ok(())
     }
 
+    pub async fn erase_flash_memory_to_page(&self, last_erase_page: u8) -> Result<(), ()> {
+        if !self.in_bootloader {
+            defmt::error!("called bootloader operation when not in bootloader context.");
+            return Err(());
+        }
+
+        // Go through all pages up to the last erase page (exclusive)
+        for page in 0..last_erase_page {
+            if let Err(err) = self.erase_device_memory_single(page).await {
+                defmt::error!("failed to erase page {}: {:?}", page, err);
+                return Err(());
+            }
+        }
+
+        Ok(())
+    }
+
     pub async fn load_firmware_image(&mut self, fw_image_bytes: &[u8]) -> Result<(), ()> {
         if !self.in_bootloader {
             if let Err(err) = self.reset_into_bootloader().await {
@@ -621,45 +706,74 @@ impl<
             return Err(err);
         }
 
-        match self.get_device_id().await {
+        let device_id = 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");
-                }
-                _ => {
-                    defmt::trace!("found unknown device id {}", device_id);
-                    return Err(());
-                }
+            68 => {
+                defmt::trace!("found stm32f1 device");
+                device_id
+            }
+            19 => {
+                defmt::trace!("found stm32f40xxx device");
+                device_id
+            }
+            _ => {
+                defmt::trace!("found unknown device id {}", device_id);
+                return Err(());
+            }
             }
         };
 
-        let mut read_data = [0u8; 63];
+        let mut read_data = [0u8; 8];
         if self.read_device_memory(&mut read_data, 0x0800_7C00).await.is_err() {
             return Err(());
         }
         defmt::info!("read device memory: {:x}", read_data);
 
-        // TODO Implement a chunk erase.
-        //let data_write: [u8; 7] = [69, 42, 00, 1, 2, 3, 4];
-        //if self.write_device_memory_chunk(&data_write, 0x0800_7C00).await.is_err() {
-        //    return Err(());
-        //}
-        //defmt::info!("wrote device memory: {:x}", data_write);
-//
-        //// Read back the data to verify it was written correctly
-        //let mut read_data_2 = [0u8; 63];
-        //if self.read_device_memory(&mut read_data_2, 0x0800_7C00).await.is_err() {
-        //    return Err(());
-        //}
-        //defmt::info!("read back device memory: {:x}", read_data_2);
+        let data_write: [u8; 8] = [0x69, 0x42, 0x00, 0x51, 0x01, 0x02, 0x03, 0x04];
+        if self.write_device_memory_chunk(&data_write, 0x0800_7C00).await.is_err() {
+            return Err(());
+        }
+        defmt::info!("wrote device memory: {:x}", data_write);
+
+        // Read back the data to verify it was written correctly
+        let mut read_data_2 = [0u8; 8];
+        if self.read_device_memory(&mut read_data_2, 0x0800_7C00).await.is_err() {
+            return Err(());
+        }
+        defmt::info!("read back device memory: {:x}", read_data_2);
+
+        // Erase the page
+        let erase_page = 31; // Example page number, adjust as needed
+        if self.erase_device_memory_single(erase_page).await.is_err() {
+            return Err(());
+        }
+        defmt::info!("erased page {}", erase_page);
+
+        // Verify the page is erased
+        let mut read_data_3 = [0u8; 8];
+        if self.read_device_memory(&mut read_data_3, 0x0800_7C00).await.is_err() {
+            return Err(());
+        }
+        defmt::info!("read back device memory after erase: {:x}", read_data_3);
+
+        let data_write2: [u8; 8] = [0xAA, 0xBB, 0xCC, 0x99, 0x88, 0x77, 0x66, 0x55];
+        if self.write_device_memory_chunk(&data_write2, 0x0800_7C00).await.is_err() {
+            return Err(());
+        }
+        defmt::info!("wrote device memory after erase: {:x}", data_write2);
+
+        // Read back the data to verify it was written correctly
+        let mut read_data_4 = [0u8; 8];
+        if self.read_device_memory(&mut read_data_4, 0x0800_7C00).await.is_err() {
+            return Err(());
+        }
+        defmt::info!("read back device memory after write: {:x}", read_data_4);
 
+        // TODO account for memory size and erase size generically
 
         // erase part
-        if let Err(err) = self.erase_flash_memory().await {
+        if let Err(err) = self.erase_flash_memory_to_page(31).await {
             return Err(err);
         }
 

From 42a9aac8b537d9dd9a6223454b99320ecfdf931b Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Wed, 18 Jun 2025 18:44:48 -0700
Subject: [PATCH 12/20] Update software-communication

---
 software-communication | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/software-communication b/software-communication
index 538834c3..3e6e6e59 160000
--- a/software-communication
+++ b/software-communication
@@ -1 +1 @@
-Subproject commit 538834c35029d9e4e2f76f20e04387bd04e784c6
+Subproject commit 3e6e6e59ae8485448e2c44c3ef500fdf75a4c7d8

From df8cf40256f67ecdda99049df3616597e0cb76d9 Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Sat, 5 Jul 2025 15:07:57 -0700
Subject: [PATCH 13/20] WIP Motor flash

---
 control-board/src/stspin_motor.rs             |   6 +-
 kicker-board/src/drivers/mod.rs               |   2 +-
 lib-stm32/src/drivers/boot/stm32_interface.rs | 113 ++++++++++++------
 software-communication                        |   2 +-
 4 files changed, 79 insertions(+), 44 deletions(-)

diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs
index 76e4670d..dec4f90f 100644
--- a/control-board/src/stspin_motor.rs
+++ b/control-board/src/stspin_motor.rs
@@ -189,7 +189,7 @@ impl<
             }
         };
         let wheel_response_timeout = Duration::from_millis(100);
-    
+
         defmt::trace!("Drive Motor - waiting for parameter response packet");
         match with_timeout(wheel_response_timeout, wheel_img_hash_future).await {
             Ok(wheel_img_hash_motr) => {
@@ -212,14 +212,12 @@ impl<
         let res;
         if controller_needs_flash {
             defmt::trace!("UART config updated");
-            res = self.stm32_uart_interface.load_firmware_image(fw_image_bytes).await;
+            res = self.stm32_uart_interface.load_motor_firmware_image(fw_image_bytes).await;
         } else {
             defmt::info!("Wheel image is up to date, skipping flash");
             res = Ok(());
         }
 
-        // let res = self.stm32_uart_interface.load_firmware_image(fw_image_bytes).await;
-
         // this is safe because load firmware image call will reset the target device
         // it will begin issueing telemetry updates
         // these are the only packets it sends so any blocked process should get the data it now needs
diff --git a/kicker-board/src/drivers/mod.rs b/kicker-board/src/drivers/mod.rs
index dd80234a..36825b37 100644
--- a/kicker-board/src/drivers/mod.rs
+++ b/kicker-board/src/drivers/mod.rs
@@ -126,7 +126,7 @@ impl<
     pub async fn load_firmware_image(&mut self, fw_image_bytes: &[u8]) -> Result<(), ()> {
         let res = self
             .stm32_uart_interface
-            .load_firmware_image(fw_image_bytes)
+            .load_motor_firmware_image(fw_image_bytes)
             .await;
 
         // this is safe because load firmware image call will reset the target device
diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs
index b4cc3d25..85b3792c 100644
--- a/lib-stm32/src/drivers/boot/stm32_interface.rs
+++ b/lib-stm32/src/drivers/boot/stm32_interface.rs
@@ -29,6 +29,9 @@ 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 fn get_bootloader_uart_config() -> Config {
     let mut config = usart::Config::default();
     config.baudrate = 115_200; // max officially support baudrate
@@ -730,55 +733,52 @@ impl<
             }
         };
 
-        let mut read_data = [0u8; 8];
-        if self.read_device_memory(&mut read_data, 0x0800_7C00).await.is_err() {
-            return Err(());
-        }
-        defmt::info!("read device memory: {:x}", read_data);
-
-        let data_write: [u8; 8] = [0x69, 0x42, 0x00, 0x51, 0x01, 0x02, 0x03, 0x04];
-        if self.write_device_memory_chunk(&data_write, 0x0800_7C00).await.is_err() {
-            return Err(());
+        // erase part
+        if let Err(err) = self.erase_flash_memory().await {
+            return Err(err);
         }
-        defmt::info!("wrote device memory: {:x}", data_write);
 
-        // Read back the data to verify it was written correctly
-        let mut read_data_2 = [0u8; 8];
-        if self.read_device_memory(&mut read_data_2, 0x0800_7C00).await.is_err() {
-            return Err(());
+        // program image
+        if let Err(err) = self.write_device_memory(fw_image_bytes, None).await {
+            return Err(err);
         }
-        defmt::info!("read back device memory: {:x}", read_data_2);
 
-        // Erase the page
-        let erase_page = 31; // Example page number, adjust as needed
-        if self.erase_device_memory_single(erase_page).await.is_err() {
-            return Err(());
-        }
-        defmt::info!("erased page {}", erase_page);
+        self.reset_into_program(false).await;
 
-        // Verify the page is erased
-        let mut read_data_3 = [0u8; 8];
-        if self.read_device_memory(&mut read_data_3, 0x0800_7C00).await.is_err() {
-            return Err(());
-        }
-        defmt::info!("read back device memory after erase: {:x}", read_data_3);
+        Ok(())
+    }
 
-        let data_write2: [u8; 8] = [0xAA, 0xBB, 0xCC, 0x99, 0x88, 0x77, 0x66, 0x55];
-        if self.write_device_memory_chunk(&data_write2, 0x0800_7C00).await.is_err() {
-            return Err(());
+    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);
+            }
         }
-        defmt::info!("wrote device memory after erase: {:x}", data_write2);
 
-        // Read back the data to verify it was written correctly
-        let mut read_data_4 = [0u8; 8];
-        if self.read_device_memory(&mut read_data_4, 0x0800_7C00).await.is_err() {
-            return Err(());
+        if let Err(err) = self.verify_bootloader().await {
+            return Err(err);
         }
-        defmt::info!("read back device memory after write: {:x}", read_data_4);
 
-        // TODO account for memory size and erase size generically
+        let device_id = 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 part
+        // Erase up to Page 31 since the last page is used for current calibration constants.
         if let Err(err) = self.erase_flash_memory_to_page(31).await {
             return Err(err);
         }
@@ -793,6 +793,43 @@ impl<
         Ok(())
     }
 
+    pub async fn write_current_calibration_constants(&mut self, constants: &[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);
+        }
+
+        let device_id = match self.get_device_id().await {
+            Err(err) => return Err(err),
+            Ok(device_id) => match device_id {
+                19 => {
+                    defmt::trace!("found stm32f40xxx 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(31).await {
+            return Err(err);
+        }
+
+        // Write the constants to the last page
+        if let Err(err) = self.write_device_memory(constants, Some(MOTOR_CURRENT_START_ADDRESS)).await {
+            return Err(err);
+        }
+
+        Ok(())
+    }
+
     pub async fn execute_code(&mut self, start_address: Option<u32>) -> Result<(), ()> {
         defmt::debug!("firmware jump/go command implementation not working. will reset part.");
         self.reset_into_program(false).await;
diff --git a/software-communication b/software-communication
index 0719fbfb..991dba44 160000
--- a/software-communication
+++ b/software-communication
@@ -1 +1 @@
-Subproject commit 0719fbfb49ee13165027bdf2fa2684283bb1c953
+Subproject commit 991dba4423e889d74571fb4170fd7b658136f95f

From 907e5585ef56cf702b9dd7d480a965e1f2cb17df Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Sat, 5 Jul 2025 19:14:04 -0700
Subject: [PATCH 14/20] WIP current offset finished???

---
 control-board/src/bin/calibrate-motor/main.rs | 42 +++++++---
 control-board/src/bin/control/main.rs         |  6 +-
 control-board/src/stspin_motor.rs             | 13 +++
 control-board/src/tasks/control_task.rs       | 83 +++++++++++++++----
 control-board/src/tasks/radio_task.rs         |  2 +-
 lib-stm32/src/drivers/boot/stm32_interface.rs | 24 ++++--
 motor-controller/bin/wheel/main.c             | 18 ++--
 motor-controller/common/current_sensing.c     | 37 +++++++++
 motor-controller/common/current_sensing.h     |  6 +-
 software-communication                        |  2 +-
 10 files changed, 186 insertions(+), 47 deletions(-)

diff --git a/control-board/src/bin/calibrate-motor/main.rs b/control-board/src/bin/calibrate-motor/main.rs
index 377e7559..1a4e7d69 100644
--- a/control-board/src/bin/calibrate-motor/main.rs
+++ b/control-board/src/bin/calibrate-motor/main.rs
@@ -9,7 +9,7 @@ use embassy_sync::pubsub::PubSubChannel;
 
 use defmt_rtt as _;
 
-use ateam_control_board::{create_motor_calibrate_task, create_imu_task, create_io_task, get_system_config, pins::{AccelDataPubSub, BatteryVoltPubSub, CommandsPubSub, GyroDataPubSub, TelemetryPubSub}, robot_state::SharedRobotState};
+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
@@ -22,16 +22,20 @@ 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 BATTERY_VOLT_CHANNEL: BatteryVoltPubSub = 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();
@@ -62,43 +66,61 @@ async fn main(main_spawner: embassy_executor::Spawner) {
 
     // commands channel
     let control_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap();
-    let test_command_publisher = RADIO_C2_CHANNEL.publisher().unwrap();
+    let kicker_command_subscriber = RADIO_C2_CHANNEL.subscriber().unwrap();
 
     // telemetry channel
     let control_telemetry_publisher = RADIO_TELEMETRY_CHANNEL.publisher().unwrap();
 
-    // Battery Channel
-    let battery_volt_publisher = BATTERY_VOLT_CHANNEL.publisher().unwrap();
-    let battery_volt_subscriber = BATTERY_VOLT_CHANNEL.subscriber().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,
-        battery_volt_publisher,
+        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_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,
-        battery_volt_subscriber,
+        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;
     }
diff --git a/control-board/src/bin/control/main.rs b/control-board/src/bin/control/main.rs
index dc08d9c7..d37d479b 100644
--- a/control-board/src/bin/control/main.rs
+++ b/control-board/src/bin/control/main.rs
@@ -7,7 +7,7 @@ use embassy_stm32::{
 };
 use embassy_sync::pubsub::PubSubChannel;
 
-use defmt_rtt as _; 
+use defmt_rtt as _;
 
 use ateam_control_board::{
     create_audio_task, create_control_task, create_dotstar_task, create_imu_task, create_io_task, create_kicker_task, create_power_task, create_radio_task, get_system_config, pins::{AccelDataPubSub, CommandsPubSub, GyroDataPubSub, KickerTelemetryPubSub, LedCommandPubSub, PowerTelemetryPubSub, TelemetryPubSub}, robot_state::SharedRobotState};
@@ -144,8 +144,8 @@ async fn main(main_spawner: embassy_executor::Spawner) {
         imu_gyro_data_publisher, imu_accel_data_publisher, imu_led_cmd_publisher,
         p);
 
-    create_control_task!(main_spawner, uart_queue_spawner, 
-        robot_state, 
+    create_control_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,
diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs
index 2069cc6a..a07cca2d 100644
--- a/control-board/src/stspin_motor.rs
+++ b/control-board/src/stspin_motor.rs
@@ -46,6 +46,7 @@ pub struct WheelMotor<
     reset_flagged: bool,
     telemetry_enabled: bool,
     motion_enabled: bool,
+    calibrate_current: bool,
 }
 
 impl<
@@ -91,6 +92,7 @@ impl<
             reset_flagged: false,
             telemetry_enabled: false,
             motion_enabled: false,
+            calibrate_current: false,
         }
     }
 
@@ -130,6 +132,7 @@ impl<
             reset_flagged: false,
             telemetry_enabled: false,
             motion_enabled: false,
+            calibrate_current: false,
         }
     }
 
@@ -217,6 +220,11 @@ impl<
         return self.load_firmware_image(self.firmware_image).await;
     }
 
+    pub async fn save_motor_current_constants(&mut self, current_constant: f32) -> Result<(), ()> {
+        defmt::debug!("Drive Motor - Saving motor current constant: {:?}", current_constant);
+        self.stm32_uart_interface.write_current_calibration_constants(current_constant).await
+    }
+
     pub fn process_packets(&mut self) {
         while let Ok(res) = self.stm32_uart_interface.try_read_data() {
             let buf = res.data();
@@ -324,6 +332,7 @@ impl<
             cmd.data.motion.set_reset(self.reset_flagged as u32);
             cmd.data.motion.set_enable_telemetry(self.telemetry_enabled as u32);
             cmd.data.motion.set_enable_motion(self.motion_enabled as u32);
+            cmd.data.motion.set_calibrate_current(self.calibrate_current as u32);
             cmd.data.motion.motion_control_type = self.motion_type;
             cmd.data.motion.setpoint = self.setpoint;
             //info!("setpoint: {:?}", cmd.data.motion.setpoint);
@@ -363,6 +372,10 @@ impl<
         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;
     }
diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs
index 2aa047e1..af60d502 100644
--- a/control-board/src/tasks/control_task.rs
+++ b/control-board/src/tasks/control_task.rs
@@ -35,7 +35,7 @@ const CURRENT_CALIBRATION_SAMPLES: usize = 1000; // number of samples to take fo
 
 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: MotorCommand_MotionType::Type = MotorCommand_MotionType::OPEN_LOOP;
+const CONTROL_MOTION_TYPE: MotionCommandType::Type = MotionCommandType::OPEN_LOOP;
 
 // Internal macro with do_motor_calibrate as a parameter
 #[macro_export]
@@ -45,14 +45,15 @@ macro_rules! __create_control_task_internal {
         $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, $do_motor_calibrate:expr
+        $p:ident,
+        $do_motor_calibrate:expr
     ) => {
         ateam_control_board::tasks::control_task::start_control_task(
             $main_spawner, $uart_queue_spawner,
             $robot_state,
             $control_command_subscriber, $control_telemetry_publisher,
-            $control_gyro_data_subscriber, $control_accel_data_subscriber,
             $power_telemetry_subscriber, $kicker_telemetry_subscriber,
+            $control_gyro_data_subscriber, $control_accel_data_subscriber,
             $p.UART7, $p.PF6, $p.PF7, $p.DMA1_CH1, $p.DMA1_CH0, $p.PF5, $p.PF4,
             $p.USART10, $p.PE2, $p.PE3, $p.DMA1_CH3, $p.DMA1_CH2, $p.PE5, $p.PE4,
             $p.USART6, $p.PC7, $p.PC6, $p.DMA1_CH5, $p.DMA1_CH4, $p.PG7, $p.PG8,
@@ -68,14 +69,14 @@ macro_rules! create_control_task {
     (
         $main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident,
         $control_command_subscriber:ident, $control_telemetry_publisher:ident,
-        $battery_volt_subscriber: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,
-            $battery_volt_subscriber,
+            $power_telemetry_subscriber, $kicker_telemetry_subscriber,
             $control_gyro_data_subscriber, $control_accel_data_subscriber,
             $p, false
         );
@@ -88,14 +89,14 @@ macro_rules! create_motor_calibrate_task {
     (
         $main_spawner:ident, $uart_queue_spawner:ident, $robot_state:ident,
         $control_command_subscriber:ident, $control_telemetry_publisher:ident,
-        $battery_volt_subscriber: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,
-            $battery_volt_subscriber,
+            $power_telemetry_subscriber, $kicker_telemetry_subscriber,
             $control_gyro_data_subscriber, $control_accel_data_subscriber,
             $p, true
         );
@@ -421,7 +422,7 @@ impl <
                             }
 
                             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, true) => MotionCommandType::VELOCITY_W_TORQUE,
                                 (true, false) => MotionCommandType::VELOCITY,
                                 (false, true) => MotionCommandType::TORQUE,
                                 (false, false) => MotionCommandType::OPEN_LOOP,
@@ -488,7 +489,7 @@ impl <
                     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.do_control_update(&mut robot_controller, cmd_vel, self.last_gyro_z_rads, controls_enabled)
                 };
 
                 self.motor_fl.set_setpoint(wheel_vels[0]);
@@ -497,8 +498,8 @@ impl <
                 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: {} {} {} {}", 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  //
@@ -545,6 +546,11 @@ impl <
             self.motor_br.set_telemetry_enabled(true);
             self.motor_fr.set_telemetry_enabled(true);
 
+            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);
+
             // Need to send one motion command to get telemetry started.
             self.motor_fl.send_motion_command();
             self.motor_bl.send_motion_command();
@@ -622,11 +628,16 @@ impl <
                     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_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);
+                            motor_fl_current_offset,
+                            motor_bl_current_offset,
+                            motor_br_current_offset,
+                            motor_fr_current_offset);
 
                         break;
                     }
@@ -636,8 +647,44 @@ impl <
             }
 
             defmt::info!("Setting motor current offsets.");
+            let mut motor_constant_error = false;
+
+
+            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 motor current constants saved.");
+            }
+
+            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 motor current constants saved.");
+            }
+
+            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 motor current constants saved.");
+            }
+
+            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 motor current constants saved.");
+            }
+
+            if motor_constant_error {
+                defmt::error!("One or more motors failed to save current constants. Run again!");
+            } else {
+                defmt::info!("All motors current constants saved.");
+            }
+
             loop {
-                // TODO
                 loop_rate_ticker.next().await;
             }
         }
@@ -761,10 +808,10 @@ pub async fn start_control_task(
     robot_state: &'static SharedRobotState,
     command_subscriber: CommandsSubscriber,
     telemetry_publisher: TelemetryPublisher,
-    gyro_subscriber: GyroDataSubscriber,
-    accel_subscriber: AccelDataSubscriber,
     power_telemetry_subscriber: PowerTelemetrySubscriber,
     kicker_telemetry_subscriber: KickerTelemetrySubscriber,
+    gyro_subscriber: GyroDataSubscriber,
+    accel_subscriber: AccelDataSubscriber,
     motor_fl_uart: MotorFLUart, motor_fl_rx_pin: MotorFLUartRxPin, motor_fl_tx_pin: MotorFLUartTxPin, motor_fl_rx_dma: MotorFLDmaRx, motor_fl_tx_dma: MotorFLDmaTx, motor_fl_boot0_pin: MotorFLBootPin, motor_fl_nrst_pin: MotorFLResetPin,
     motor_bl_uart: MotorBLUart, motor_bl_rx_pin: MotorBLUartRxPin, motor_bl_tx_pin: MotorBLUartTxPin, motor_bl_rx_dma: MotorBLDmaRx, motor_bl_tx_dma: MotorBLDmaTx, motor_bl_boot0_pin: MotorBLBootPin, motor_bl_nrst_pin: MotorBLResetPin,
     motor_br_uart: MotorBRUart, motor_br_rx_pin: MotorBRUartRxPin, motor_br_tx_pin: MotorBRUartTxPin, motor_br_rx_dma: MotorBRDmaRx, motor_br_tx_dma: MotorBRDmaTx, motor_br_boot0_pin: MotorBRBootPin, motor_br_nrst_pin: MotorBRResetPin,
diff --git a/control-board/src/tasks/radio_task.rs b/control-board/src/tasks/radio_task.rs
index 7ee7b820..75d25f0a 100644
--- a/control-board/src/tasks/radio_task.rs
+++ b/control-board/src/tasks/radio_task.rs
@@ -1,5 +1,5 @@
 
-use ateam_common_packets::{bindings::BasicTelemetry, radio::TelemetryPacket};
+use ateam_common_packets::{bindings::BasicTelemetry, radio::TelemetryPacket, bindings::RadioPacket };
 use ateam_lib_stm32::{idle_buffered_uart_read_task, idle_buffered_uart_write_task, static_idle_buffered_uart, uart::queue::{IdleBufferedUart, UartReadQueue, UartWriteQueue}};
 use credentials::WifiCredential;
 use embassy_executor::{SendSpawner, Spawner};
diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs
index 85b3792c..049efb17 100644
--- a/lib-stm32/src/drivers/boot/stm32_interface.rs
+++ b/lib-stm32/src/drivers/boot/stm32_interface.rs
@@ -31,6 +31,8 @@ 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();
@@ -714,7 +716,7 @@ impl<
             return Err(err);
         }
 
-        let device_id = match self.get_device_id().await {
+        match self.get_device_id().await {
             Err(err) => return Err(err),
             Ok(device_id) => match device_id {
                 68 => {
@@ -759,7 +761,7 @@ impl<
             return Err(err);
         }
 
-        let device_id = match self.get_device_id().await {
+        match self.get_device_id().await {
             Err(err) => return Err(err),
             Ok(device_id) => match device_id {
                 68 => {
@@ -779,7 +781,7 @@ impl<
         };
 
         // Erase up to Page 31 since the last page is used for current calibration constants.
-        if let Err(err) = self.erase_flash_memory_to_page(31).await {
+        if let Err(err) = self.erase_flash_memory_to_page(MOTOR_CURRENT_PAGE).await {
             return Err(err);
         }
 
@@ -793,7 +795,7 @@ impl<
         Ok(())
     }
 
-    pub async fn write_current_calibration_constants(&mut self, constants: &[u8]) -> Result<(), ()> {
+    pub async fn write_current_calibration_constants(&mut self, current_constant: f32) -> Result<(), ()> {
         if !self.in_bootloader {
             if let Err(err) = self.reset_into_bootloader().await {
                 return Err(err);
@@ -804,7 +806,7 @@ impl<
             return Err(err);
         }
 
-        let device_id = match self.get_device_id().await {
+        match self.get_device_id().await {
             Err(err) => return Err(err),
             Ok(device_id) => match device_id {
                 19 => {
@@ -818,12 +820,18 @@ impl<
         };
 
         // Erase the last page
-        if let Err(err) = self.erase_device_memory_single(31).await {
+        if let Err(err) = self.erase_device_memory_single(MOTOR_CURRENT_PAGE).await {
             return Err(err);
         }
 
-        // Write the constants to the last page
-        if let Err(err) = self.write_device_memory(constants, Some(MOTOR_CURRENT_START_ADDRESS)).await {
+        // 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(&current_bytes);
+
+        if let Err(err) = self.write_device_memory(&data_to_write, Some(MOTOR_CURRENT_START_ADDRESS)).await {
             return Err(err);
         }
 
diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c
index ba93a009..0b33ab2f 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -335,7 +335,15 @@ int main() {
         if (run_torque_loop) {
             // 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 current_sense_I = 0.0f;
+            if (motor_command_packet.data.motion.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);
@@ -388,7 +396,8 @@ int main() {
             // load data frame
             // torque control data
             response_packet.data.motion.torque_setpoint = r_Nm;
-            response_packet.data.motion.vbus_voltage = vbus_voltage;
+            // 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;
@@ -409,7 +418,7 @@ int main() {
             vel_target_accel_rads2 = (r_motor_board - enc_vel_rads_filt) / VELOCITY_LOOP_RATE_S;
 
             // 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;
@@ -432,7 +441,7 @@ int main() {
             // velocity control data
             response_packet.data.motion.vel_setpoint = r_motor_board;
             response_packet.data.motion.encoder_delta = enc_vel_rads;
-            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_rads = control_setpoint_vel_rads;
             response_packet.data.motion.vel_computed_duty = vel_computed_duty;
@@ -535,7 +544,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;
diff --git a/motor-controller/common/current_sensing.c b/motor-controller/common/current_sensing.c
index a7fe3713..44811226 100644
--- a/motor-controller/common/current_sensing.c
+++ b/motor-controller/common/current_sensing.c
@@ -23,6 +23,8 @@ 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;
 }
@@ -374,6 +376,28 @@ float currsen_get_motor_current()
     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.
@@ -406,3 +430,16 @@ int32_t currsen_get_temperature()
     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 148e8d6c..f998ffa6 100644
--- a/motor-controller/common/current_sensing.h
+++ b/motor-controller/common/current_sensing.h
@@ -56,6 +56,10 @@ static const float V_MIN_OP_AMP = 0.2f; // V
 // 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 = 0x0800_7C00;
+static const uint32_t CURRENT_OFFSET_MAGIC = 0xAABBCCDD;
+
 typedef enum {
     CS_MODE_PWM_DMA,
     CS_MODE_SOFTWARE
@@ -153,7 +157,7 @@ CS_Status_t currsen_adc_dis();
 CS_Status_t calculate_motor_zero_current_setpoint();
 
 float currsen_get_motor_current();
-float currsen_get_motor_current_raw();
+float currsen_get_motor_current_with_offset();
 float currsen_get_vbus_voltage();
 int32_t currsen_get_temperature();
 uint16_t currsen_get_motor_current_zero_adc_raw();
diff --git a/software-communication b/software-communication
index 991dba44..2ff637e5 160000
--- a/software-communication
+++ b/software-communication
@@ -1 +1 @@
-Subproject commit 991dba4423e889d74571fb4170fd7b658136f95f
+Subproject commit 2ff637e5218c5e783d2a62302f1f614e13c19647

From 2a0f1f56b8885852592e35c6b58b7cef2d2612ef Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Sun, 6 Jul 2025 19:22:22 -0700
Subject: [PATCH 15/20] Add additional for calibrate

---
 control-board/src/stspin_motor.rs             | 26 ++++---
 control-board/src/tasks/control_task.rs       | 24 +++---
 lib-stm32/src/drivers/boot/stm32_interface.rs | 76 +++++++++++--------
 motor-controller/bin/wheel/main.c             |  4 +-
 motor-controller/common/current_sensing.h     |  3 +-
 5 files changed, 78 insertions(+), 55 deletions(-)

diff --git a/control-board/src/stspin_motor.rs b/control-board/src/stspin_motor.rs
index a07cca2d..4e945c28 100644
--- a/control-board/src/stspin_motor.rs
+++ b/control-board/src/stspin_motor.rs
@@ -193,17 +193,23 @@ impl<
         return wheel_needs_flash;
     }
 
-    pub async fn load_firmware_image(&mut self, fw_image_bytes: &[u8]) -> Result<(), ()> {
-        let controller_needs_flash: bool = self.check_wheel_needs_flash().await;
-        defmt::debug!("Motor Controller Needs Flash - {:?}", controller_needs_flash);
-
+    pub async fn load_firmware_image(&mut self, debug_switch: bool, fw_image_bytes: &[u8]) -> Result<(), ()> {
         let res;
-        if controller_needs_flash {
-            defmt::trace!("UART config updated");
+        if debug_switch {
+            defmt::info!("Drive Motor - Flashing firmware image in debug mode");
             res = self.stm32_uart_interface.load_motor_firmware_image(fw_image_bytes).await;
         } else {
-            defmt::info!("Wheel image is up to date, skipping flash");
-            res = Ok(());
+            defmt::info!("Drive Motor - Will check if firmware image needs flashing");
+            let controller_needs_flash: bool = self.check_wheel_needs_flash().await;
+            defmt::debug!("Motor Controller Needs Flash - {:?}", controller_needs_flash);
+
+            if controller_needs_flash {
+                defmt::trace!("UART config updated");
+                res = self.stm32_uart_interface.load_motor_firmware_image(fw_image_bytes).await;
+            } else {
+                defmt::info!("Wheel image is up to date, skipping flash");
+                res = Ok(());
+            }
         }
 
         // this is safe because load firmware image call will reset the target device
@@ -216,8 +222,8 @@ impl<
         return res;
     }
 
-    pub async fn load_default_firmware_image(&mut self) -> Result<(), ()> {
-        return self.load_firmware_image(self.firmware_image).await;
+    pub async fn load_default_firmware_image(&mut self, debug_switch: bool) -> Result<(), ()> {
+        return self.load_firmware_image(debug_switch, self.firmware_image).await;
     }
 
     pub async fn save_motor_current_constants(&mut self, current_constant: f32) -> Result<(), ()> {
diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs
index af60d502..c057629d 100644
--- a/control-board/src/tasks/control_task.rs
+++ b/control-board/src/tasks/control_task.rs
@@ -158,8 +158,8 @@ impl <
                 telemetry_publisher: telemetry_publisher,
                 gyro_subscriber: gyro_subscriber,
                 accel_subscriber: accel_subscriber,
-                power_telemetry_subscriber,
-                kicker_telemetry_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,
@@ -344,14 +344,14 @@ impl <
             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();
 
-            Timer::after_millis(10).await;
-
             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();
@@ -693,28 +693,28 @@ impl <
             defmt::info!("flashing firmware");
             if debug {
                 let mut had_motor_error = false;
-                if self.motor_fl.load_default_firmware_image().await.is_err() {
+                if self.motor_fl.load_default_firmware_image(debug).await.is_err() {
                     defmt::error!("failed to flash FL");
                     had_motor_error = true;
                 } else {
                     defmt::info!("FL flashed");
                 }
 
-                if self.motor_bl.load_default_firmware_image().await.is_err() {
+                if self.motor_bl.load_default_firmware_image(debug).await.is_err() {
                     defmt::error!("failed to flash BL");
                     had_motor_error = true;
                 } else {
                     defmt::info!("BL flashed");
                 }
 
-                if self.motor_br.load_default_firmware_image().await.is_err() {
+                if self.motor_br.load_default_firmware_image(debug).await.is_err() {
                     defmt::error!("failed to flash BR");
                     had_motor_error = true;
                 } else {
                     defmt::info!("BR flashed");
                 }
 
-                if self.motor_fr.load_default_firmware_image().await.is_err() {
+                if self.motor_fr.load_default_firmware_image(debug).await.is_err() {
                     defmt::error!("failed to flash FR");
                     had_motor_error = true;
                 } else {
@@ -728,10 +728,10 @@ impl <
                 }
             } else {
                 let res = embassy_futures::join::join4(
-                    self.motor_fl.load_default_firmware_image(),
-                    self.motor_bl.load_default_firmware_image(),
-                    self.motor_br.load_default_firmware_image(),
-                    self.motor_fr.load_default_firmware_image(),
+                    self.motor_fl.load_default_firmware_image(debug),
+                    self.motor_bl.load_default_firmware_image(debug),
+                    self.motor_br.load_default_firmware_image(debug),
+                    self.motor_fr.load_default_firmware_image(debug),
                 )
                 .await;
 
diff --git a/lib-stm32/src/drivers/boot/stm32_interface.rs b/lib-stm32/src/drivers/boot/stm32_interface.rs
index 049efb17..b8656c8b 100644
--- a/lib-stm32/src/drivers/boot/stm32_interface.rs
+++ b/lib-stm32/src/drivers/boot/stm32_interface.rs
@@ -189,7 +189,10 @@ impl<
                 } else {
                     defmt::debug!("bootloader replied with NACK after calibration.");
                 }
+            } else {
+                defmt::debug!("bootloader reply too short after calibration.");
             }
+            res
         })).await;
 
         if sync_res.is_err() {
@@ -446,13 +449,18 @@ impl<
     // 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 single page of device memory.
-    pub async fn erase_device_memory_single(&self, erase_page: u8) -> Result<(), ()> {
+    // 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| {
@@ -471,6 +479,8 @@ impl<
                 } else {
                     defmt::error!("Bootloader replied to erase command with NACK");
                 }
+            } else {
+                defmt::error!("Erase command reply too short.");
             }
         }).await?;
 
@@ -481,16 +491,27 @@ impl<
         // defmt::debug!("sending the page number...");
         self.writer
         .write(|buf| {
-            // Quantity is N + 1, so 0x0000 for single page erase, MSB
+            // 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] = 0x00;
-            // Page number is the page to erase, but never greater than a byte, MSB
-            buf[2] = 0x00;
-            buf[3] = erase_page;
+            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[4] = erase_page;
+            buf[buf_indx] = checksum;
             // defmt::debug!("send buffer {:?}", buf);
-            5
+            // Final size is buf_indx + 1 from checksum byte
+            buf_indx + 1
         })
         .await?;
 
@@ -510,6 +531,11 @@ impl<
         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<(), ()> {
         if !self.in_bootloader {
             defmt::error!("called bootloader operation when not in bootloader context.");
@@ -611,7 +637,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);
@@ -688,23 +714,6 @@ impl<
         Ok(())
     }
 
-    pub async fn erase_flash_memory_to_page(&self, last_erase_page: u8) -> Result<(), ()> {
-        if !self.in_bootloader {
-            defmt::error!("called bootloader operation when not in bootloader context.");
-            return Err(());
-        }
-
-        // Go through all pages up to the last erase page (exclusive)
-        for page in 0..last_erase_page {
-            if let Err(err) = self.erase_device_memory_single(page).await {
-                defmt::error!("failed to erase page {}: {:?}", page, err);
-                return Err(());
-            }
-        }
-
-        Ok(())
-    }
-
     pub async fn load_firmware_image(&mut self, fw_image_bytes: &[u8]) -> Result<(), ()> {
         if !self.in_bootloader {
             if let Err(err) = self.reset_into_bootloader().await {
@@ -781,7 +790,12 @@ impl<
         };
 
         // Erase up to Page 31 since the last page is used for current calibration constants.
-        if let Err(err) = self.erase_flash_memory_to_page(MOTOR_CURRENT_PAGE).await {
+        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);
         }
 
@@ -790,7 +804,7 @@ impl<
             return Err(err);
         }
 
-        self.reset_into_program(false).await;
+        self.reset_into_program(true).await;
 
         Ok(())
     }
@@ -809,8 +823,8 @@ impl<
         match self.get_device_id().await {
             Err(err) => return Err(err),
             Ok(device_id) => match device_id {
-                19 => {
-                    defmt::trace!("found stm32f40xxx device");
+                68 => {
+                    defmt::trace!("found STSPINF0 device");
                 }
                 _ => {
                     defmt::error!("Invalid device id for current calibration constants {}", device_id);
diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c
index 0b33ab2f..faacc3fd 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -62,6 +62,7 @@ int main() {
     uint32_t ticks_since_last_command_packet = 0;
     bool telemetry_enabled = false;
     bool motion_enabled = false;
+    bool calibrate_current = false;
 
     // Setup encoder.
     quadenc_setup();
@@ -255,6 +256,7 @@ 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) {
@@ -336,7 +338,7 @@ int main() {
             // 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 (motor_command_packet.data.motion.calibrate_current) {
+            if (calibrate_current) {
                 // Just return the current with no offset applied.
                 current_sense_I = currsen_get_motor_current();
             }
diff --git a/motor-controller/common/current_sensing.h b/motor-controller/common/current_sensing.h
index f998ffa6..b17ef172 100644
--- a/motor-controller/common/current_sensing.h
+++ b/motor-controller/common/current_sensing.h
@@ -57,7 +57,7 @@ static const float V_MIN_OP_AMP = 0.2f; // V
 static const float VBUS_SCALE = 0.0091575f;
 
 // TODO Move to software-communication
-static const uint32_t CURRENT_OFFSET_ADDRESS = 0x0800_7C00;
+static const uint32_t CURRENT_OFFSET_ADDRESS = 0x08007C00;
 static const uint32_t CURRENT_OFFSET_MAGIC = 0xAABBCCDD;
 
 typedef enum {
@@ -158,6 +158,7 @@ CS_Status_t calculate_motor_zero_current_setpoint();
 
 float currsen_get_motor_current();
 float currsen_get_motor_current_with_offset();
+float currsen_get_motor_current_offset();
 float currsen_get_vbus_voltage();
 int32_t currsen_get_temperature();
 uint16_t currsen_get_motor_current_zero_adc_raw();

From eafd768079fe4cba197100e138b20a31866f936f Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Fri, 18 Jul 2025 14:22:27 -0400
Subject: [PATCH 16/20] Update software-communication

---
 software-communication | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/software-communication b/software-communication
index 0fc77eee..2ff637e5 160000
--- a/software-communication
+++ b/software-communication
@@ -1 +1 @@
-Subproject commit 0fc77eee7b20b4113e452816be8dd7bc42609f21
+Subproject commit 2ff637e5218c5e783d2a62302f1f614e13c19647

From a5d32f720e8610d3ffea9b842f0a56c2a88e9ea7 Mon Sep 17 00:00:00 2001
From: Joe Spall <jspall0327@gmail.com>
Date: Fri, 18 Jul 2025 14:25:30 -0400
Subject: [PATCH 17/20] Update software-communication again

---
 software-communication | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/software-communication b/software-communication
index 2ff637e5..2c8ddaae 160000
--- a/software-communication
+++ b/software-communication
@@ -1 +1 @@
-Subproject commit 2ff637e5218c5e783d2a62302f1f614e13c19647
+Subproject commit 2c8ddaae90c537f99490d519e97ec158700e25ff

From 99b858797a2f019f9e3cbf2a9dca1fc6d6f48932 Mon Sep 17 00:00:00 2001
From: Will Stuckey <guyfleeman@gmail.com>
Date: Tue, 2 Sep 2025 20:58:24 -0400
Subject: [PATCH 18/20] uart test cleanup

---
 control-board/.cargo/config.toml           |  2 +-
 control-board/src/bin/hwtest-motor/main.rs | 15 ---------------
 control-board/src/tasks/control_task.rs    | 15 +++++++++++++--
 motor-controller/bin/wheel/main.c          | 14 ++++++++++++--
 motor-controller/bin/wheel/system.h        |  2 +-
 5 files changed, 27 insertions(+), 21 deletions(-)

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/src/bin/hwtest-motor/main.rs b/control-board/src/bin/hwtest-motor/main.rs
index 5f10c62a..70f213be 100644
--- a/control-board/src/bin/hwtest-motor/main.rs
+++ b/control-board/src/bin/hwtest-motor/main.rs
@@ -126,7 +126,6 @@ async fn main(main_spawner: embassy_executor::Spawner) {
     loop {
         Timer::after_millis(10).await;
 
-<<<<<<< HEAD
         test_command_publisher.publish(DataPacket::BasicControl(BasicControl {
             _bitfield_1: Default::default(),
             _bitfield_align_1: Default::default(),
@@ -137,19 +136,5 @@ async fn main(main_spawner: embassy_executor::Spawner) {
             dribbler_speed: 10.0,
             kick_request: KickRequest::KR_DISABLE,
         })).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;
->>>>>>> main
     }
 }
diff --git a/control-board/src/tasks/control_task.rs b/control-board/src/tasks/control_task.rs
index 977dc20a..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 =
@@ -355,6 +355,17 @@ impl <
                 Timer::after_millis(10).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);
+
+            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.flash_motor_firmware(
                 self.shared_robot_state.hw_in_debug_mode()).await;
 
diff --git a/motor-controller/bin/wheel/main.c b/motor-controller/bin/wheel/main.c
index 78ad813b..47e1735f 100644
--- a/motor-controller/bin/wheel/main.c
+++ b/motor-controller/bin/wheel/main.c
@@ -214,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
@@ -618,9 +620,17 @@ 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
diff --git a/motor-controller/bin/wheel/system.h b/motor-controller/bin/wheel/system.h
index d4368174..683cf4f2 100644
--- a/motor-controller/bin/wheel/system.h
+++ b/motor-controller/bin/wheel/system.h
@@ -18,7 +18,7 @@
 ///////////////////
 
 #define UART_ENABLED
-#define IOQ_BUF_LENGTH 64
+#define IOQ_BUF_LENGTH 80
 #define IOQ_BUF_DEPTH 4
 
 //////////////////

From 13ee0d30f9fced6e314d49b9afeb0118cf63ee52 Mon Sep 17 00:00:00 2001
From: Will Stuckey <guyfleeman@gmail.com>
Date: Tue, 2 Sep 2025 22:41:43 -0400
Subject: [PATCH 19/20] clean up IOQ len defs

---
 motor-controller/bin/dribbler/system.h | 2 +-
 motor-controller/bin/wheel-test/main.h | 1 -
 motor-controller/bin/wheel/main.h      | 1 -
 3 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/motor-controller/bin/dribbler/system.h b/motor-controller/bin/dribbler/system.h
index b8835c02..d7982023 100644
--- a/motor-controller/bin/dribbler/system.h
+++ b/motor-controller/bin/dribbler/system.h
@@ -16,7 +16,7 @@
 ///////////////////
 
 #define UART_ENABLED
-#define IOQ_BUF_LENGTH 64
+#define IOQ_BUF_LENGTH 80
 #define IOQ_BUF_DEPTH 4
 
 //////////////////
diff --git a/motor-controller/bin/wheel-test/main.h b/motor-controller/bin/wheel-test/main.h
index 6006d9db..515aaa2d 100644
--- a/motor-controller/bin/wheel-test/main.h
+++ b/motor-controller/bin/wheel-test/main.h
@@ -21,7 +21,6 @@
 // expect cmd packets at 100H Hz (10ms), ticks are currently 1ms per
 // timeout ticks equates to 10 consecutive missed packets.
 #define COMMAND_PACKET_TIMEOUT_TICKS 100
-#define IOQ_BUF_LEN 64
 
 #define AVDD_V 3.3f
 
diff --git a/motor-controller/bin/wheel/main.h b/motor-controller/bin/wheel/main.h
index 843a7eec..b25263cf 100644
--- a/motor-controller/bin/wheel/main.h
+++ b/motor-controller/bin/wheel/main.h
@@ -21,7 +21,6 @@
 // expect cmd packets at 100H Hz (10ms), ticks are currently 1ms per
 // timeout ticks equates to 10 consecutive missed packets.
 #define COMMAND_PACKET_TIMEOUT_TICKS 100
-#define IOQ_BUF_LEN 64
 
 ///////////////////////////////////////////
 //  current sense amplification network  //

From a95f07931c216ab53d81d16bbf37dd2076ec9e09 Mon Sep 17 00:00:00 2001
From: Will Stuckey <guyfleeman@gmail.com>
Date: Tue, 2 Sep 2025 23:05:46 -0400
Subject: [PATCH 20/20] more uart header cleanup

---
 motor-controller/bin/dribbler/main.h     | 2 --
 motor-controller/bin/wheel-test/system.h | 2 +-
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/motor-controller/bin/dribbler/main.h b/motor-controller/bin/dribbler/main.h
index e3ec1128..4a8598da 100644
--- a/motor-controller/bin/dribbler/main.h
+++ b/motor-controller/bin/dribbler/main.h
@@ -15,8 +15,6 @@
 // timeout ticks equates to 10 consecutive missed packets.
 #define COMMAND_PACKET_TIMEOUT_TICKS 100
 
-#define IOQ_BUF_LEN 64
-
 ///////////////////////////////////////////
 //  current sense amplification network  //
 ///////////////////////////////////////////
diff --git a/motor-controller/bin/wheel-test/system.h b/motor-controller/bin/wheel-test/system.h
index 9ac68795..da3df585 100644
--- a/motor-controller/bin/wheel-test/system.h
+++ b/motor-controller/bin/wheel-test/system.h
@@ -18,7 +18,7 @@
 ///////////////////
 
 #define UART_ENABLED
-#define IOQ_BUF_LENGTH 64
+#define IOQ_BUF_LENGTH 80
 #define IOQ_BUF_DEPTH 4
 
 //////////////////