cl step dev

2020-06_ucbus-stepper-melted/eagle.epf

@@ -9,17 +9,17 @@ AutoSaveProject=1
 UsedLibrary="C:/Dropbox/CBA/circuits/eagle/parts/SparkFun-Eagle-Libraries/SparkFun-Connectors.lbr"
 
 [Win_1]
-Type="Board Editor"
-Number=2
-File="2020-06_ucbus-stepper-melted.brd"
-View="17.4797 11.3003 31.7696 26.4656"
-WireWidths=" 0.8 0.9 0.1 0.05 0.2032 0.5 0.3 0 0.1524"
+Type="Schematic Editor"
+Number=1
+File="2020-06_ucbus-stepper-melted.sch"
+View="147.615 19.4173 415.853 165.287"
+WireWidths=" 0.0762 0.1016 0.127 0.15 0.2 0.2032 0.254 0.3048 0.4064 0.508 0.6096 0.8128 1.016 1.27 2.54 0.1524"
 PadDiameters=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 6.4516 0"
 PadDrills=" 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.65 0.7 0.75 0.8 0.85 0.9 1 0.6"
 ViaDiameters=" 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1 1.05 1.1 1.15 1.2 1.3 0"
-ViaDrills=" 0.2 0.25 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35 0.4 0.3"
+ViaDrills=" 0.2 0.25 0.3 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35"
 HoleDrills=" 0.2 0.25 0.3 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35"
-TextSizes=" 0.254 0.3048 0.4064 0.6096 0.8128 1.27 1.4224 1.6764 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.778 1.016"
+TextSizes=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.778"
 PolygonSpacings=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.27"
 PolygonIsolates=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 6.4516 0"
 MiterRadiuss=" 0.254 0.3175 0.635 1.27 2.54 1 2 2.5 5 7.5 10 0"
@@ -28,14 +28,14 @@ DimensionExtWidths=" 0.127 0.254 0.1 0.13 0.26 0"
 DimensionExtLengths=" 1.27 2.54 1 2 3 0"
 DimensionExtOffsets=" 1.27 2.54 1 2 3 0"
 SmdSizes=" 0.3048 0.1524 0.4064 0.2032 0.6096 0.3048 0.8128 0.4064 1.016 0.508 1.27 0.6604 1.4224 0.7112 1.6764 0.8128 1.778 0.9144 1.9304 0.9652 2.1844 1.0668 2.54 1.27 3.81 1.9304 5.08 2.54 6.4516 3.2512 1.27 0.635"
-WireBend=3
-WireBendSet=0
+WireBend=4
+WireBendSet=31
 WireCap=1
 MiterStyle=1
 PadShape=0
 ViaShape=1
 PolygonPour=0
-PolygonRank=4
+PolygonRank=0
 PolygonThermals=1
 PolygonOrphans=0
 TextRatio=8
@@ -50,20 +50,22 @@ SwapLevel=0
 ArcDirection=0
 AddLevel=2
 PadsSameType=0
-Layer=51
+Layer=91
+Views=" 1: 147.615 19.4173 415.853 165.287"
+Sheet="1"
 
 [Win_2]
-Type="Schematic Editor"
-Number=1
-File="2020-06_ucbus-stepper-melted.sch"
-View="147.615 19.4173 415.853 165.287"
-WireWidths=" 0.0762 0.1016 0.127 0.15 0.2 0.2032 0.254 0.3048 0.4064 0.508 0.6096 0.8128 1.016 1.27 2.54 0.1524"
+Type="Board Editor"
+Number=2
+File="2020-06_ucbus-stepper-melted.brd"
+View="12.0251 1.84836 26.315 17.0137"
+WireWidths=" 0.8 0.9 0.1 0.05 0.2032 0.5 0.3 0 0.1524"
 PadDiameters=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 6.4516 0"
 PadDrills=" 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.65 0.7 0.75 0.8 0.85 0.9 1 0.6"
 ViaDiameters=" 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1 1.05 1.1 1.15 1.2 1.3 0"
-ViaDrills=" 0.2 0.25 0.3 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35"
+ViaDrills=" 0.2 0.25 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35 0.4 0.3"
 HoleDrills=" 0.2 0.25 0.3 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35"
