Change to new style I/O

Make TWI communication interrupt controlled

linear_pendulum_2009/avr/vel_control.c

@@ -10,7 +10,6 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <inttypes.h>
-#include <compat/deprecated.h>
 
 #include "vel_control.h"
 #include "serialio.h"
@@ -44,16 +43,16 @@
 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 vel_ref = 0;    // 11 frac bits (variable in mm/s, max 2^12)
+volatile int32_t g_vel_ref = 0;  // 11 frac bits (variable in mm/s, max 2^12)
 volatile int32_t refCtrl = 0;    // ref used in ctrl-loop (=ref sent from simulink)
-volatile int32_t acc_ref = 0;    // 14 frac bits (variable in mm/s, max 2^15)
-volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates
+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 uSend = 0;      // ctrl sent to simulink
 volatile int32_t v = 0;          // 11 frac bits
 volatile int32_t vSat = 0;
 volatile int8_t brake = 0;       // brake variable if pos-sample missed
@@ -62,7 +61,7 @@ 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 K = 807;        // 6 frac bits, prop constant
 volatile int32_t Ke = 13;        // 6 frac bits, integral constant
 volatile int8_t fr_comp = (10<<3);
 #define V_MAX (120<<4)
@@ -84,7 +83,6 @@ volatile int8_t oldY;            // previous encoder signal
 
 // velocity estimation parameters
 volatile int32_t velEst = 0;     // vel-estimate, 5 frac bits
-volatile int32_t velEstSend = 0; // vel-estimate sent to simulink
 int16_t a = 116;                 // 7 frac bits (parameter in velocity estimate)
 int16_t b = 152;                 // 5 frac bits (parameter in velocity estimate)
 
@@ -105,35 +103,44 @@ int32_t getPosition() {
 
 /* return velocity (in mm/s) */
 int32_t getVelocity() {
-  return velEstSend;
+  int32_t result;
+  cli();
+  result = velEst;
+  sei();
+  return result;
 }
 
 
 /* return last angle measurement */
 int16_t getAngle() {
-  low = inp(ADCL);
-  high = inp(ADCH);
-  int16_t result = ADC - 512 -angleOffset;
-  return result;
+  int16_t adc = ADC;
+  while (adc != ADC) {
+    adc = ADC;
+  }
+  return adc - 512 - angleOffset;
 }
 
 
 /* return current-reference */
 int32_t getCurrentRef() {
-  return uSend;
+  int32_t result;
+  cli();
+  result = u;
+  sei();
+  return result;
 }
 
 
 /* Set new acceleration reference value */
 void setAccRef(int32_t newAccRef) {
-  acc_ref = newAccRef;
+  g_acc_ref = newAccRef;
   nbrSamples = 0;
 }
 
 
 /* Set new velocity reference value */
 void setVelRef(int32_t newVelRef) {
-  vel_ref = newVelRef;
+  g_vel_ref = newVelRef;
 }
 
 /* Routine used to initialize the positional encoders */
@@ -149,26 +156,26 @@ SIGNAL(TIMER2_COMP_vect) {
   newX = ENCODERX;
   newY = ENCODERY;
   if((newX != oldX) || (newY != oldY)) { //Check if any value changed
-      /*
+    /*
       sum = (oldX<<2)+oldY+newX+(newY>>2);  
       if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
-	pos = pos+1;
+      pos = pos+1;
       } else if (sum == 1 || sum == 7 || sum ==  8 || sum == 14) {
-	pos = pos-1;
+      pos = pos-1;
       } else {
-	brake = 1; // emergency brake
+      brake = 1; // emergency brake
       }
-      */
+    */
     // Works like if-clauses above, but faster!
-      if ((oldX == newY) && (oldY != newX)) {
-	pos = pos+1;
-      } else if ((oldX != newY) && (oldY == newX)) {
-	pos = pos-1;
-      } else {
-	brake = 1;
-      }
-      oldX = newX;
-      oldY = newY;
+    if ((oldX == newY) && (oldY != newX)) {
+      pos = pos+1;
+    } else if ((oldX != newY) && (oldY == newX)) {
+      pos = pos-1;
+    } else {
+      brake = 1;
+    }
+    oldX = newX;
+    oldY = newY;
   }
 }
 
@@ -214,59 +221,50 @@ SIGNAL(USART_RXC_vect) {
   }
 }
 
