From 314736dd701cffc81677afd28a41a3e59b7df7f7 Mon Sep 17 00:00:00 2001
From: Jake Read <jake.read@cba.mit.edu>
Date: Wed, 14 Oct 2020 18:26:59 -0400
Subject: [PATCH] 50khz up
---
.../src/drivers/enc_as5047.cpp | 1 -
.../src/drivers/step_a4950.cpp | 11 +---
.../src/drivers/step_cl.cpp | 64 +++++++++++--------
.../cl-step-controller/src/drivers/step_cl.h | 5 +-
firmware/cl-step-controller/src/main.cpp | 18 ++++--
.../cl-step-controller/src/utils/clamp.cpp | 9 +++
firmware/cl-step-controller/src/utils/clamp.h | 6 ++
.../src/utils/clocks_d51_module.cpp | 34 ++++++++++
.../src/utils/clocks_d51_module.h | 1 +
log/cl-step-control-log.md | 10 ++-
10 files changed, 115 insertions(+), 44 deletions(-)
create mode 100644 firmware/cl-step-controller/src/utils/clamp.cpp
create mode 100644 firmware/cl-step-controller/src/utils/clamp.h
diff --git a/firmware/cl-step-controller/src/drivers/enc_as5047.cpp b/firmware/cl-step-controller/src/drivers/enc_as5047.cpp
index ad377c8..2d1adf0 100644
--- a/firmware/cl-step-controller/src/drivers/enc_as5047.cpp
+++ b/firmware/cl-step-controller/src/drivers/enc_as5047.cpp
@@ -146,7 +146,6 @@ void ENC_AS5047::rxcISR(void){
}
void SERCOM4_2_Handler(void){
- DEBUG1PIN_TOGGLE;
enc_as5047->rxcISR();
}
diff --git a/firmware/cl-step-controller/src/drivers/step_a4950.cpp b/firmware/cl-step-controller/src/drivers/step_a4950.cpp
index 421601f..9f2cee4 100644
--- a/firmware/cl-step-controller/src/drivers/step_a4950.cpp
+++ b/firmware/cl-step-controller/src/drivers/step_a4950.cpp
@@ -13,8 +13,7 @@ is; no warranty is provided, and users accept all liability.
*/
#include "step_a4950.h"
-//#include "ucbus_drop.h"
-
+#include "../utils/clamp.h"
// sine, 0-8190, 4095 center / 'zero', 256 steps
#define LUT_LENGTH 1024
@@ -99,14 +98,6 @@ STEP_A4950* stepper_hw = STEP_A4950::getInstance();
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){
// all of 'em, outputs
AIN1_PORT.DIRSET.reg = AIN1_BM;
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.cpp b/firmware/cl-step-controller/src/drivers/step_cl.cpp
index 4ec2736..a3e65e4 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.cpp
+++ b/firmware/cl-step-controller/src/drivers/step_cl.cpp
@@ -14,6 +14,7 @@ is; no warranty is provided, and users accept all liability.
#include "step_cl.h"
#include "../utils/FlashStorage.h"
+#include "../utils/clamp.h"
Step_CL* Step_CL::instance = 0;
@@ -26,11 +27,6 @@ Step_CL* Step_CL::getInstance(void){
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){}
#define CALIB_CSCALE 0.4F
@@ -43,34 +39,32 @@ Step_CL::Step_CL(void){}
void Step_CL::init(void){
stepper_hw->init(false, 0.4);
enc_as5047->init();
- // this lut == stored lut
- //lut = flash_lut.read();
+ _tc = 0; // torque command -> 0;
}
-// #define BYTES_PER_PAGE 512
-// #define FLOATS_PER_PAGE 128
-// #define PAGES_PER_BLOCK 16 // for 8192 bytes / block
-
-//const float __attribute__((__aligned__(BYTES_PER_PAGE))) lut[LUT_SIZE] = {};
-//const void* page_ptr;
+// LUT / flash work
+/*
+the D51J19 flash is organized into *pages* of 512bytes,
+and *blocks* of 16 pages (8192 bytes)
+write granularity is to the page, erase granularity is per block
+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 FLOATS_PER_BLOCK 2048
-const float __attribute__((__aligned__(8192))) lut[16834] = {};
-FlashClass flashClass((const uint8_t*)lut);
-
-// write mechanics
-const void* block_ptr;
-
-// write buffer
-static float buffer[FLOATS_PER_BLOCK];
+const float __attribute__((__aligned__(8192))) lut[16834] = {}; // 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
uint32_t bfi = 0; // buffer indice
uint32_t bli = 0; // block indice
-//FlashStorage(flash_storage, flts);
-
void flash_write_init(void){
block_ptr = (const uint8_t*) lut;
bfi = 0;
@@ -91,7 +85,7 @@ void flash_write_value(float val){
flash_write_page();
bfi = 0;
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){
sysError("reading from lut");
for(uint32_t i = 0; i < ENCODER_COUNTS; i ++){
float ra = lut[i];
- sysError(String(ra));
+ sysError("real angle at enc " + String(i) + ": " + String(ra));
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
boolean Step_CL::calibrate(void){
// (1) first, build a table for 200 full steps w/ encoder averaged values at each step
diff --git a/firmware/cl-step-controller/src/drivers/step_cl.h b/firmware/cl-step-controller/src/drivers/step_cl.h
index b419de1..2b0b083 100644
--- a/firmware/cl-step-controller/src/drivers/step_cl.h
+++ b/firmware/cl-step-controller/src/drivers/step_cl.h
@@ -24,13 +24,16 @@ class Step_CL {
private:
static Step_CL* instance;
float calib_readings[201];
+ float _tc;
public:
Step_CL();
static Step_CL* getInstance(void);
void init(void);
- boolean calibrate(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 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 272f937..e9bcfae 100644
--- a/firmware/cl-step-controller/src/main.cpp
+++ b/firmware/cl-step-controller/src/main.cpp
@@ -54,6 +54,8 @@ void setup() {
ucBusDrop->init(false, BUS_DROP);
// cl controller
step_cl->init();
+ // start ctrl timer for 100khz loop,
+ d51_clock_boss->start_50kHz_ticker_tc0();
}
uint8_t bChPck[64];
@@ -69,11 +71,18 @@ volatile boolean setBlock = false;
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() {
- DEBUG1PIN_TOGGLE;
osap->loop();
stepper_hw->dacRefresh();
-
+ /*
tick++;
if(tick > 500){
tick = 0;
@@ -82,6 +91,7 @@ void loop() {
uint16_t reading = 0b0011111111111111 & enc_as5047->get_reading();
//sysError(String(reading));
}
+ */
// do packet B grab / handle
/*
if(ucBusDrop->ctr_b()){
@@ -125,10 +135,6 @@ void 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){
// track end of header, to reply with
uint16_t replyPtr = ptr;
diff --git a/firmware/cl-step-controller/src/utils/clamp.cpp b/firmware/cl-step-controller/src/utils/clamp.cpp
new file mode 100644
index 0000000..3238c8b
--- /dev/null
+++ b/firmware/cl-step-controller/src/utils/clamp.cpp
@@ -0,0 +1,9 @@
+#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
diff --git a/firmware/cl-step-controller/src/utils/clamp.h b/firmware/cl-step-controller/src/utils/clamp.h
new file mode 100644
index 0000000..0e18f65
--- /dev/null
+++ b/firmware/cl-step-controller/src/utils/clamp.h
@@ -0,0 +1,6 @@
+#ifndef CLAMP_H_
+#define CLAMP_H_
+
+void clamp(float *val, float min, float max);
+
+#endif
\ No newline at end of file
diff --git a/firmware/cl-step-controller/src/utils/clocks_d51_module.cpp b/firmware/cl-step-controller/src/utils/clocks_d51_module.cpp
index 317c403..de2bbe8 100644
--- a/firmware/cl-step-controller/src/utils/clocks_d51_module.cpp
+++ b/firmware/cl-step-controller/src/utils/clocks_d51_module.cpp
@@ -80,6 +80,40 @@ void D51_Clock_Boss::start_100kHz_ticker_tc0(void){
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){
setup_16mhz_xtal();
// ok
diff --git a/firmware/cl-step-controller/src/utils/clocks_d51_module.h b/firmware/cl-step-controller/src/utils/clocks_d51_module.h
index 084141e..2e0fcd8 100644
--- a/firmware/cl-step-controller/src/utils/clocks_d51_module.h
+++ b/firmware/cl-step-controller/src/utils/clocks_d51_module.h
@@ -35,6 +35,7 @@ class D51_Clock_Boss {
// builds 100kHz clock on TC0 or TC2
// todo: tell these fns a frequency,
void start_100kHz_ticker_tc0(void);
+ void start_50kHz_ticker_tc0(void);
void start_100kHz_ticker_tc2(void);
};
diff --git a/log/cl-step-control-log.md b/log/cl-step-control-log.md
index 168c8c2..3cc14d8 100644
--- a/log/cl-step-control-log.md
+++ b/log/cl-step-control-log.md
@@ -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.
-Great, this calibrates, so now I can roll a tiny control loop, yeah?
\ No newline at end of file
+Great, this calibrates, so now I can roll a tiny control loop, yeah?
+
+### 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
--
GitLab