Making vel and acceleration ref active at the same time.

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