diff --git a/firmware/cl-step-controller/src/drivers/step_a4950.cpp b/firmware/cl-step-controller/src/drivers/step_a4950.cpp
index 9f2cee4abd51411f7586d7c561f9688c2960b79a..67ab359f26da9dbd6860ab90dcbcf29f171ad355 100644
--- a/firmware/cl-step-controller/src/drivers/step_a4950.cpp
+++ b/firmware/cl-step-controller/src/drivers/step_a4950.cpp
@@ -94,7 +94,7 @@ STEP_A4950* STEP_A4950::getInstance(void){
return instance;
}
-STEP_A4950* stepper_hw = STEP_A4950::getInstance();
+STEP_A4950* step_a4950 = STEP_A4950::getInstance();
STEP_A4950::STEP_A4950() {}
diff --git a/firmware/cl-step-controller/src/drivers/step_a4950.h b/firmware/cl-step-controller/src/drivers/step_a4950.h
index 76cd1edc7ee2c4f6b10c880c38be4a01bf381d80..0cc143a80865c7ea194edb21c118b7e7912c279f 100644
--- a/firmware/cl-step-controller/src/drivers/step_a4950.h
+++ b/firmware/cl-step-controller/src/drivers/step_a4950.h
@@ -98,6 +98,6 @@ class STEP_A4950 {
void dacRefresh(void);
};
-extern STEP_A4950* stepper_hw;
+extern STEP_A4950* step_a4950;
#endif
\ No newline at end of file
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp
index a3e65e4b495b7fd7e0aa83b6d386b0514b03133a..eef83d525ab25c6e63a2dd68205c77c2fa023b14 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.cpp
+++ b/firmware/cl-step-controller/src/drivers/step_cl.cpp
@@ -37,9 +37,10 @@ Step_CL::Step_CL(void){}
#define ENCODER_COUNTS 16384
void Step_CL::init(void){
- stepper_hw->init(false, 0.4);
+ step_a4950->init(false, 0.4);
enc_as5047->init();
- _tc = 0; // torque command -> 0;
+ _tc = -1.0; // torque command -> 0;
+ is_calibrating = false;
}
// LUT / flash work
@@ -72,11 +73,11 @@ void flash_write_init(void){
}
void flash_write_page(void){
- sysError("erasing");
+ sysError("erasing 0x" + String((uint32_t)block_ptr));
flashClass.erase(block_ptr, BYTES_PER_BLOCK);
- sysError("writing");
+ sysError("writing 0x" + String((uint32_t)block_ptr));
flashClass.write(block_ptr, (const uint8_t*)buffer, BYTES_PER_BLOCK);
- delay(100);
+ delay(10);
}
void flash_write_value(float val){
@@ -105,26 +106,59 @@ void Step_CL::set_torque(float tc){
_tc = tc;
}
+float Step_CL::get_torque(void){
+ return _tc;
+}
+
// the control loop
void Step_CL::run_torque_loop(void){
+ if(is_calibrating) return;
// ok, first we read the encoder
enc_as5047->trigger_read();
// this kicks off the party, proceeds below
}
+#define MAP_7p2_TO_1 (1.0F / 7.2F)
+
void ENC_AS5047::on_read_complete(uint16_t result){
+ if(step_cl->is_calibrating) return;
float ra = lut[result];
+ // to find the phase, want to find modulus this ra / 1.8
+ while(ra > 7.2F){
+ ra -= 7.2F;
+ }
+ // the output is inside of 0-1, some parameterized point inside of the phase angle
+ // so we want to map 0-1.8 angle-in-phase to 0-1 point-in-output,
+ // this maps 0-1.8 -> 0-1.0,
+ ra = ra * MAP_7p2_TO_1;
+ // this is the phase angle we want to apply, 90 degs off & wrap't to 1
+ if(step_cl->get_torque() < 0){
+ ra -= 0.5; // 180* phase swop
+ if(ra < 0){
+ ra += 1.0F;
+ }
+ } else {
+ ra += 0.5;
+ if(ra > 1){
+ ra -= 1.0F;
+ }
+ }
+ // now we ask our voltage modulation machine to put this on the coils
+ // with the *amount* commanded by our _tc torque ask
+ step_a4950->point(ra, abs(step_cl->get_torque()));
// debug loop completion
DEBUG1PIN_OFF;
}
// the calib routine
boolean Step_CL::calibrate(void){
+ is_calibrating = true;
+ delay(1);
// (1) first, build a table for 200 full steps w/ encoder averaged values at each step
float phase_angle = 0.0F;
for(uint8_t i = 0; i < 200; i ++){
// pt to new angle
- stepper_hw->point(phase_angle, CALIB_CSCALE);
+ step_a4950->point(phase_angle, CALIB_CSCALE);
// wait to settle / go slowly
delay(CALIB_STEP_DELAY);
// do readings
@@ -270,6 +304,7 @@ boolean Step_CL::calibrate(void){
flash_write_value(ra); // this just happens in order, we zeroe'd out global counters at the start
} // end sweep thru 2^14 pts
sysError("calib complete");
+ is_calibrating = false;
return true; // went OK
}
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.h b/firmware/cl-step-controller/src/drivers/step_cl.h
index 2b0b083308574d9d6a7bb73afb32e61f8e13bbc0..d6ce85415d1aa56c450520a74f5e0f17d0f9b04f 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.h
+++ b/firmware/cl-step-controller/src/drivers/step_cl.h
@@ -32,8 +32,10 @@ class Step_CL {
void init(void);
void print_table(void);
void set_torque(float tc);
+ float get_torque(void);
void run_torque_loop(void);
boolean calibrate(void);
+ boolean is_calibrating;
//float __attribute__((__aligned__(256))) lut[16384]; // nor does this !
//float lut[16384]; // nor does this work
//step_cl_calib_table_t lut; // not even this works ?? too big ??
diff --git a/firmware/cl-step-controller/src/main.cpp b/firmware/cl-step-controller/src/main.cpp
index e9bcfae47b5419cfb3e3f9015ab06b75f6a26b22..a3af43969679b30d240dc0cdbd53a89ad99eeecc 100644
--- a/firmware/cl-step-controller/src/main.cpp
+++ b/firmware/cl-step-controller/src/main.cpp
@@ -81,7 +81,7 @@ void TC0_Handler(void){
// async loop
void loop() {
osap->loop();
- stepper_hw->dacRefresh();
+ step_a4950->dacRefresh();
/*
tick++;
if(tick > 500){
@@ -106,7 +106,7 @@ void loop() {
currentChunks[1] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
currentChunks[2] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
// set current
- stepper_hw->setCurrent(currentChunks[AXIS_PICK].f);
+ 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
@@ -199,9 +199,9 @@ void UCBus_Drop::onRxISR(void){
// nothing
} else {
//DEBUG1PIN_TOGGLE;
- stepper_hw->step();
+ step_a4950->step();
delta_steps --;
- if(stepper_hw->getDir()){
+ if(step_a4950->getDir()){
current_step_pos ++;
} else {
current_step_pos --;
@@ -235,9 +235,9 @@ void UCBus_Drop::onPacketARx(void){
//move_counter = 0.0F; // shouldn't need this ? try deleting
// direction swop,
if(delta > 0){
- stepper_hw->dir(true);
+ step_a4950->dir(true);
} else {
- stepper_hw->dir(false);
+ step_a4950->dir(false);
}
// how many steps,
delta_steps = lroundf(abs(delta * SPU));
diff --git a/firmware/cl-step-controller/src/utils/FlashStorage.cpp b/firmware/cl-step-controller/src/utils/FlashStorage.cpp
index 7108db48ca0ce87aaec5c85c835a5c874677597f..24411c93e7d1be3b51ebadbe362bc952c0b3a21e 100644
--- a/firmware/cl-step-controller/src/utils/FlashStorage.cpp
+++ b/firmware/cl-step-controller/src/utils/FlashStorage.cpp
@@ -86,7 +86,7 @@ void FlashClass::write(const volatile void *flash_ptr, const void *data, uint32_
}
// Execute "WP" Write Page
- sysError("write 0x" + String((uint32_t)flash_ptr));
+ //sysError("write 0x" + String((uint32_t)flash_ptr));
NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_WP;
while (NVMCTRL->INTFLAG.bit.DONE == 0) { }
invalidate_CMCC_cache();
@@ -107,7 +107,7 @@ void FlashClass::erase(const volatile void *flash_ptr, uint32_t size){
}
void FlashClass::erase(const volatile void *flash_ptr){
- sysError("erase 0x" + String((uint32_t)flash_ptr));
+ //sysError("erase 0x" + String((uint32_t)flash_ptr));
NVMCTRL->ADDR.reg = ((uint32_t)flash_ptr);
NVMCTRL->CTRLB.reg = NVMCTRL_CTRLB_CMDEX_KEY | NVMCTRL_CTRLB_CMD_EB;
while (!NVMCTRL->INTFLAG.bit.DONE) { }
diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md
index 3cc14d8257672ff166fda8f3d980105685deda4f..813d5d91969a866fa8b315dbf3612eb39f2db7fb 100644
--- a/log/cl-step-control-log.md
+++ b/log/cl-step-control-log.md
@@ -972,4 +972,27 @@ First up, I'm hanging this thing up when I try to run it in an interrupt: might
Ah - no, this is probably because I am running the control loop in an interrupt, but the var is never returning because that happens in *another* interrupt.
-OK, have 50khz of this going on, now I need to apply some twerk vector and see if I can poke it around.
\ No newline at end of file
+OK, have 50khz of this going on, now I need to apply some twerk vector and see if I can poke it around.
+
+To track performance:
+- read takes 7.7us
+- w/ all the whistles, down to the set, it's about 10us even
+- 7.9us w/o the write to the dacs, so that's 2us alone
+
+OK, now I just have to prevent this from running when things are non-calib'd. Or maybe there should be a dummy table in there, to start? Have to turn the timer IRQ off during calib, let's see...
+
+OK, yep, but now the loop ranges from 40us to 10us because of this line:
+
+```cpp
+ float ra = lut[result];
+ // to find the phase, want to find modulus this ra / 1.8
+ while(ra > 1.8F){
+ ra -= 1.8F;
+ }
+```
+
+Not cool. Also seems pretty terrence regardless: easy to stall.
+
+Ah, I was mapping 1.8 - 1.0, should have been 7.2 - 1.0: the 0-1 'phase angle' sweeps thru 4 full steps, not one.
+
+OK well this is a decent minimum viable thing: it commutates. There's (I think) an obvious relationship between control loop and maximum RPM, so it's probably all maths and optimization from here out. The encoder read is the the obvious bottleneck, so is that modulus operation. I'm not totally sure I'm applying phase angle ideally... ah yeah, obviously better results when I do 180* phase rotation rather than 90, why did I think it was 90? Thing's rippen at 5krpm w/ no load, that should do pretty well, stoked to put it on an axis tomorrow and issue some commands from the browser to whip things around. Exciting futures for motion control.
\ No newline at end of file