-TextSizes=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.778"
+TextSizes=" 0.254 0.3048 0.4064 0.6096 0.8128 1.27 1.4224 1.6764 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.778 1.016"
 PolygonSpacings=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.27"
 PolygonIsolates=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 6.4516 0"
 MiterRadiuss=" 0.254 0.3175 0.635 1.27 2.54 1 2 2.5 5 7.5 10 0"
@@ -72,14 +74,14 @@ DimensionExtWidths=" 0.127 0.254 0.1 0.13 0.26 0"
 DimensionExtLengths=" 1.27 2.54 1 2 3 0"
 DimensionExtOffsets=" 1.27 2.54 1 2 3 0"
 SmdSizes=" 0.3048 0.1524 0.4064 0.2032 0.6096 0.3048 0.8128 0.4064 1.016 0.508 1.27 0.6604 1.4224 0.7112 1.6764 0.8128 1.778 0.9144 1.9304 0.9652 2.1844 1.0668 2.54 1.27 3.81 1.9304 5.08 2.54 6.4516 3.2512 1.27 0.635"
-WireBend=4
-WireBendSet=31
+WireBend=3
+WireBendSet=0
 WireCap=1
 MiterStyle=1
 PadShape=0
 ViaShape=1
 PolygonPour=0
-PolygonRank=0
+PolygonRank=4
 PolygonThermals=1
 PolygonOrphans=0
 TextRatio=8
@@ -94,9 +96,7 @@ SwapLevel=0
 ArcDirection=0
 AddLevel=2
 PadsSameType=0
-Layer=91
-Views=" 1: 147.615 19.4173 415.853 165.287"
-Sheet="1"
+Layer=51
 
 [Win_3]
 Type="Control Panel"

2020-06_ucbus-stepper-module/eagle.epf

@@ -352,15 +352,15 @@ UsedLibraryUrn="urn:adsk.eagle:library:146"
 UsedLibraryUrn="urn:adsk.eagle:library:168"
 
 [Win_1]
-Type="Board Editor"
-Number=2
-File="2020-06_ucbus-stepper-module.brd"
-View="4.03467 13.6269 14.4835 23.0259"
-WireWidths=" 0.127 0.15 0.2 2.54 1.016 0.2032 0.254 0.6096 0 2.032 0.508 0.4064 0.3048 1.27 0.8128 0.1524"
+Type="Schematic Editor"
+Number=1
+File="2020-06_ucbus-stepper-module.sch"
+View="93.8998 -5.32858 236.93 67.1657"
+WireWidths=" 0.0762 0.1016 0.127 0.15 0.2 0.2032 0.254 0.3048 0.4064 0.508 0.6096 0.8128 1.016 1.27 2.54 0.1524"
 PadDiameters=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 6.4516 0"
 PadDrills=" 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.65 0.7 0.75 0.8 0.85 0.9 1 0.6"
 ViaDiameters=" 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1 1.05 1.1 1.15 1.2 1.3 0"
-ViaDrills=" 0.2 0.25 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35 0.4 0.3"
+ViaDrills=" 0.2 0.25 0.3 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35"
 HoleDrills=" 0.2 0.25 0.3 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35"
 TextSizes=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.778"
 PolygonSpacings=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.27"
@@ -371,14 +371,14 @@ DimensionExtWidths=" 0.127 0.254 0.1 0.13 0.26 0"
 DimensionExtLengths=" 1.27 2.54 1 2 3 0"
 DimensionExtOffsets=" 1.27 2.54 1 2 3 0"
 SmdSizes=" 0.3048 0.1524 0.4064 0.2032 0.6096 0.3048 0.8128 0.4064 1.016 0.508 1.27 0.6604 1.4224 0.7112 1.6764 0.8128 1.778 0.9144 1.9304 0.9652 2.1844 1.0668 2.54 1.27 3.81 1.9304 5.08 2.54 6.4516 3.2512 1.27 0.635"
-WireBend=1
-WireBendSet=0
+WireBend=4
+WireBendSet=31
 WireCap=1
 MiterStyle=1
 PadShape=0
 ViaShape=1
 PolygonPour=0
-PolygonRank=1
+PolygonRank=0
 PolygonThermals=1
 PolygonOrphans=0
 TextRatio=8
@@ -393,18 +393,20 @@ SwapLevel=0
 ArcDirection=0
 AddLevel=2
 PadsSameType=0
