Working velocity control without simulink communication

linear_pendulum_2009/avr/vel_control.c

+#include <avr/twi.h>
+#include <avr/io.h>
+#include <avr/signal.h>
+#include <avr/interrupt.h>
+#include <inttypes.h>
+
+
+// reference variables
+volatile int32_t ref = 0;  // 11 frac bits
+volatile int16_t refFlag = 0;
+volatile int16_t deltaRef = 2;
+volatile int16_t refCount = 0;
+
+
+// velocity control variables
+volatile int32_t u = 0;  // 11 frac bits
+volatile int32_t v = 0;  // 11 frac bits
+volatile int8_t brake = 0;
+volatile int32_t I = 0; // 11 frac bits
+volatile int32_t e = 0; // 11 frac bits
+volatile int32_t K = 1200; // 6 frac bits
+volatile int32_t Ke = 45; // 6 frac bits
+volatile int32_t Ksat = 3; // 6 frac bits
+
+// encoder variables
+#define ENCODERY  (PIND&(uint8_t)(1<<2))        //Positional encoder pins
+#define ENCODERX  (PINB&(uint8_t)(1<<1))        //Positional encoder pins
+volatile int16_t pos = 0;
+volatile int16_t oldPos = 0;
+volatile int16_t deltaPos = 0;
+volatile int8_t newX;
+volatile int8_t newY;
+volatile int8_t oldX;
+volatile int8_t oldY;
+volatile int8_t sum = 0;
+
+// velocity estimation parameters
+volatile int32_t velEst = 0; // 5 frac bits
+volatile int32_t velEstTemp = 0; 
+volatile int16_t posTemp = 0;
+int16_t a = 116; //7 frac bits
+int16_t b = 152; // 5 frac bits
+
+
+/* Routine used to transmit bytes on the serial port */
+static void putchar(unsigned char ch)
+{
+	while ((inp(UCSRA) & 0x20) == 0) {};
+	outp(ch, UDR);
+	while ((inp(UCSRA) & 0x20) == 0) {};
+}
+
+/* Interrupt service routine for handling incoming bytes on the serial port 
+ might be needed to catch incoming bytes */
+SIGNAL(SIG_UART_RECV){}
+
+
+/* Send logged data over Serial connection */
+static inline void sendData() {
+
+}
+
+
+
+/* Routine used to set the red LED */
+void setLED(uint8_t on)
+{
+  if (on) PORTB &= ~0x80; //Turn on
+  else PORTB |= 0x80;     //Turn off
+}
+
+/* Routine used to initialize the positional encoders */
+void initPos()
+{
+  oldX = ENCODERX;
+  oldY = ENCODERY;
+}
+
+/* Routine used to track the cart position */
+void setPos()
+{
+
+}
+
+/* Timer 2, Encoder */
+SIGNAL(SIG_OUTPUT_COMPARE2) {
+  
+  deltaPos = 0;
+  // Update position from encoder
+  newX = ENCODERX;
+  newY = ENCODERY;
+  if((newX != oldX) || (newY != oldY))                            //Check if any value changed
+    {
+      sum = (oldX<<2)+oldY+newX+(newY>>2);                    //Find state
+      if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {      //Predetermined values determine direction
+	//pos = pos+1;
+	deltaPos = 1;
+      } else if (sum == 1 || sum == 7 || sum ==  8 || sum == 14) {
+	//pos = pos-1;
+	deltaPos = -1;
+      } else {
+	brake = 1;
+	// emergency brake
+      }
+      oldX = newX;
+      oldY = newY;
+    }
+
+
+  // velocity estimation cut-off frequency 500 Hz
+  pos = pos+deltaPos;    // update oldPos, 0 frac bits on pos and oldPos
+  
+}
+
+
+
+/* Timer 0, control loop */
+SIGNAL(SIG_OUTPUT_COMPARE0) {
+
+  sei(); // to enable interupts from timer2
+
+  PORTC |= 0x10;  // to clock calulation time
+
+  // linear velocity estimator
+  // velocity estimate in mm/s 
+  velEst = (((a*velEst+64)>>7)+b*(pos-oldPos));  // 5 fracbits on velEst
+  oldPos = pos;
+
+  // control error
+  //e = ref-((int16_t)((velEst+16)>>5));  // mm/s
+  e = ref-((velEst+16)>>5);  // mm/s
+  
+  v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));
+
+  //saturation and update integral part of ctrl
+  if (v > 2047) {
+    I = I + ((((Ke*e) + (Ksat)*(2047-v))+(1<<1))>>2);
+  } else if (v < -2048) {
+    I = I + ((((Ke*e) + (Ksat)*(-2048-v))+(1<<1))>>2);
+  } else {
+    I = I + ((Ke*e+(1<<3))>>2);
+  }
+  
+  u = (v+8)>>4; //8 frac bits to current loop
+
+  // friction compensation
+  if (ref > 0) {
+    u = u+10;
+  } else if (ref < 0) {
+    u = u-10;
+  }
+
+  // Saturation
+  if (u > 127) {
+    u = 127;
+  } else if (u < -128) {
+    u = -128;
+  }
+
+  // reference calculations
+  refCount++;
+  
+  
+  if (refFlag == 0) {
+    if  (refCount%1 == 0)
+      ref = ref+deltaRef;
+    if (refCount == 500) {
+      refFlag = 1;
+      deltaRef = -deltaRef;
+      refCount = 0;
+    }
+  } else {
+    if  (refCount%1 == 0)
+      ref = ref+deltaRef;
+    if (refCount == 1000) {
+      refCount = 0;
+      deltaRef = -deltaRef;
+    }
+  }
+  
+
+  /*
+  if (refCount == 1000) {
+    ref = 0;
+  }
+  */
+
+  
+  pos = pos*(1-brake);
+
+  
+  // TWI-communication
+  // 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
+  outp((int8_t)(u&0x000000ff),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);
+
+
+  
+  //velEstTemp = velEst;
+  /*
+  putchar((unsigned char) ((((velEstTemp+16)>>5)&0x0000ff00)>>8));
+  putchar((unsigned char) (((velEstTemp+16)>>5)&0x000000ff));
+
+  //putchar((unsigned char) ((deltaPos&0xff00)>>8));
+  //putchar((unsigned char) (deltaPos&0x00ff));
+  */
+  
+  putchar((unsigned char) ((velEst&0xff000000)>>24));
+  putchar((unsigned char) ((velEst&0x00ff0000)>>16));
+
+  putchar((unsigned char) ((velEst&0x0000ff00)>>8));
+  putchar((unsigned char) (velEst&0x000000ff));
+
+  putchar((unsigned char) ((I&0xff000000)>>24));
+  putchar((unsigned char) ((I&0x00ff0000)>>16));
+
+  putchar((unsigned char) ((I&0x0000ff00)>>8));
+  putchar((unsigned char) (I&0x000000ff));
+  
+
+  PORTC &= ~0x10;  
+
+}
+
+
+int main()
+{
+  cli();
+  
+  //Port directions
+  outp(0x80,DDRB);   // Led output
+  outp(0x10,DDRC);  // timer calculation port
+
+  /* Timer section */
+  // Enable timer2 compare match interrupts
+  outp(BV(OCIE0)|BV(OCIE2),TIMSK);
+  
+  /* Timer 2, 59 kHz Prescaler 1 */
+  outp(BV(WGM21)|BV(CS20),TCCR2);
+  outp(200,OCR2);
+  /* Reset timer 2 */
+  outp(0,TCNT2);
+
+  /* Timer 0, 1 kHz Prescaler 64 */
+  outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
+  outp(230,OCR0);
+  /* Reset timer 0 */
+  outp(0,TCNT0);
+
+  
+  //Serial communication
+  outp(0x00, UCSRA);	// USART:
+  outp(0x98, UCSRB);	// USART: RxIntEnable|RxEnable|TxEnable
+  outp(0x86, UCSRC);	// USART: 8bit, no parity
+  outp(0x00, UBRRH);	// USART: 115200 @ 14.7456MHz
+  outp(7,UBRRL);	// USART: 115200 @ 14.7456MHz
+  
+
+  
+  /* AREF (AREF is 5V) pin external capacitor, MUX0 for current */
+  // outp(BV(REFS0)|BV(MUX0),ADMUX); 	
+  
+  // Enable ADC, start first conversion, prescaler 32, not free running mode
+  // outp(BV(ADEN)|BV(ADSC)|BV(ADPS2)|BV(ADPS0),ADCSRA);
+
+
+  // Initialize Master TWI
+  outp(0x10,TWBR);  // set SCL-frequency CPU-freq/(16+2*16)
+  outp(BV(TWEN),TWCR); // enable TWI
+
+  // initialize position measurements
+  initPos();
+  
+  //Enable interrupts
+  sei();
+
+  // loop
+  while (1) {}
+}