-#if 0
-/* Timer 0, serial communication with simulink */
-SIGNAL(TIMER0_COMP_vect) {
-
-  TIMSK &= ~(_BV(OCIE0)|_BV(OCIE1A));   // Disable communication and ctrl-interrupts
-
-  sei(); // enable interrupts from encoder-counter
-  
-  // Poll UART receiver
-  uint8_t status = UCSRA;
-  if (status & (1<<RXC)) {
-    char ch = UDR;
-    pccom_receiveByte(ch);
-    
-    if (status & ((1<<FE)|(1<<DOR)|(1<<PE))) { 
-      //main_emergencyStop(); // stop on USART error
-    }     
-  }
-  
-  // Poll UART sender
-  if (UCSRA & (1<<UDRE)) {
-    int16_t toSend = pccom_getNextByteToSend();
-    if (toSend >= 0) UDR = (char)toSend;
+SIGNAL(TWI_vect) {
+  unsigned char twsr = TWSR;
+  switch (twsr) {
+    case 0x08: {
+      TWDR = 0x02; // slave is 0x02 (sla+w)
+      TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWIE);
+    } break;
+    case 0x18:
+    case 0x20: {
+      TWDR = u;
+      TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWIE);
+    } break;
+    case 0x28:
+    case 0x30: {
+      TWCR = _BV(TWINT)|_BV(TWEN)|_BV(TWSTO)|_BV(TWIE);
+    } break;
+    default: {
+      // This should never happen
+      TWCR = (_BV(TWINT)|_BV(TWEN)|_BV(TWIE));
+    } break;
   }
-
-  TIFR = (1<<OCF0);        // skip pending interrupts from serial comm, (but not from ctrl)
-
-  TIMSK |= (_BV(OCIE0)|_BV(OCIE1A)); // reenable communication and ctrl-interrupts
-
 }
-#endif
-
 
 /* Timer 0, control loop , 1 kHz */
 SIGNAL(TIMER1_COMPA_vect) {
   posCtrl = pos; // store pos to use in this loop
-
-  sei(); // to enable interupts from encoder counter and communication
+  int32_t vel_ref = g_vel_ref;
+  int32_t acc_ref = g_acc_ref;
+  if (nbrSamples < 65535) {
+    nbrSamples++;
+  }
+  sei(); // to enable interupts from encoder counter 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();
-  velEstSend = velEst;
-  nbrSamples++;
+//  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
-  sei();
 
   // control error
   e = refCtrl-((velEst+16)>>5);  // mm/s
@@ -300,32 +298,18 @@ SIGNAL(TIMER1_COMPA_vect) {
   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
 
-  // u that is sent to simulink
-  cli();
-  uSend = u;
-  sei();
-
 
   // TWI-communication to set current reference on the other atmel
   // send start command
-  outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTA),TWCR);
-  while (!(TWCR&_BV(TWINT))) {}
-    
-  // Contact slave  
-  outp(0x02,TWDR);  // slave is 0x02 (sla+w)
-  outp(_BV(TWINT)|_BV(TWEN),TWCR);
-  while (!(TWCR&_BV(TWINT))) {}
- 
-  // Send reference byte to slave
-  outp((int8_t)u,TWDR);  // send 8 bits reference
-  outp(_BV(TWINT)|_BV(TWEN),TWCR);
-  while (!(TWCR&_BV(TWINT))) {}
-    
-  // stop transmission
-  outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTO),TWCR);
+  if (TWCR == (_BV(TWEN)|_BV(TWIE)) || TWCR == 0) {
+    TWCR = _BV(TWEN)|_BV(TWSTA)|_BV(TWIE);
+  }
 }
 
 
@@ -334,36 +318,35 @@ int main()
   cli();
   
   //Port directions
