Commit 8a5477b7 authored by Jake Read's avatar Jake Read
Browse files

does torque-effort commands

parent 0edabfac
......@@ -120,10 +120,10 @@ void ENC_AS5047::txcISR(void){
ENC_SER_SPI.DATA.reg = outWord02;
firstWord = false;
} else {
ENC_CS_DESELECT;
ENC_SER_SPI.INTENCLR.bit.TXC = 1;
if(firstAction){
firstAction = false;
ENC_CS_DESELECT;
start_spi_interaction(AS5047_SPI_READ_POS);
}
}
......@@ -139,6 +139,7 @@ void ENC_AS5047::rxcISR(void){
} else {
inWord02 = data;
result = 0b0011111111111111 & ((inWord01 << 8) | inWord02);
ENC_CS_DESELECT;
on_read_complete(result);
readComplete = true;
}
......
......@@ -39,7 +39,7 @@ Step_CL::Step_CL(void){}
void Step_CL::init(void){
step_a4950->init(false, 0.4);
enc_as5047->init();
_tc = -1.0; // torque command -> 0;
_tc = 0; // torque command -> 0;
is_calibrating = false;
}
......@@ -58,7 +58,7 @@ and increment that void* thru the array in blocks, when our buffer is full
#define BYTES_PER_BLOCK 8192
#define FLOATS_PER_BLOCK 2048
const float __attribute__((__aligned__(8192))) lut[16834] = {}; // the actual LUT: const means it gets allocated to flash
const float __attribute__((__aligned__(8192))) lut[32768] = {}; // the actual LUT: const means it gets allocated to flash
FlashClass flashClass((const uint8_t*)lut); // helper class (lib) // https://github.com/cmaglie/FlashStorage
const void* block_ptr; // void* to section-of-lut for write
static float buffer[FLOATS_PER_BLOCK]; // one full block (16 pages) of flash mem, buffered
......@@ -92,9 +92,10 @@ void flash_write_value(float val){
void Step_CL::print_table(void){
sysError("reading from lut");
for(uint32_t i = 0; i < ENCODER_COUNTS; i ++){
float ra = lut[i];
sysError("real angle at enc " + String(i) + ": " + String(ra));
for(uint32_t i = 0; i < ENCODER_COUNTS * 2; i ++){
float ra = lut[i * 2];
float pa = lut[i * 2 + 1];
sysError("real angle at enc " + String(i) + ": " + String(ra) + "phase angle: " + String(pa));
delay(5);
}
}
......@@ -113,39 +114,36 @@ float Step_CL::get_torque(void){
// the control loop
void Step_CL::run_torque_loop(void){
if(is_calibrating) return;
// mark time
DEBUG1PIN_ON;
// 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)
volatile float _ra;
volatile float _pa;
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;
_ra = lut[result * 2]; // the real angle (position 0-360)
_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
if(step_cl->get_torque() < 0){
ra -= 0.5; // 180* phase swop
if(ra < 0){
ra += 1.0F;
_pa -= 0.25; // 90* phase swop
if(_pa < 0){
_pa += 1.0F;
}
} else {
ra += 0.5;
if(ra > 1){
ra -= 1.0F;
_pa += 0.25;
if(_pa > 1){
_pa -= 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()));
step_a4950->point(_pa, abs(step_cl->get_torque()));
// debug loop completion
DEBUG1PIN_OFF;
}
......@@ -301,7 +299,13 @@ boolean Step_CL::calibrate(void){
delay(10);
}
// ok, have the real angle (ra) at the encoder tick (e), now write it
flash_write_value(ra); // this just happens in order, we zeroe'd out global counters at the start
flash_write_value(ra); // log the real angle here
float pa = ra;
while(pa > 7.2F){
pa -= 7.2F;
}
pa = pa * MAP_7p2_TO_1;
flash_write_value(pa); // log the phase angle beside it
} // end sweep thru 2^14 pts
sysError("calib complete");
is_calibrating = false;
......
......@@ -24,7 +24,7 @@ class Step_CL {
private:
static Step_CL* instance;
float calib_readings[201];
float _tc;
volatile float _tc;
public:
Step_CL();
......
......@@ -74,7 +74,6 @@ uint16_t tick = 0;
void TC0_Handler(void){
TC0->COUNT32.INTFLAG.bit.MC0 = 1;
TC0->COUNT32.INTFLAG.bit.MC1 = 1;
DEBUG1PIN_ON;
step_cl->run_torque_loop();
}
......@@ -171,6 +170,13 @@ void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t seg
step_cl->print_table();
// do work
break;
case AK_SET_TC: {
ptr ++;
reply[rl ++] = AK_SET_TC;
chunk_float32 tcs = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] }};
step_cl->set_torque(tcs.f);
break;
}
default:
break;
}
......
......@@ -51,6 +51,7 @@ no warranty is provided, and users accept all liability.
#define AK_RUNCALIB 121
#define AK_READCALIB 122
#define AK_SET_TC 123
// -------------------------------------------------------- MVC Endpoints
......
......@@ -899,6 +899,8 @@ OK this is crazy, I have every fn in the class commented out, and the thing stil
So, tried my way around that, I have honestly no idea what is going on here. I'm going to try writing from the base class again.
![bugfix](2020-10-14_bugfix.mp4)
OK, did that and seems like I'm writing into the thing. Crazy. Here's the code:
```cpp
......@@ -995,4 +997,48 @@ 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
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.
![motor](2020-10-14_cl-step-control.mp4)
## 2020 10 16
Next real steps for this are formulating it into a paper / research question, then getting into the work.
OK - the work. Am going to do a torque command from the browser first, full width.
That works, now I'm noticing some oddball behaviour - the control loop seems to lag depending on where the encoder reading is: I thought that was because the maths was taking longer (father to modulo down) but I might just have some pins mixed up? Ah - this was a mistaken end-of-transmission from the SPI level... fixed, I was correct orginally, just had the pin flipping confused.
So yeah, this is the long modulo. I guess it won't hurt to try writing a second LUT for phase angle, see if I can bring that execution time down... ready to brick in 3..2..1.. bricked haha, IDK how to align such a big table in RAM I guess.
So to store that in flash, I could perhaps build the LUT to be like [ra0, pa0, ra1, pa1, ..., ra1024, pa1024] and access with 2x or 2x + 1 indices. Let's see if I can write a flash table this long without any trouble.
OK, so my loop is now more like:
```cpp
_ra = lut[result * 2]; // the real angle (position 0-360)
_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
if(step_cl->get_torque() < 0){
_pa -= 0.25; // 90* phase swop
if(_pa < 0){
_pa += 1.0F;
}
} else {
_pa += 0.25;
if(_pa > 1){
_pa -= 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(_pa, abs(step_cl->get_torque()));
```
and runs 2.4us. The whole loop is now nicely just around 10us, so I could choke the whole micro and run 100kHz and probably trip up, or I'll try running ~ 75kHz, or I could be satisfied with 50 and leave overhead for the processor to do ... well ... other stuff, haha. I.E I will probably want this headroom for positioning PID, etc. Maybe I should just get it mounted on an axis and proceed with the next steps instead of fiddling with speed before I know it's necessary or warranted.
- consider writing another LUT for RA -> Phase Angle (PA)
- could do in RAM at startup, might crash ur shit tho
- do commands from the browser to jiggy jog
- put on an axis, see how fast it can swing it
- begin the maths, the control, etc... see if you can optimize that loop, high speed is ur friend
\ 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