diff --git a/2020-06_ucbus-stepper-melted/eagle.epf b/2020-06_ucbus-stepper-melted/eagle.epf
index f0b033d27772b1164c3a653fb2396d717e937791..352648b220baab7bf3fd9f676bc0182ed8368034 100644
--- a/2020-06_ucbus-stepper-melted/eagle.epf
+++ b/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"
diff --git a/2020-06_ucbus-stepper-module/eagle.epf b/2020-06_ucbus-stepper-module/eagle.epf
index cd32d3b496243fd4b153ac8bedfe6c090353a253..6ff93f3c6df83f582a47739a6528487b93a6d81e 100644
--- a/2020-06_ucbus-stepper-module/eagle.epf
+++ b/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"
diff --git a/firmware/cl-step-controller/src/drivers/enc_as5047.cpp b/firmware/cl-step-controller/src/drivers/enc_as5047.cpp
index c864917c0823da1001f11332bd6ab456ef8bcc59..790277ee0ffec54a471195cc21789937619a73f5 100644
--- a/firmware/cl-step-controller/src/drivers/enc_as5047.cpp
+++ b/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
diff --git a/firmware/cl-step-controller/src/drivers/enc_as5047.h b/firmware/cl-step-controller/src/drivers/enc_as5047.h
index 7de3d6ddb1cfd24e8f8969774a2afd12949db582..49c9aad2b2bb3fa9584295fc5ce9604cda0fe6ff 100644
--- a/firmware/cl-step-controller/src/drivers/enc_as5047.h
+++ b/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;
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp
index 051cb3b1409e619109c0680f49a2a0963ae41ee7..3af6ca356eabe32cbbaaee998fbc6c478d2cd4db 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.cpp
+++ b/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
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.h b/firmware/cl-step-controller/src/drivers/step_cl.h
index 1343da1663fdcf51e5e38b9758861187c8696878..fa26bf1baed57ac7d0a448ac4f142a8eaac71c5d 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.h
+++ b/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;
diff --git a/firmware/cl-step-controller/src/drivers/ucbus_drop.cpp b/firmware/cl-step-controller/src/drivers/ucbus_drop.cpp
index 657919b0a40ca4d5f2ec6a653ff6e94a9b0cf7ae..3053d2ac214c9298d70525ca37334b62b6809970 100644
--- a/firmware/cl-step-controller/src/drivers/ucbus_drop.cpp
+++ b/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
diff --git a/firmware/cl-step-controller/src/drivers/ucbus_drop.h b/firmware/cl-step-controller/src/drivers/ucbus_drop.h
index de1d55eae2f771880b5f140450cf6033fa0d001c..844645cdb3390c9dcd539aa39357a5bc618bdc27 100644
--- a/firmware/cl-step-controller/src/drivers/ucbus_drop.h
+++ b/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
diff --git a/firmware/cl-step-controller/src/main.cpp b/firmware/cl-step-controller/src/main.cpp
index 10954416692b66e0f506189181f8ef1021468231..cdbdf30adda00fb8266e733e472cbb383fa7e16e 100644
--- a/firmware/cl-step-controller/src/main.cpp
+++ b/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] }
- };
- float target_current_pos = target.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;
- vel = 0;
- delta_steps = 0;
- setBlock = false;
- break;
- }
- default:
- break;
- }
- //DEBUG2PIN_OFF;
- */
+
}
diff --git a/firmware/cl-step-controller/src/osap/ts.h b/firmware/cl-step-controller/src/osap/ts.h
index 16e3d5fa73cdd530543e1bcefb81778835a06e65..27c57cc6c72695478381e9694c4e3c7a34361eaa 100644
--- a/firmware/cl-step-controller/src/osap/ts.h
+++ b/firmware/cl-step-controller/src/osap/ts.h
@@ -52,6 +52,8 @@ no warranty is provided, and users accept all liability.
#define AK_RUNCALIB 121
#define AK_READCALIB 122
#define AK_SET_TC 123
+#define AK_READ_MAG 124
+#define AK_READ_DIAG 125
// -------------------------------------------------------- MVC Endpoints
diff --git a/firmware/cl-step-controller/src/utils/clocks_d51_module.cpp b/firmware/cl-step-controller/src/utils/clocks_d51_module.cpp
index de2bbe86624330f78c44747ab55fefb7547e343b..6c4422162a01869d5b360af487756733a3f12995 100644
--- a/firmware/cl-step-controller/src/utils/clocks_d51_module.cpp
+++ b/firmware/cl-step-controller/src/utils/clocks_d51_module.cpp
@@ -102,7 +102,7 @@ void D51_Clock_Boss::start_50kHz_ticker_tc0(void){
TC0->COUNT32.INTENSET.bit.MC1 = 1;
// set the period,
while (TC0->COUNT32.SYNCBUSY.bit.CC0);
- TC0->COUNT32.CC[0].reg = 160; // at DIV2, 240 for 10us ('realtime') (with
+ TC0->COUNT32.CC[0].reg = 400; // at DIV2, 240 for 10us ('realtime') (with
// DFLL), 80 for 10us (with XTAL 16MHZ)
// 400 for 50us,
// enable, sync for enable write
@@ -112,6 +112,8 @@ void D51_Clock_Boss::start_50kHz_ticker_tc0(void){
TC1->COUNT32.CTRLA.bit.ENABLE = 1;
// enable the IRQ
NVIC_EnableIRQ(TC0_IRQn);
+ // and set the priority level
+ NVIC_SetPriority(TC0_IRQn, 2);
}
void D51_Clock_Boss::start_100kHz_ticker_tc2(void){
diff --git a/firmware/osape-smoothieroll-drop-stepper/src/main.cpp b/firmware/osape-smoothieroll-drop-stepper/src/main.cpp
index dcccbb6922614af207e6534eaf6014b511afcf48..f2090c54dec39367ef33893f879059bb584bf85a 100644
--- a/firmware/osape-smoothieroll-drop-stepper/src/main.cpp
+++ b/firmware/osape-smoothieroll-drop-stepper/src/main.cpp
@@ -25,11 +25,20 @@ union chunk_float64 {
// ...
// 8: DACs go full width
-#define BUS_DROP 3 // Z: 1, YL: 2, X: 3, YR: 4
+// clank cz:
+// Z: Bus Drop 5, Axis Pick 2, Invert ?, SPU 1386.6666667
+// TC: Bus Drop 4,
+// YL: Bus Drop 1, Invert true
+// YR: Bus Drop 2, Invert false
+// X: Bus Drop 3, Invert false
+
+#define BUS_DROP 3
+#define IS_LAST_DROP false
+
#define AXIS_PICK 0 // Z: 2, Y: 1, X: 0
-#define AXIS_INVERT true // Z: ?, YL: ?, YR: ?, X: ?
-#define SPU 400.0F // always posiive! z: 3200, xy: 400
-#define C_SCALE 0.2F // 0-1, floating
+#define AXIS_INVERT false // Z: ?, YL: ?, YR: ?, X: ?
+#define SPU 320.0F //1386.6666667F // always posiive! z: 3200, xy: 400, clank-cz: xy: 320
+#define C_SCALE 0.3F // 0-1, floating
#define TICKS_PER_PACKET 20.0F
void setup() {
@@ -42,7 +51,7 @@ void setup() {
// osap
osap->description = "remote stepper drop";
// bus
- ucBusDrop->init(false, BUS_DROP);
+ ucBusDrop->init(IS_LAST_DROP, BUS_DROP);
// stepper
stepper_hw->init(AXIS_INVERT, C_SCALE);
}
@@ -78,20 +87,6 @@ void loop() {
stepper_hw->setCurrent(currentChunks[AXIS_PICK].f);
break;
}
- 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;
diff --git a/log/2020-11-16_cpol-zero-stray.png b/log/2020-11-16_cpol-zero-stray.png
new file mode 100644
index 0000000000000000000000000000000000000000..c845d6e21748da56050e2316f92361bae41389f8
Binary files /dev/null and b/log/2020-11-16_cpol-zero-stray.png differ
diff --git a/log/2020-11-16_cpol-zero-typical.png b/log/2020-11-16_cpol-zero-typical.png
new file mode 100644
index 0000000000000000000000000000000000000000..7177ba03efe5b5fd386e0b4d4a3053807020c15e
Binary files /dev/null and b/log/2020-11-16_cpol-zero-typical.png differ
diff --git a/log/2020-11-16_noisy-encoder.png b/log/2020-11-16_noisy-encoder.png
new file mode 100644
index 0000000000000000000000000000000000000000..6f1b3f7b0417386f237e5ccd6631046066a1a22c
Binary files /dev/null and b/log/2020-11-16_noisy-encoder.png differ
diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md
index 9c7f45298e62be775f6541bbc6b0b412d00fbda2..c8f6b10aa66f8513cb265caca7ba4beaa4ba08bf 100644
--- a/log/cl-step-control-log.md
+++ b/log/cl-step-control-log.md
@@ -1185,4 +1185,48 @@ So - going to put this down for a minute.
- the current track is just trying to get a decent, non-noisy position and velocity estimate(s) from the thing. then you can use the vel estimate to write phase advance term per your notes above, and also to measure accel (xddot) = f, for your 2D torque-estimate lut.
- *then* you want to do both
- measuring torque guess success: use calibrated loadcell or somesuch?
- - (or) simply go forwards, towards using this in a machine: build a PID position controller, and tune it, maybe using a JS setPID() command, and remotely evaluate some set of moves, to tune. an auto-cal / auto-tune routine, etc. likely via the bus & with smoothie dishing moves, maybe just over usb, who knows
\ No newline at end of file
+ - (or) simply go forwards, towards using this in a machine: build a PID position controller, and tune it, maybe using a JS setPID() command, and remotely evaluate some set of moves, to tune. an auto-cal / auto-tune routine, etc. likely via the bus & with smoothie dishing moves, maybe just over usb, who knows
+
+## 2020 11 16
+
+OK, looks like it's been a month. Not sure where the days are dissappearing to lately. I'm going to start by pulling the LUT back home.
+
+First, easy test, can just send the encoder reading back to see what the direct noise is like.
+
+Glad I did this, there's obviously some erroneous (eliminate-able) noise on top of a reasonable floor:
+
+
+
+These are just raw encoder values. My honest guess is that I'm missing some bits in the signal back. I might still have some polarity wrong on the SPI, or it's going too fast and missing edges (or something like this).
+
+I turned the clock polarity to 1 in the D51, looks like I eliminate most of these spikes;
+
+
+
+That's just about 2 bits of noise, totally understandable from a 14 bit encoder. However, I still see the occasional spike:
+
+
+
+I can try tuning the speed down, it's near a limit at the moment.
+
+Well, yeah, the only thing that reliably de-noises these readings is to take the speed from around 8MHz with a nice small 5us read time to around 1.6MHz, where it takes about 25us.
+
+I think I should tack on an encoder-magnetic-strength reading to the calibration phase. That's set up, but the value looks to be high enough... though it fluctuates between 9k and 16k (16384 beign tops: it's a 14 bit value).
+
+There's also an error-flags register I can read, that'll directly tell me if things are too high or too low.
+
+Cool, I see some flags there - magnetic field is too high. So I can re-outfit my diagnostic to return a string with this whole message, then boot that back to the browser. And I can try bigger standoffs (nice), to compensate.
+
+Well, that's all instrumented. I have some strangeness here though: maghi / maglo seem to sweep between the full range: so does the cordic magnitude. This is a bit confusing because the magnet stays separated by the same amount, so I am wondering if because the magnet is a bit smaller - 6x2mm rather than the 8x3 spec'd in the datasheet - that the field is doing strange things due to misalignment. I should pick up the 8x3 magnets to check, that's my last best guess.
+
+In any case, slowing the read time down has significantly improved the noise, and I think I can get back to a speed estimate.
+
+### Alpha Filter
+
+I want to try this, but always want to take the underlying encoder reading back as well, so I can see if errors are arising somewhere in the filtering code or in the real reading.
+
+... ??? wtf
+
+OK, my new theory is that something is *up* with the volatile vals in here.
+
+Confused, IDK what is up. Another day.
\ No newline at end of file