-  //outp(0xf0,DDRB);   // Led output
-  outp(0x10,DDRC);  // timer calculation port
+  PORTC = 0x03;  // Pull up for systems without TWI resistors
   PORTD = 0x40;  // pull up on reset switch
 
 
   /* Timer section */
-  // Enable timer0, timer1, timer2 compare match interrupts
-  outp(_BV(OCIE0)|_BV(OCIE1A)|_BV(OCIE2),TIMSK);
-  outp(_BV(OCIE1A)|_BV(OCIE2),TIMSK);
+  // Enable timer1, timer2 compare match interrupts
+  TIMSK = _BV(OCIE1A)|_BV(OCIE2);
   
   /* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
-  outp(_BV(WGM21)|_BV(CS20),TCCR2);
-  outp(200,OCR2);
+  TCCR2 = _BV(WGM21)|_BV(CS20);
+  OCR2 = 200;
+
   /* Reset timer 2 */
-  outp(0,TCNT2);
+  TCNT2 = 0;
   
   /* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
-  outp(_BV(WGM12)|_BV(CS10),TCCR1B);
-  outp(0x38,OCR1AH);
-  outp(0x3f,OCR1AL);
-  outp(0,TCNT1H);
-  outp(0,TCNT1L);
+  TCCR1B = _BV(WGM12)|_BV(CS10);
+  OCR1AH = 0x38;
+  OCR1AL = 0x3f;
+  TCNT1H = 0;
+  TCNT1L = 0;
   
 
   /* Timer 0, 40 kHz, Prescaler 8, serial communication */
-  outp(_BV(WGM01)|_BV(CS01),TCCR0);
-  //outp(184-1,OCR0); // 10 kHz
-  outp(45-1,OCR0); // 40 kHz
+  TCCR0 = _BV(WGM01)|_BV(CS01);
+  //OCR0 = 184-1; // 10 kHz
+  OCR0 = 45-1; // 40 kHz
    /* Reset timer 0 */
-  outp(0,TCNT0);
+  TCNT0 = 0;
 
 
   // syncronization (ctrl interrupt 34 micros before communication interrupt)
@@ -371,11 +354,11 @@ int main()
   
   
   //Serial communication initialization
-  outp(0x00, UCSRA);	// USART:
-  outp(0x18, UCSRB);	// USART: RxEnable|TxEnable
-  outp(0x86, UCSRC);	// USART: 8bit, no parity
-  outp(0x00, UBRRH);	// USART: 115200 @ 14.7456MHz
-  outp(7,UBRRL);	// USART: 115200 @ 14.7456MHz 
+  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 
@@ -385,15 +368,15 @@ int main()
   
   
   /* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
-  outp(_BV(REFS0)|_BV(MUX0)|_BV(MUX1),ADMUX);
+  ADMUX = _BV(REFS0)|_BV(MUX0)|_BV(MUX1);
   
   // Enable ADC on adc3, start first conversion, prescaler 128, free running mode
-  outp(_BV(ADEN)|_BV(ADSC)|_BV(ADATE)|_BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0),ADCSRA);
+  ADCSRA = _BV(ADEN)|_BV(ADSC)|_BV(ADATE)|_BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0);
 
 
   // Initialize Master TWI
-  outp(0x10,TWBR);  // set SCL-frequency CPU-freq/(16+2*16)
-  outp(_BV(TWEN),TWCR); // enable TWI
+  TWBR = 0x10;  // set SCL-frequency CPU-freq/(16+2*16)
+  TWCR = _BV(TWEN)|_BV(TWIE);
 
   // initialize position measurements
   initPos();
@@ -404,6 +387,7 @@ int main()
   
   //Enable interrupts
   sei();
+  
 
   // loop
   while (1) {
@@ -411,8 +395,8 @@ int main()
     // reset position, velocity estimate and integral part of ctrl if reset button pushed
     if (!(PIND & 0x40)) {
       cli();
-      low = inp(ADCL);
-      high = inp(ADCH);
+      low = ADCL;
+      high = ADCH;
       pos = 0; // reset position
       angleOffset =  ((int16_t) ((high<<8) | low) - 512);
       oldPos = 0;