Step response again, with lower pwm resultion in order to match pwm-cycle with...

Step response again, with lower pwm resultion in order to match pwm-cycle with sampling period, an attempt to reduce noise (but not very succesful).

linear_pendulum_2009/avr/current_control.c

@@ -17,7 +17,7 @@
 
 
 // reference generation variables
-volatile int16_t ref = 100;
+volatile int16_t ref = 0;
 volatile int16_t refCount = 0;
 volatile int8_t refFlag = 0;
 
@@ -32,10 +32,10 @@ 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 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 K = 51;//5;//375; // 7 frac bits
+volatile int16_t Ke = 20;//2;//6; //7 frac bits, K*h/Ti
 volatile int16_t Ksat = 51;//1; // 7 frac bits, h/Tr
 
 
@@ -44,7 +44,7 @@ volatile int16_t Ksat = 51;//1; // 7 frac bits, h/Tr
 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 skipSamples = 1;
 volatile int16_t countSamples = 0;
 volatile int16_t jj=0;
 volatile int8_t stop = 0;
@@ -58,12 +58,6 @@ void setLED(uint8_t on)
   else PORTB |= 0x80;	//Turn off
 }
 
-/* Routine used to set pin PD7 */
-void setPA4(uint8_t on)
-{
-  if (on == 0) PORTA &= 0xef;	//Turn off
-  else PORTA |= 0x10;	//Turn on
-}
 
 /* Routine used to transmit bytes on the serial port */
 static void putchar(unsigned char ch)
@@ -128,67 +122,89 @@ static inline void sendData() {
 /* Interrupt when AD-conversion completes */
 SIGNAL(SIG_ADC)
 {
-  setPA4(1);
-
+  PORTA |= 0x10;	//Turn on calculation timer
 
   // Read input
   low = inp(ADCL);
   high = inp(ADCH);
   y =  ((high<<8) | low) - 512; //y 9 frac bits
  
-  // Scale incoming current measurement
-  //y = y*3; //!!!!!!!!!!!!!!!!!!!!!!!
-
-  // Scale incoming current measurement
-  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 (v > 511)
-     u = 511;
-  else if (v < -512)
-    u = -512;
-  else
-    u = v;
-
-  I = I +((int32_t)Ke)*((int32_t)e) + ((int32_t)Ksat)*((int32_t)(u-v)); //16 frac bits 
+  //v = (int16_t)((K*e+I+64)>>7);
+  v = (int16_t)(((K*e+64)>>7)+(temp>>4));
+  //saturation and update integral part of ctrl
+  if (v > 511) {
+    temp = temp + (((Ke*e) + (Ksat)*(511-v))>>3);
+  } else if (v < -512) {
+    temp = temp + (((Ke*e) + (Ksat)*(-512-v))>>3);
+  } else {
+    temp = temp + ((Ke*e)>>3);
+  }
 
+  /*
+  //saturation and update integral part of ctrl
+  if (v > 511) {
+    I = I +(int32_t)((Ke*e) + (Ksat)*(511-v));
+  } else if (v < -512) {
+    I = I +(int32_t)((Ke*e) + (Ksat)*(-512-v));
+  } else {
+    I = I +(int32_t)(Ke*e);
+  }
+  */
   // write output, inverting mode means set pwm to 127-ctrl_out
 
-  u = (u)>>2; //7 frac bits to pwm 
+  // Original styrning med 7 bitar + direction
+  /*
+  u = (v+2)>>2; //7 frac bits to pwm 
 
+  if (u > 127) {
+    u = 127;
+  } else if (u < -128) {
+    u = -128;
+  }
+  */
+
+  // upplsning p styrsignal [-104,103] fr att matcha sampelperiod
+  u = ((v*13+32)>>6);
   
+  if (u > 104) {
+    u = 104;
+  } else if (u < -105) {
+    u = -105;
+  }
 
   if (u < 0) {
-    PORTC = 0x80+(PORTC & 0x7F);
-    OCR1BL = (unsigned char) (128-(-u));
+    //PORTC = 0x80+(PORTC & 0x7F);
+    PORTC |= 0x80;
+    OCR1BL = (unsigned char) (104-(-u));
   } else {
     PORTC = (PORTC & 0x7F);
-    OCR1BL = (unsigned char) (127-u);
+    OCR1BL = (unsigned char) (105-u);
   }
 
   // For logging
   countSamples++;
   if (countSamples == skipSamples) {
     ctrl_log[jj] = y;
-    I_log[jj] = I;
+    I_log[jj] = u;
     error_log[jj] = e;
     jj++;
     countSamples = 0;
-  }
      
+    // Stop after a while
     if ((jj == (log_len-1)) & !stop) {
       outp(0x7f,OCR1BL);
       stop = 1;
       sendData();
     }
+    if (jj == 30)
+      ref = 100;
+  }
 
-  setPA4(0);
+  PORTA &= 0xef;	//Turn off calculation timer  
 }
 
 
@@ -247,13 +263,25 @@ int main()
   outp(BV(CS10)|BV(WGM13)|BV(WGM12),TCCR1B);
   
   // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> h_pwm=8.61 micros)
+  // 7 bit ctrl-signal and direction
+  //outp(0x00,OCR1AH);
+  //outp(0x7f,OCR1AL);
+
+  // 104-quant on ctrl-signal and direction
   outp(0x00,OCR1AH);
-  outp(0x7f,OCR1AL);
+  outp(104,OCR1AL);
+
   outp(0x00,TCNT1H);
   outp(0x00,TCNT1L);
+
+  // 7 bit ctrl-signal and direction
+  //outp(0x00,OCR1BH);
+  //outp(0x7f,OCR1BL); // to not start motor-rotation before control
+
   outp(0x00,OCR1BH);
-  outp(0x7f,OCR1BL); // to not start motor-rotation before control
+  outp(104,OCR1BL); // to not start motor-rotation before control
 
+  // 104-quant on ctrl-signal and direction
   
   
   /* Timer 2 (control loop), prescaler 256, clear on compare match (28), -> h = 0.5 ms */