Velocity control with low jitter on simulink communication

linear_pendulum_2009/avr/vel_control.c

@@ -28,12 +28,14 @@ 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
+//#define ENCODERY  (PIND&(uint8_t)(1<<2))        //Positional encoder pins
+//#define ENCODERX  (PINB&(uint8_t)(1<<1))        //Positional encoder pins
+#define ENCODERY  (PIND&0x04)        //Positional encoder pins
+#define ENCODERX  ((PINB&0x02)<<1)        //Positional encoder pins
+
 volatile int16_t pos = 0;
 volatile int16_t posTemp = 0;
 volatile int16_t oldPos = 0;
-volatile int16_t deltaPos = 0;
 volatile int8_t newX;
 volatile int8_t newY;
 volatile int8_t oldX;
@@ -86,46 +88,81 @@ void setPos()
 
 }
 
-/* Timer 2, Encoder */
+/* Timer 2, Encoder counter, 73 kHz */
 SIGNAL(SIG_OUTPUT_COMPARE2) {
-  
-  deltaPos = 0;
+
+   
+
   // Update position from encoder
   newX = ENCODERX;
   newY = ENCODERY;
-  if((newX != oldX) || (newY != oldY))                            //Check if any value changed
-    {
+  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;
+	pos = pos+1;
       } else if (sum == 1 || sum == 7 || sum ==  8 || sum == 14) {
-	//pos = pos-1;
-	deltaPos = -1;
+	pos = pos-1;
+      } else {
+	brake = 1; // emergency brake
+      }
+      */
+      if ((oldX == newY) && (oldY != newX)) {
+	pos = pos+1;
+      } else if ((oldX != newY) && (oldY == newX)) {
+	pos = pos-1;
       } else {
 	brake = 1;
-	// emergency brake
       }
       oldX = newX;
       oldY = newY;
-    }
+  }
+
+
+
+}
+
+
+/* Timer 1, serial communication */
+SIGNAL(SIG_OUTPUT_COMPARE0) {
+
+  TIMSK &= ~(BV(OCIE1A)|BV(OCIE0));
 
+  sei(); // enable interrupts from timer 0 and timer 2
+  
+  //PORTC |= 0x10;  // to clock calulation time
 
-  // velocity estimation cut-off frequency 500 Hz
-  pos = pos+deltaPos;    // update oldPos, 0 frac bits on pos and oldPos
+  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;
+  }
+  //PORTC &= ~0x10; 
   
+  TIMSK |= (BV(OCIE1A)|BV(OCIE0)); 
+ 
 }
 
 
 
-/* Timer 0, control loop */
-SIGNAL(SIG_OUTPUT_COMPARE0) {
+/* Timer 0, control loop , 1 kHz */
+SIGNAL(SIG_OUTPUT_COMPARE1A) {
 
+  TIMSK &= ~(BV(OCIE0)|BV(OCIE1A));
+  
   sei(); // to enable interupts from timer2
 
-  TIMSK &= ~BV(OCIE0);
-
-  PORTC |= 0x10;  // to clock calulation time
+   
 
   // linear velocity estimator
   // velocity estimate in mm/s 
@@ -221,41 +258,12 @@ SIGNAL(SIG_OUTPUT_COMPARE0) {
   // stop transmission
   outp(BV(TWINT)|BV(TWEN)|BV(TWSTO),TWCR);
 
+ 
 
+  TIMSK |= (BV(OCIE0)|BV(OCIE1A));
 
 
-  // ------- Noncritical section -------
 
-  
-  // Poll UART receiver
-  
-  int8_t tempVar = 0;
-  for (tempVar = 0;tempVar < 10; tempVar++) {
-    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;
-      while (toSend >= 0) {
-	UDR = (char)toSend;
-	while ((UCSRA & (1<<UDRE)) == 0) {} // wait for previous send
-	toSend = pccom_getNextByteToSend();
-      }
-    }
-  }
-  
-    
-  PORTC &= ~0x10;
-  TIMSK |= BV(OCIE0);
 }
 
 
@@ -268,22 +276,54 @@ int main()
   outp(0x10,DDRC);  // timer calculation port
 
   /* Timer section */
-  // Enable timer2 compare match interrupts
-  outp(BV(OCIE0)|BV(OCIE2),TIMSK);
+  // Enable timer0, timer1, timer2 compare match interrupts
+  outp(BV(OCIE0)|BV(OCIE1A)|BV(OCIE2),TIMSK);
   
-  /* Timer 2, 59 kHz Prescaler 1 */
+  /* Timer 2, 73 kHz Prescaler 1 */
   outp(BV(WGM21)|BV(CS20),TCCR2);
   outp(200,OCR2);
   /* Reset timer 2 */
   outp(0,TCNT2);
+  
+  // --------old-----------------------------------------------
+  /* Timer 1, Prescaler 1, 73/8 kHz*/
+  
+  //outp(BV(WGM12)|BV(CS10),TCCR1B);
+  //outp(0x06,OCR1AH);
+  //outp(0x40,OCR1AL);
+  //outp(0,TCNT1H);
+  //outp(0,TCNT1L);
+  
 
   /* Timer 0, 1 kHz Prescaler 64 */
+  
+  //outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
+  //outp(230,OCR0);
+  
+  /* Reset timer 0 */
+  //outp(0,TCNT0);
+  
+  // --------new-----------------------------------------------
+  /* Timer 1, 1 kHz */
+  outp(BV(WGM12)|BV(CS10),TCCR1B);
+  outp(0x39,OCR1AH);
+  outp(0x99,OCR1AL);
+  outp(0,TCNT1H);
+  outp(0,TCNT1L);
+  
+
+  /* Timer 0, 10 kHz, Prescaler 64 */
   outp(BV(WGM01)|BV(CS01)|BV(CS00),TCCR0);
-  outp(230,OCR0);
+  //outp(200,OCR0);
+  outp(23,OCR0); // 10 kHz
+
   /* Reset timer 0 */
   outp(0,TCNT0);
+  //----------------------------------------------------------
 
   
+  
+  
   //Serial communication
   outp(0x00, UCSRA);	// USART:
   outp(0x18, UCSRB);	// USART: RxEnable|TxEnable