PI-current-control, with logging, 0.56 s runtime, measurement y-route 1 nF filter'

linear_pendulum_2009/avr/current_control.c

@@ -17,7 +17,7 @@
 
 
 // reference generation variables
-volatile int16_t ref = 200;
+volatile int16_t ref = 100;
 volatile int16_t refCount = 0;
 volatile int8_t refFlag = 0;
 
@@ -25,14 +25,30 @@ volatile int8_t refFlag = 0;
 
 // control variables
 volatile unsigned char lbyte,hbyte;  
-volatile int16_t result;
-volatile int16_t ctrl; 
-volatile int16_t ctrl_out;
-volatile int16_t temp;
+volatile int16_t y;
 volatile uint8_t alt = 1;
-
 volatile uint8_t low, high;
 
+//test variables
+volatile int16_t e = 0;
+volatile int16_t v = 0;
+volatile int32_t I = 0;
+volatile int16_t u = 0;
+volatile int16_t K = 20;//5;//375; // 7 frac bits
+volatile int16_t Ke = 8;//2;//6; //7 frac bits, K*h/Ti
+volatile int16_t Ksat = 51;//1; // 7 frac bits, h/Tr
+
+
+// logging variables
+#define log_len 100
+volatile int16_t ctrl_log[log_len];
+volatile int16_t error_log[log_len];
+volatile int32_t I_log[log_len];
+volatile int16_t skipSamples = 200;
+volatile int16_t countSamples = 0;
+volatile int16_t jj=0;
+volatile int8_t stop = 0;
+volatile int16_t temp;
 
 
 /* Routine used to set the red LED */
@@ -86,48 +102,97 @@ static inline void writeOutput(int16_t val) {
 }
 
 
+/* Send logged data over Serial connection */
+static inline void sendData() {
+  int16_t ii = 0;
+  while (ii < log_len) {
+
+    putchar((unsigned char) ((ctrl_log[ii]&0xff00)>>8));
+    putchar((unsigned char) (ctrl_log[ii]&0x00ff));
+
+    putchar((unsigned char) ((error_log[ii]&0xff00)>>8));
+    putchar((unsigned char) (error_log[ii]&0x00ff));
+
+    putchar((unsigned char) ((I_log[ii]&0xff000000)>>24));
+    putchar((unsigned char) ((I_log[ii]&0x00ff0000)>>16));
+
+    putchar((unsigned char) ((I_log[ii]&0x0000ff00)>>8));
+    putchar((unsigned char) (I_log[ii]&0x000000ff));
+
+
+    ii++;
+  }
+}
+
 
 /* Interrupt when AD-conversion completes */
 SIGNAL(SIG_ADC)
 {
   setPA4(1);
 
+  
   // Read input
   low = inp(ADCL);
   high = inp(ADCH);
-  result =  ((high<<8) | low) - 512;
-  
+  y =  ((high<<8) | low) - 512; //y 9 frac bits
+
   // Scale incoming current measurement
-  result = result*3; //!!!!!!!!!!!!!!!!!!!!!!!
+  //y = y*3; //!!!!!!!!!!!!!!!!!!!!!!!
 
   // Scale incoming current measurement
-  // result += 13;
-    
-  //control
-  ctrl = ref-result;
-  ctrl = ((ctrl + (1<< 2)) >> 3);
+  y += 70;
+ 
+  //control, since negative measurements
+  e = ref+y; //e 9 frac bits
+
   
+  v = (int16_t)(((K*e+(1<<6)) >> 7)+(int16_t)((I+(1<<6))>>7));
+
   //saturation/rounding to 8 bit
-  if (ctrl > 511)
-    ctrl_out = 511;
-  else if (ctrl < -511)
-    ctrl_out = -511;
+  if (v > 511)
+     u = 511;
+  else if (v < -512)
+    u = -512;
   else
-    ctrl_out = ctrl;
+    u = v;
 
+  I = I +((int32_t)Ke)*((int32_t)e) + ((int32_t)Ksat)*((int32_t)(u-v)); //16 frac bits 
+
+  // write output, inverting mode means set pwm to 127-ctrl_out
+  
+  u = (u)>>2; //7 frac bits to pwm 
 
-  // write output
-  if (ctrl_out < 0) {
+
+
+  if (u < 0) {
     PORTC = 0x80+(PORTC & 0x7F);
-    OCR1BL = (unsigned char) (-ctrl_out);
+    OCR1BL = (unsigned char) (128-(-u));
   } else {
     PORTC = (PORTC & 0x7F);
-    OCR1BL = (unsigned char) (ctrl_out);
+    OCR1BL = (unsigned char) (127-u);
+  }
+
+  // For logging
+  countSamples++;
+  if (countSamples == skipSamples) {
+    ctrl_log[jj] = y;
+    I_log[jj] = I;
+    error_log[jj] = e;
+    jj++;
+    countSamples = 0;
+  }
+  
+  if ((jj == (log_len-1)) & !stop) {
+    outp(0x7f,OCR1BL);
+    stop = 1;
+    sendData();
   }
+  
   setPA4(0);
 }
 
 
+
 /* reference square- or triangle wave generator with timer 0 */
 SIGNAL(SIG_OVERFLOW0) {
   int8_t rectangle = 1;
@@ -146,12 +211,12 @@ SIGNAL(SIG_OVERFLOW0) {
       }
     }
   } else {
-    if (refCount <= 400) {  // ref*2
-      ref -= 1;
+    if (refCount <= 20) {  // ref*2
+      ref -= 2;
     } else {
-      ref += 1;
+      ref += 2;
     }
-    if (refCount == 800) { // ref*4
+    if (refCount == 40) { // ref*4
       refCount = 0;
     }
   }
@@ -160,6 +225,7 @@ SIGNAL(SIG_OVERFLOW0) {
 
 int main()
 {
+  cli();
   int i,j;
   
   //Port directions
@@ -176,16 +242,17 @@ int main()
   // Enable timer0 overflow interrupts
   outp(BV(TOIE0),TIMSK);
   
-  /* Timer 1, 8 bit fast PWM no prescaling -> h_pwm=17.35 micros */
-  outp(BV(COM1A1)|BV(COM1B1)|BV(WGM10),TCCR1A);
-  // Fast PWM
-  // outp(BV(WGM12)|BV(CS10),TCCR1B);
-  // Phase and frequency correct pwm
-  outp(BV(CS10),TCCR1B);
-  /* Reset Timer1*/
+  // Timer 1, fast PWM no prescaling (inverting mode (start low, switch to high))
+  outp(BV(COM1A1)|BV(COM1B1)|BV(COM1A0)|BV(COM1B0)|BV(WGM11)|BV(WGM10),TCCR1A);
+  outp(BV(CS10)|BV(WGM13)|BV(WGM12),TCCR1B);
   
-  //outp(0x00,TCNT1H);   		// Reset TCNT1 high
-  //outp(0x00,TCNT1L);		// Reset TCNT1 low
+  // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> h_pwm=8.61 micros)
+  outp(0x00,OCR1AH);
+  outp(0x7f,OCR1AL);
+  outp(0x00,TCNT1H);
+  outp(0x00,TCNT1L);
+  outp(0x00,OCR1BH);
+  outp(0x7f,OCR1BL); // to not start motor-rotation before control
 
   
   
@@ -216,14 +283,19 @@ int main()
   
   /* Wait a little bit, probably not needed...*/
   int tmp;
-  for(i=0;i<2000;i++) 
-    for(j=0;j<400;j++)
-      tmp = j*j*j;
+  //  for(i=0;i<2000;i++) 
+  //   for(j=0;j<400;j++)
+  //   tmp = j*j*j;
   
 
   //Enable interrupts
   sei();
 
   // loop
-  while (1) {}
+  while (1) {
+    if (stop) {
+      cli();
+      OCR1BL = 0x7f;
+    }
+  }
 }