Commit 05dfb458 authored by Jake Read's avatar Jake Read
Browse files

cl step dev

parent 651d8815
......@@ -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"
......
......@@ -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"
......
......@@ -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
......@@ -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;
......
......@@ -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
......
......@@ -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;
......
......@@ -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
......
......@@ -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
......
......@@ -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] }