Commit 314736dd authored by Jake Read's avatar Jake Read
Browse files

50khz up

parent 1d328cff
...@@ -146,7 +146,6 @@ void ENC_AS5047::rxcISR(void){ ...@@ -146,7 +146,6 @@ void ENC_AS5047::rxcISR(void){
} }
void SERCOM4_2_Handler(void){ void SERCOM4_2_Handler(void){
DEBUG1PIN_TOGGLE;
enc_as5047->rxcISR(); enc_as5047->rxcISR();
} }
......
...@@ -13,8 +13,7 @@ is; no warranty is provided, and users accept all liability. ...@@ -13,8 +13,7 @@ is; no warranty is provided, and users accept all liability.
*/ */
#include "step_a4950.h" #include "step_a4950.h"
//#include "ucbus_drop.h" #include "../utils/clamp.h"
// sine, 0-8190, 4095 center / 'zero', 256 steps // sine, 0-8190, 4095 center / 'zero', 256 steps
#define LUT_LENGTH 1024 #define LUT_LENGTH 1024
...@@ -99,14 +98,6 @@ STEP_A4950* stepper_hw = STEP_A4950::getInstance(); ...@@ -99,14 +98,6 @@ STEP_A4950* stepper_hw = STEP_A4950::getInstance();
STEP_A4950::STEP_A4950() {} STEP_A4950::STEP_A4950() {}
void clamp(float *val, float min, float max){
if(*val < min){
*val = min;
} else if (*val > max){
*val = max;
}
}
void STEP_A4950::init(boolean invert, float cscale){ void STEP_A4950::init(boolean invert, float cscale){
// all of 'em, outputs // all of 'em, outputs
AIN1_PORT.DIRSET.reg = AIN1_BM; AIN1_PORT.DIRSET.reg = AIN1_BM;
......
...@@ -14,6 +14,7 @@ is; no warranty is provided, and users accept all liability. ...@@ -14,6 +14,7 @@ is; no warranty is provided, and users accept all liability.
#include "step_cl.h" #include "step_cl.h"
#include "../utils/FlashStorage.h" #include "../utils/FlashStorage.h"
#include "../utils/clamp.h"
Step_CL* Step_CL::instance = 0; Step_CL* Step_CL::instance = 0;
...@@ -26,11 +27,6 @@ Step_CL* Step_CL::getInstance(void){ ...@@ -26,11 +27,6 @@ Step_CL* Step_CL::getInstance(void){
Step_CL* step_cl = Step_CL::getInstance(); Step_CL* step_cl = Step_CL::getInstance();
// https://github.com/cmaglie/FlashStorage
// flash LUT
// FlashStorage(flash_lut, step_cl_calib_table_t);
// float __attribute__((__aligned__(256))) lut[16384];
Step_CL::Step_CL(void){} Step_CL::Step_CL(void){}
#define CALIB_CSCALE 0.4F #define CALIB_CSCALE 0.4F
...@@ -43,34 +39,32 @@ Step_CL::Step_CL(void){} ...@@ -43,34 +39,32 @@ Step_CL::Step_CL(void){}
void Step_CL::init(void){ void Step_CL::init(void){
stepper_hw->init(false, 0.4); stepper_hw->init(false, 0.4);
enc_as5047->init(); enc_as5047->init();
// this lut == stored lut _tc = 0; // torque command -> 0;
//lut = flash_lut.read();
} }
// #define BYTES_PER_PAGE 512 // LUT / flash work
// #define FLOATS_PER_PAGE 128 /*
// #define PAGES_PER_BLOCK 16 // for 8192 bytes / block the D51J19 flash is organized into *pages* of 512bytes,
and *blocks* of 16 pages (8192 bytes)
//const float __attribute__((__aligned__(BYTES_PER_PAGE))) lut[LUT_SIZE] = {}; write granularity is to the page, erase granularity is per block
//const void* page_ptr; we must erase flash before writing to it: this is how flash hardware works
to do flash storage of this monster table, we declare a const LUT array
const keyword will cause the compiler to put it in flash mem,
and so we point the helper class at that void* (the head of the array) to start,
and increment that void* thru the array in blocks, when our buffer is full
*/
#define BYTES_PER_BLOCK 8192 #define BYTES_PER_BLOCK 8192
#define FLOATS_PER_BLOCK 2048 #define FLOATS_PER_BLOCK 2048
const float __attribute__((__aligned__(8192))) lut[16834] = {}; const float __attribute__((__aligned__(8192))) lut[16834] = {}; // the actual LUT: const means it gets allocated to flash
FlashClass flashClass((const uint8_t*)lut); 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
// write mechanics static float buffer[FLOATS_PER_BLOCK]; // one full block (16 pages) of flash mem, buffered
const void* block_ptr;
// write buffer
static float buffer[FLOATS_PER_BLOCK];
uint32_t bfi = 0; // buffer indice uint32_t bfi = 0; // buffer indice
uint32_t bli = 0; // block indice uint32_t bli = 0; // block indice
//FlashStorage(flash_storage, flts);
void flash_write_init(void){ void flash_write_init(void){
block_ptr = (const uint8_t*) lut; block_ptr = (const uint8_t*) lut;
bfi = 0; bfi = 0;
...@@ -91,7 +85,7 @@ void flash_write_value(float val){ ...@@ -91,7 +85,7 @@ void flash_write_value(float val){
flash_write_page(); flash_write_page();
bfi = 0; bfi = 0;
bli ++; bli ++;
block_ptr = ((const uint8_t *)(&(lut[bli * FLOATS_PER_BLOCK]))); block_ptr = ((const uint8_t *)(&(lut[bli * FLOATS_PER_BLOCK])));
} }
} }
...@@ -99,11 +93,31 @@ void Step_CL::print_table(void){ ...@@ -99,11 +93,31 @@ void Step_CL::print_table(void){
sysError("reading from lut"); sysError("reading from lut");
for(uint32_t i = 0; i < ENCODER_COUNTS; i ++){ for(uint32_t i = 0; i < ENCODER_COUNTS; i ++){
float ra = lut[i]; float ra = lut[i];
sysError(String(ra)); sysError("real angle at enc " + String(i) + ": " + String(ra));
delay(5); delay(5);
} }
} }
// set twerks
// tc: -1 : 1
void Step_CL::set_torque(float tc){
clamp(&tc, -1.0F, 1.0F);
_tc = tc;
}
// the control loop
void Step_CL::run_torque_loop(void){
// ok, first we read the encoder
enc_as5047->trigger_read();
// this kicks off the party, proceeds below
}
void ENC_AS5047::on_read_complete(uint16_t result){
float ra = lut[result];
// debug loop completion
DEBUG1PIN_OFF;
}
// the calib routine // the calib routine
boolean Step_CL::calibrate(void){ boolean Step_CL::calibrate(void){
// (1) first, build a table for 200 full steps w/ encoder averaged values at each step // (1) first, build a table for 200 full steps w/ encoder averaged values at each step
......
...@@ -24,13 +24,16 @@ class Step_CL { ...@@ -24,13 +24,16 @@ class Step_CL {
private: private:
static Step_CL* instance; static Step_CL* instance;
float calib_readings[201]; float calib_readings[201];
float _tc;
public: public:
Step_CL(); Step_CL();
static Step_CL* getInstance(void); static Step_CL* getInstance(void);
void init(void); void init(void);
boolean calibrate(void);
void print_table(void); void print_table(void);
void set_torque(float tc);
void run_torque_loop(void);
boolean calibrate(void);
//float __attribute__((__aligned__(256))) lut[16384]; // nor does this ! //float __attribute__((__aligned__(256))) lut[16384]; // nor does this !
//float lut[16384]; // nor does this work //float lut[16384]; // nor does this work
//step_cl_calib_table_t lut; // not even this works ?? too big ?? //step_cl_calib_table_t lut; // not even this works ?? too big ??
......
...@@ -54,6 +54,8 @@ void setup() { ...@@ -54,6 +54,8 @@ void setup() {
ucBusDrop->init(false, BUS_DROP); ucBusDrop->init(false, BUS_DROP);
// cl controller // cl controller
step_cl->init(); step_cl->init();
// start ctrl timer for 100khz loop,
d51_clock_boss->start_50kHz_ticker_tc0();
} }
uint8_t bChPck[64]; uint8_t bChPck[64];
...@@ -69,11 +71,18 @@ volatile boolean setBlock = false; ...@@ -69,11 +71,18 @@ volatile boolean setBlock = false;
uint16_t tick = 0; 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();
}
// async loop
void loop() { void loop() {
DEBUG1PIN_TOGGLE;
osap->loop(); osap->loop();
stepper_hw->dacRefresh(); stepper_hw->dacRefresh();
/*
tick++; tick++;
if(tick > 500){ if(tick > 500){
tick = 0; tick = 0;
...@@ -82,6 +91,7 @@ void loop() { ...@@ -82,6 +91,7 @@ void loop() {
uint16_t reading = 0b0011111111111111 & enc_as5047->get_reading(); uint16_t reading = 0b0011111111111111 & enc_as5047->get_reading();
//sysError(String(reading)); //sysError(String(reading));
} }
*/
// do packet B grab / handle // do packet B grab / handle
/* /*
if(ucBusDrop->ctr_b()){ if(ucBusDrop->ctr_b()){
...@@ -125,10 +135,6 @@ void loop() { ...@@ -125,10 +135,6 @@ void loop() {
*/ */
} // end loop } // end loop
void ENC_AS5047::on_read_complete(uint16_t pos){
// do work
}
void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp){ 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 // track end of header, to reply with
uint16_t replyPtr = ptr; uint16_t replyPtr = ptr;
......
#include "clamp.h"
void clamp(float *val, float min, float max){
if(*val < min){
*val = min;
} else if (*val > max){
*val = max;
}
}
\ No newline at end of file
#ifndef CLAMP_H_
#define CLAMP_H_
void clamp(float *val, float min, float max);
#endif
\ No newline at end of file
...@@ -80,6 +80,40 @@ void D51_Clock_Boss::start_100kHz_ticker_tc0(void){ ...@@ -80,6 +80,40 @@ void D51_Clock_Boss::start_100kHz_ticker_tc0(void){
NVIC_EnableIRQ(TC0_IRQn); NVIC_EnableIRQ(TC0_IRQn);
} }
void D51_Clock_Boss::start_50kHz_ticker_tc0(void){
setup_16mhz_xtal();
// ok
TC0->COUNT32.CTRLA.bit.ENABLE = 0;
TC1->COUNT32.CTRLA.bit.ENABLE = 0;
// unmask clocks
MCLK->APBAMASK.reg |= MCLK_APBAMASK_TC0 | MCLK_APBAMASK_TC1;
// ok, clock to these channels...
GCLK->PCHCTRL[TC0_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(d51_clock_boss->mhz_xtal_gclk_num);
GCLK->PCHCTRL[TC1_GCLK_ID].reg = GCLK_PCHCTRL_CHEN | GCLK_PCHCTRL_GEN(d51_clock_boss->mhz_xtal_gclk_num);
// turn them ooon...
TC0->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV2 | TC_CTRLA_CAPTEN0;
TC1->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCSYNC_PRESC | TC_CTRLA_PRESCALER_DIV2 | TC_CTRLA_CAPTEN0;
// going to set this up to count at some time, we will tune
// that freq. with
TC0->COUNT32.WAVE.reg = TC_WAVE_WAVEGEN_MFRQ;
TC1->COUNT32.WAVE.reg = TC_WAVE_WAVEGEN_MFRQ;
// allow interrupt to trigger on this event (overflow)
TC0->COUNT32.INTENSET.bit.MC0 = 1;
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
// DFLL), 80 for 10us (with XTAL 16MHZ)
// 400 for 50us,
// enable, sync for enable write
while (TC0->COUNT32.SYNCBUSY.bit.ENABLE);
TC0->COUNT32.CTRLA.bit.ENABLE = 1;
while (TC0->COUNT32.SYNCBUSY.bit.ENABLE);
TC1->COUNT32.CTRLA.bit.ENABLE = 1;
// enable the IRQ
NVIC_EnableIRQ(TC0_IRQn);
}
void D51_Clock_Boss::start_100kHz_ticker_tc2(void){ void D51_Clock_Boss::start_100kHz_ticker_tc2(void){
setup_16mhz_xtal(); setup_16mhz_xtal();
// ok // ok
......
...@@ -35,6 +35,7 @@ class D51_Clock_Boss { ...@@ -35,6 +35,7 @@ class D51_Clock_Boss {
// builds 100kHz clock on TC0 or TC2 // builds 100kHz clock on TC0 or TC2
// todo: tell these fns a frequency, // todo: tell these fns a frequency,
void start_100kHz_ticker_tc0(void); void start_100kHz_ticker_tc0(void);
void start_50kHz_ticker_tc0(void);
void start_100kHz_ticker_tc2(void); void start_100kHz_ticker_tc2(void);
}; };
......
...@@ -964,4 +964,12 @@ boolean Step_CL::calibrate(void){ ...@@ -964,4 +964,12 @@ boolean Step_CL::calibrate(void){
So I think now I'll carry on actually running the calibration, then see if I can send a 'torque' command down / operate a control loop. So I think now I'll carry on actually running the calibration, then see if I can send a 'torque' command down / operate a control loop.
Great, this calibrates, so now I can roll a tiny control loop, yeah? Great, this calibrates, so now I can roll a tiny control loop, yeah?
\ No newline at end of file
### Rolling a Loop
First up, I'm hanging this thing up when I try to run it in an interrupt: might need to make some volatile vars.
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
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