-Layer=1
+Layer=91
+Views=" 1: 93.8998 -5.32858 236.93 67.1657"
+Sheet="1"
 
 [Win_2]
-Type="Schematic Editor"
-Number=1
-File="2020-06_ucbus-stepper-module.sch"
-View="93.8998 -5.32858 236.93 67.1657"
-WireWidths=" 0.0762 0.1016 0.127 0.15 0.2 0.2032 0.254 0.3048 0.4064 0.508 0.6096 0.8128 1.016 1.27 2.54 0.1524"
+Type="Board Editor"
+Number=2
+File="2020-06_ucbus-stepper-module.brd"
+View="16.5161 15.8653 25.2235 23.6978"
+WireWidths=" 0.127 0.15 0.2 2.54 1.016 0.2032 0.254 0.6096 0 2.032 0.508 0.4064 0.3048 1.27 0.8128 0.1524"
 PadDiameters=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 6.4516 0"
 PadDrills=" 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 0.65 0.7 0.75 0.8 0.85 0.9 1 0.6"
 ViaDiameters=" 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1 1.05 1.1 1.15 1.2 1.3 0"
-ViaDrills=" 0.2 0.25 0.3 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35"
+ViaDrills=" 0.2 0.25 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35 0.4 0.3"
 HoleDrills=" 0.2 0.25 0.3 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 1 0.35"
 TextSizes=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.27 1.4224 1.6764 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.778"
 PolygonSpacings=" 0.254 0.3048 0.4064 0.6096 0.8128 1.016 1.4224 1.6764 1.778 1.9304 2.1844 2.54 3.81 5.08 6.4516 1.27"
@@ -415,14 +417,14 @@ DimensionExtWidths=" 0.127 0.254 0.1 0.13 0.26 0"
 DimensionExtLengths=" 1.27 2.54 1 2 3 0"
 DimensionExtOffsets=" 1.27 2.54 1 2 3 0"
 SmdSizes=" 0.3048 0.1524 0.4064 0.2032 0.6096 0.3048 0.8128 0.4064 1.016 0.508 1.27 0.6604 1.4224 0.7112 1.6764 0.8128 1.778 0.9144 1.9304 0.9652 2.1844 1.0668 2.54 1.27 3.81 1.9304 5.08 2.54 6.4516 3.2512 1.27 0.635"
-WireBend=4
-WireBendSet=31
+WireBend=1
+WireBendSet=0
 WireCap=1
 MiterStyle=1
 PadShape=0
 ViaShape=1
 PolygonPour=0
-PolygonRank=0
+PolygonRank=1
 PolygonThermals=1
 PolygonOrphans=0
 TextRatio=8
@@ -437,9 +439,7 @@ SwapLevel=0
 ArcDirection=0
 AddLevel=2
 PadsSameType=0
-Layer=91
-Views=" 1: 93.8998 -5.32858 236.93 67.1657"
-Sheet="1"
+Layer=1
 
 [Win_3]
 Type="Control Panel"

firmware/cl-step-controller/src/drivers/enc_as5047.cpp

