Commit 0edabfac authored by Jake Read's avatar Jake Read
Browse files

cl commutates

parent 314736dd
......@@ -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() {}
......
......@@ -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
......@@ -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
}
......@@ -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 ??
......
......@@ -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));
......
......@@ -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) { }
......
......@@ -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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment