diff --git a/linear_pendulum_2009/avr/vel_control.c b/linear_pendulum_2009/avr/vel_control.c
index a022b7c752af1fa1ccc93cef4da8f0700a106e0c..b65e5f140de8ebb0b67de010a31277a64aeafa35 100644
--- a/linear_pendulum_2009/avr/vel_control.c
+++ b/linear_pendulum_2009/avr/vel_control.c
@@ -43,23 +43,30 @@
static volatile uint8_t pccom_poll=0;
-// Keep alive timer
-volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates
-
// reference variables
-volatile int32_t g_vel_ref = 0; // 11 frac bits (variable in mm/s, max 2^12)
+struct {
+ volatile uint16_t samples; // nbr of samples since last ctrl-ref update
+ struct {
+ volatile unsigned char pending;
+ volatile int32_t next;
+ volatile int32_t value;
+ } vel;
+ struct {
+ volatile int32_t value;
+ } acc;
+} ref = {
+ .samples = 0,
+ .vel.pending = 0,
+ .vel.next = 0,
+ .vel.value = 0,
+ .acc.value = 0,
+};
volatile int32_t refCtrl = 0; // ref used in ctrl-loop (=ref sent from simulink)
-volatile int32_t g_acc_ref = 0; // 14 frac bits (variable in mm/s, max 2^15)
// velocity control variables
volatile int32_t u = 0; // 11 frac bits
-volatile int32_t v = 0; // 11 frac bits
-volatile int32_t vSat = 0;
volatile int8_t brake = 0; // brake variable if pos-sample missed
volatile int32_t I = 0; // 11 frac bits
-volatile int32_t e = 0; // 11 frac bits
-volatile int8_t intCond = 0;
-
volatile int32_t K = 807; // 6 frac bits, prop constant
volatile int32_t Ke = 13; // 6 frac bits, integral constant
@@ -133,14 +140,23 @@ int32_t getCurrentRef() {
/* Set new acceleration reference value */
void setAccRef(int32_t newAccRef) {
- g_acc_ref = newAccRef;
- nbrSamples = 0;
+ // Called from serial interrupt, so should be atomic by itself
+ if (ref.vel.pending) {
+ ref.vel.pending = 0;
+ ref.vel.value = ref.vel.next;
+ ref.acc.value = newAccRef;
+ ref.samples = 0;
+ } else {
+ // TODO: report error
+ }
}
/* Set new velocity reference value */
void setVelRef(int32_t newVelRef) {
- g_vel_ref = newVelRef;
+ // Called from serial interrupt, so should be atomic by itself
+ ref.vel.pending = 1;
+ ref.vel.next = newVelRef;
}
/* Routine used to initialize the positional encoders */
@@ -246,64 +262,67 @@ SIGNAL(TWI_vect) {
/* Timer 0, control loop , 1 kHz */
SIGNAL(TIMER1_COMPA_vect) {
- posCtrl = pos; // store pos to use in this loop
- int32_t vel_ref = g_vel_ref;
- int32_t acc_ref = g_acc_ref;
- if (nbrSamples < 65535) {
- nbrSamples++;
+ if (ref.samples <= 500) {
+ ref.samples++;
}
- sei(); // to enable interupts from encoder counter and TWI
+ posCtrl = pos; // store pos to use in this loop
+ int32_t vel_ref = ref.vel.value;
+ int32_t acc_ref = ref.acc.value;
+ int16_t samples = ref.samples;
+ sei(); // to enable interupts from encoder counter, serial and TWI
// velocity estimation in mm/s
velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos)); // 5 fracbits on velEst
oldPos = posCtrl;
-
- // store velEst and ref to be sent/used here
-// cli();
- refCtrl = vel_ref + ((acc_ref*nbrSamples)>>10); //shift nbrSamples 10 steps (= nbrSamples*h)
- if (nbrSamples > 500) {
- refCtrl = 0; // for safety reasons if doesn't send anything in 0.5 s
- }
- // vel_ref = vel_ref*(1-brake); // emergency stop
- // control error
- e = refCtrl-((velEst+16)>>5); // mm/s
+ if (samples > 500) {
+ // Communication lost, stop and reset
+ I = 0;
+ // Protect u and TWCR for concurrent updates
+ cli();
+ u = 0;
+ } else {
+ // store velEst and ref to be sent/used here
+ refCtrl = vel_ref + ((acc_ref*samples)>>10); //shift nbrSamples 10 steps (= nbrSamples*h)
+
+ // control error
+ int32_t e = refCtrl-((velEst+16)>>5); // mm/s
- // temporary ctrl-signal
- v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
+ // temporary ctrl-signal
+ int32_t v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
- // friction compensation
- if (refCtrl > 0) {
- v = v+fr_comp;
- } else if (refCtrl < 0) {
- v = v-fr_comp;
- }
+ // friction compensation
+ if (refCtrl > 0) {
+ v = v+fr_comp;
+ } else if (refCtrl < 0) {
+ v = v-fr_comp;
+ }
- // variable that decides if I-part should be updated
- intCond = 1;
+ // variable that decides if I-part should be updated
+ int8_t intCond = 1;
- // saturation of v
- if (v > V_MAX) {
- vSat = V_MAX;
- if (e > 0)
- intCond = 0;
- } else if (v < V_MIN) {
- vSat = V_MIN;
- if (e < 0)
- intCond = 0;
- } else {
- vSat = v;
- }
+ // saturation of v
+ int32_t vSat;
+ if (v > V_MAX) {
+ vSat = V_MAX;
+ if (e > 0) { intCond = 0; }
+ } else if (v < V_MIN) {
+ vSat = V_MIN;
+ if (e < 0) { intCond = 0; }
+ } else {
+ vSat = v;
+ }
- if (intCond)
- I = I + (((Ke*e)+(1<<1))>>2);
-
- // Protect u and TWCR for concurrent updates
- cli();
-
- // scale ctrl-signal to send over twi
- u = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114
+ if (intCond) {
+ I = I + (((Ke*e)+(1<<1))>>2);
+ }
+ // Protect u and TWCR for concurrent updates
+ cli();
+
+ // scale ctrl-signal to send over twi
+ u = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114
+ }
// TWI-communication to set current reference on the other atmel
// send start command
@@ -353,15 +372,9 @@ int main()
//Serial communication initialization
- UCSRA = 0x00; // USART:
- UCSRB = 0x18; // USART: RxEnable|TxEnable
- UCSRC = 0x86; // USART: 8bit, no parity
- UBRRH = 0x00; // USART: 115200 @ 14.7456MHz
- UBRRL = 7; // USART: 115200 @ 14.7456MHz
-
UCSRA = 0x00; // USART:
UCSRB = 0x98; // USART: RxIntEnable|RxEnable|TxEnable
- UCSRC = 0x86; // USART: 8bit, no parity, 9600
+ UCSRC = 0x86; // USART: 8bit, no parity
UBRRH = 0x0; // USART: 115200 @ 14.7456MHz
UBRRL = 7; // USART: 115200 @ 14.7456MHz