@@ -75,22 +75,20 @@ void ENC_AS5047::init(void){
     // AS5047 datasheet says CPOL = 1, CPHA = 0, msb first, and parity checks 
     // bit: func 
     // 15: parity, 14: 0/read, 1/write, 13:0 address to read or write 
-    ENC_SER_SPI.CTRLA.reg = //SERCOM_SPI_CTRLA_CPOL | // CPOL = 1
-                            SERCOM_SPI_CTRLA_CPHA | // ?
+    ENC_SER_SPI.CTRLA.reg = SERCOM_SPI_CTRLA_CPOL | // CPOL = 1
+                            //SERCOM_SPI_CTRLA_CPHA | // ?
                             SERCOM_SPI_CTRLA_DIPO(3) | // pad 3 is data input 
                             SERCOM_SPI_CTRLA_DOPO(0) | // pad 0 is data output, 1 is clk  
                             SERCOM_SPI_CTRLA_MODE(3);  // mode 3: head operation 
     ENC_SER_SPI.CTRLB.reg = SERCOM_SPI_CTRLB_RXEN; // enable rx, char size is 8, etc 
-    ENC_SER_SPI.BAUD.reg = SERCOM_SPI_BAUD_BAUD(2); // f_baud = f_ref / ((2*BAUD) + 1) 
+    ENC_SER_SPI.BAUD.reg = SERCOM_SPI_BAUD_BAUD(14); // f_baud = f_ref / ((2*BAUD) + 1) 
                                                     // BAUD = 2 ~= 8MHz / 124ns clock period 
                                                     // BAUD = 3 ~= 6MHz / 164ns clock period: AS5047 min period is 100ns
-    
+                                                    // BAUD = 7 ~= 3MHz, less noisy... 
+                                                    // BAUD = 14 ~= 1.6MHz, much less noisy, reads are about 25us. 
     // enable interrupts 
-    NVIC_EnableIRQ(SERCOM4_0_IRQn);
     NVIC_EnableIRQ(SERCOM4_1_IRQn);
     NVIC_EnableIRQ(SERCOM4_2_IRQn);
-    NVIC_EnableIRQ(SERCOM4_3_IRQn);
-
     // turn it back on 
     while(ENC_SER_SPI.SYNCBUSY.bit.ENABLE);
     ENC_SER_SPI.CTRLA.bit.ENABLE = 1;
@@ -167,4 +165,30 @@ boolean ENC_AS5047::is_read_complete(void){
 
 uint16_t ENC_AS5047::get_reading(void){
     return result;
+}
+
+uint16_t ENC_AS5047::read_field_mag(void){
+    start_spi_interaction(AS5047_SPI_READ_MAG);
+    while(!readComplete);
+    return result;
+}
+
+String ENC_AS5047::read_enc_diagnostics(void){
+    start_spi_interaction(AS5047_SPI_READ_DIAG);
+    while(!readComplete);
+    uint16_t res = result;
+    boolean magHi = false;
+    boolean magLo = false;
+    boolean cordicOvf = false;
+    boolean compComplete = false;
+    if(res & (1 << 11)) magHi = true; // maghi 
+    if(res & (1 << 10)) magLo = true; // maglo 
+    if(res & (1 << 9)) cordicOvf = true; // cordic overflow 
+    if(res & (1 << 8)) compComplete = true; // offset compensation completed 
+    uint16_t agc = res & 0b0000000001111111;
+    return "magHi: " + String(magHi) 
+            + "\nmagLo: " + String(magLo)
+            + "\ncordicOverflow: " + String(cordicOvf)
+            + "\ncompensationComplete: " + String(compComplete)
+            + "\nautomaticGainCorrection: " + String(agc);
 }
\ No newline at end of file

firmware/cl-step-controller/src/drivers/enc_as5047.h

@@ -42,8 +42,12 @@ is; no warranty is provided, and users accept all liability.
 #define ENC_SER_GCLKNUM 8
 
 #define AS5047_SPI_READ_POS (0b1100000000000000 | 0x3FFF)
+#define AS5047_SPI_READ_POS_UNC (0b1100000000000000 | 0x3FFE)
 #define AS5047_SPI_NO_OP (0b1000000000000000)
 
+#define AS5047_SPI_READ_MAG (0b1100000000000000 | 0x3FFD)
+#define AS5047_SPI_READ_DIAG (0b1100000000000000 | 0x3FFC)
+
 class ENC_AS5047 {
     private:
         // is singleton
@@ -70,6 +74,9 @@ class ENC_AS5047 {
         boolean is_read_complete(void);
         uint16_t get_reading(void);
         void on_read_complete(uint16_t pos);
+        // *blocking*
+        uint16_t read_field_mag(void);
+        String read_enc_diagnostics(void);
 };
 
 extern ENC_AS5047* enc_as5047;

firmware/cl-step-controller/src/drivers/step_cl.cpp

@@ -73,9 +73,9 @@ void flash_write_init(void){
 }
 
 void flash_write_page(void){
-    sysError("erasing 0x" + String((uint32_t)block_ptr));
+    //sysError("erasing 0x" + String((uint32_t)block_ptr));
     flashClass.erase(block_ptr, BYTES_PER_BLOCK);
-    sysError("writing 0x" + String((uint32_t)block_ptr));
+    //sysError("writing 0x" + String((uint32_t)block_ptr));
     flashClass.write(block_ptr, (const uint8_t*)buffer, BYTES_PER_BLOCK);
     delay(1);
 }
@@ -115,7 +115,7 @@ float Step_CL::get_torque(void){
 void Step_CL::run_torque_loop(void){
     if(is_calibrating) return;
     // mark time 
-    DEBUG1PIN_ON;
+    //DEBUG1PIN_ON;
     // ok, first we read the encoder 
     enc_as5047->trigger_read();
     // this kicks off the party, proceeds below
@@ -124,34 +124,36 @@ void Step_CL::run_torque_loop(void){
 #define MAP_7p2_TO_1 (1.0F / 7.2F)
 #define TICKS_PER_SEC 50000.0F
 #define SECS_PER_TICK 1.0F / TICKS_PER_SEC
-volatile float q = 0.05F;//0.125F;      // process noise covariance  
-volatile float r = 1.0F;        // measurement noise covariance 
-volatile float measurement;     // the measurement... 
-volatile float a = 0.0F;        // the estimate on a 
-volatile float p = 100.0F;        // estimation error covariance 
-volatile float k;               // kalman gain 
-
-volatile float _deg_s;      // real angle / sec 
+
+volatile float a_measurement = 0.0F;
+volatile float last_measurement = 0.0F;
+volatile float a_est = 0.0F;
+volatile float a_last_est = 0.0F;
+volatile float a_alpha = 0.9F;       // trust in measurement 
+
 volatile float _pa;         // phase angle 
 
-float Step_CL::get_deg_sec(void){
-    return _deg_s;
+float Step_CL::get_last_pos_est(void){
+    return a_est;
+}
+
+float Step_CL::get_last_reading(void){
+    return a_measurement;
 }
 
 void ENC_AS5047::on_read_complete(uint16_t result){
     if(step_cl->is_calibrating) return;
-
-    // (1) get the new measurement,
-    measurement = lut[result * 2]; 
-
-    p = p + q;
-
-    k = p / (p + r);
-    a = a + k * (measurement - a);
-    p = (1 - k) * p;
-
-    // write a 'real' speed value 
-    _deg_s = measurement;
+    last_measurement = a_measurement;
+    a_measurement = lut[result * 2]; 
+    // wtf ?? 
+    // a_est = (a_measurement * a_alpha) + (a_last_est * (1 - a_alpha));
+    // a_last_est = a_est;
+    // or like 
+    // I am completely confused by the noise that this generates ? seems like it's taking a difference?
+    // should just be averaging? 
+    // something about getting the variables out, under interrupt? 
+    a_est = 0.5 * a_measurement + 0.5 * last_measurement;
+    //output = alpha * (input - output) + output 
 
     _pa = lut[result * 2 + 1];      // the phase angle (0 - 1 in a sweep of 4 steps)
     // this is the phase angle we want to apply, 90 degs off & wrap't to 1 
@@ -170,7 +172,6 @@ void ENC_AS5047::on_read_complete(uint16_t result){
     // with the *amount* commanded by our _tc torque ask 
     step_a4950->point(_pa, abs(step_cl->get_torque()));
     // debug loop completion 
-    DEBUG1PIN_OFF;
 }
 
 // the calib routine 

firmware/cl-step-controller/src/drivers/step_cl.h

@@ -33,7 +33,8 @@ class Step_CL {
         void print_table(void);
         void set_torque(float tc);
         float get_torque(void);
-        float get_deg_sec(void);
+        float get_last_pos_est(void);
+        float get_last_reading(void);
         void run_torque_loop(void);
         boolean calibrate(void);
         boolean is_calibrating;

firmware/cl-step-controller/src/drivers/ucbus_drop.cpp

@@ -100,6 +100,10 @@ void UCBus_Drop::init(boolean useDipPick, uint8_t ID) {
 	NVIC_EnableIRQ(SERCOM1_2_IRQn); // rx, 
   NVIC_EnableIRQ(SERCOM1_1_IRQn); // txc ?
   NVIC_EnableIRQ(SERCOM1_0_IRQn); // tx, 
+  // priority
+  NVIC_SetPriority(SERCOM1_2_IRQn, 1);
+  NVIC_SetPriority(SERCOM1_1_IRQn, 1);
+  NVIC_SetPriority(SERCOM1_0_IRQn, 1);
 	// set baud 
   UBD_SER_USART.BAUD.reg = UBD_BAUD_VAL;
   // and finally, a kickoff

firmware/cl-step-controller/src/drivers/ucbus_drop.h

@@ -105,7 +105,6 @@ class UCBus_Drop {
     // handlers 
     void onRxISR(void);
     void onPacketARx(void);
-    //void onPacketBRx(void);
     // our physical bus address, 
     volatile uint8_t id = 0;
     // the api, eh 

firmware/cl-step-controller/src/main.cpp

@@ -35,7 +35,7 @@ VPort* replyBlankVp;
 uint16_t replyBlankVpi = 0;
 uint16_t lastQueueSpaceTxd = 0;
 
-#define BUS_DROP 1    // Z: 1, YL: 2, X: 3, YR: 4
+#define BUS_DROP 4    // Z: 1, YL: 2, X: 3, YR: 4
 #define AXIS_PICK 2   // Z: 2, Y: 1, X: 0
 #define AXIS_INVERT false  // Z: false, YL: true, YR: false, X: false
 #define SPU 3200.0F // always positive! Z: 3200, XY: 400 
@@ -58,82 +58,48 @@ void setup() {
   d51_clock_boss->start_50kHz_ticker_tc0();
 }
 
-uint8_t bChPck[64];
+// 50kHz control tick 
+void TC0_Handler(void){
+  TC0->COUNT32.INTFLAG.bit.MC0 = 1;
+  TC0->COUNT32.INTFLAG.bit.MC1 = 1;
+  step_cl->run_torque_loop();
+}
 
+uint8_t bChPck[64];
 volatile float current_floating_pos = 0.0F;
 volatile int32_t current_step_pos = 0;
 volatile uint32_t delta_steps = 0;
-
 volatile float vel = 0.0F; 
 volatile float move_counter = 0.0F;
-
 volatile boolean setBlock = false;
-
 uint16_t tick = 0;
 
-void TC0_Handler(void){
-  TC0->COUNT32.INTFLAG.bit.MC0 = 1;
-  TC0->COUNT32.INTFLAG.bit.MC1 = 1;
-  step_cl->run_torque_loop();
-}
-
 // async loop 
 void loop() {
   osap->loop();
   step_a4950->dacRefresh();
-  /*
-  tick++;
-  if(tick > 500){
-    tick = 0;
-    enc_as5047->trigger_read();
-    while(!enc_as5047->is_read_complete());
-    uint16_t reading = 0b0011111111111111 & enc_as5047->get_reading();
-    //sysError(String(reading));
-  }
-  */
-  // do packet B grab / handle 
-  /*
+  // check chb packes 
   if(ucBusDrop->ctr_b()){
     uint16_t len = ucBusDrop->read_b(bChPck);
     uint16_t ptr = 0;
     switch(bChPck[ptr]){
-      case AK_SETCURRENT: {
-        // get currents 
-        ptr ++;
-        chunk_float32 currentChunks[3];
-        currentChunks[0] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
-        currentChunks[1] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
-        currentChunks[2] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
-        // set current
-        step_a4950->setCurrent(currentChunks[AXIS_PICK].f);
-        // bug here 
-        // I noticed that setcurrent commands to the z motor seem to fail: the motor drops to a low current no matter the value transmitted
-        // I've checked (by inserting a flag to pick y-values) here, and those values work well, so it's nothing with the motor driver itself
-        // so, I suspect the b-channel bus packets are coming out of those structures one byte short - makes sense there would be some bug in/around delineation 
-        break;
+      case AK_SET_TC: {
+          ptr ++;
+          chunk_float32 tcs = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] }};
+          step_cl->set_torque(tcs.f);
         }
-      case AK_SETPOS: {
-        // get posns, 
-        ptr ++;
-        chunk_float32 posChunks[3];
-        posChunks[0] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
-        posChunks[1] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
-        posChunks[2] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
-        float target_current_pos = posChunks[AXIS_PICK].f;
-        int32_t target_current_steps = lroundf(target_current_pos * SPU);
-        setBlock = true; // don't do step work while these are modified 
-        current_floating_pos = target_current_pos;
-        current_step_pos = target_current_steps;
-        setBlock = false;
-      }
-      default:
-        // noop,
         break;
+      case AK_RUNCALIB:
+        step_cl->calibrate();
+        break;
+      default:
+        // noop 
+        break;  
     }
   }
-  */
 } // end loop 
 
+// usb packets 
 void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp){
   // track end of header, to reply with 
   uint16_t replyPtr = ptr;
@@ -154,6 +120,19 @@ void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t seg
   // do the reading:
   ptr ++; // walk appcode DK_APP
   switch(pck[ptr]){
+    case AK_READ_MAG:{
+      ptr ++;
+      reply[rl ++] = AK_READ_MAG;
+      enc_as5047->read_enc_diagnostics();
+      uint16_t mag = enc_as5047->read_field_mag();
+      ts_writeUint16(mag, reply, &rl);
+      break;
+    }
+    case AK_READ_DIAG:
+      ptr ++;
+      reply[rl ++] = AK_READ_DIAG;
+      ts_writeString(enc_as5047->read_enc_diagnostics(), reply, &rl);
+      break;
     case AK_RUNCALIB:
       ptr ++; // walk stepcode 
       reply[rl ++] = AK_RUNCALIB;
@@ -175,8 +154,10 @@ void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t seg
         reply[rl ++] = AK_SET_TC;
         chunk_float32 tcs = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] }};
         step_cl->set_torque(tcs.f);
-        float deg_sec = step_cl->get_deg_sec();
-        ts_writeFloat32(deg_sec, reply, &rl);
+        float pos_est = step_cl->get_last_pos_est();
+        float last_reading = step_cl->get_last_reading();
+        ts_writeFloat32(pos_est, reply, &rl);
+        ts_writeFloat32(last_reading, reply, &rl);
         break;
       }
     default:
@@ -194,92 +175,14 @@ void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t seg
   vp->clearPacket(pwp);
 }
 
+// on words rx'd from bus, 
 void UCBus_Drop::onRxISR(void){
-  /*
-  if(setBlock) return;
-  //DEBUG2PIN_TOGGLE;
-  move_counter += vel;
-  boolean move_check = (move_counter > 1.0F);
-  //DEBUG2PIN_TOGGLE;
-  if(move_check){
-    move_counter -= 1.0F;
-    if(delta_steps == 0){
-      // nothing 
-    } else {
-      //DEBUG1PIN_TOGGLE;
-      step_a4950->step();
-      delta_steps --;
-      if(step_a4950->getDir()){
-        current_step_pos ++;
-      } else {
-        current_step_pos --;
-      }
-    }
-  }
-  */
+
 }
 
+// on timed (interrupt) rx of bus packet, channel A 
+// this is where we will eventually read-in positions, and target them
 void UCBus_Drop::onPacketARx(void){
-  /*
-  if(setBlock) return;
-  //DEBUG2PIN_TOGGLE;
-  // last move is done, convert back steps -> float,
-  current_floating_pos = current_step_pos / SPU;
-  vel = 0.0F; // reset zero in case packet is not move 
-  uint8_t bptr = 0;
-  // switch bus packet types 
-  switch(inBufferA[0]){
-    case UB_AK_GOTOPOS:
-      {
-        bptr = AXIS_PICK * 4 + 1;
-        chunk_float32 target = {
-          .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] }
-        };
-        // chunk_float64 target = { 
-        //   .bytes = { inBuffer[bptr], inBuffer[bptr + 1], inBuffer[bptr + 2], inBuffer[bptr + 3],
-        //             inBuffer[bptr + 4], inBuffer[bptr + 5], inBuffer[bptr + 6], inBuffer[bptr + 7] }};
-        float delta = target.f - current_floating_pos;
-        // update,
-        //move_counter = 0.0F; // shouldn't need this ? try deleting 
-        // direction swop,
-        if(delta > 0){
-          step_a4950->dir(true);
-        } else {
-          step_a4950->dir(false);
-        }
-        // how many steps, 
-        delta_steps = lroundf(abs(delta * SPU));
-        // what speed 
-        vel = abs(delta * SPU) / TICKS_PER_PACKET;
-        // for str8 r8 
-        // if(delta_steps == 0){
-        //   vel = 0.0F;
-        // } else {
-        //   vel = abs(delta * SPU) / TICKS_PER_PACKET;
-        // }
-        break; // end gotopos 
-      }
-    case UB_AK_SETPOS:
-      {
-        // reqest is to set position, not go to it... 
-        bptr = AXIS_PICK * 4 + 1;
-        chunk_float32 target = {
-          .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] }