Changeset 506

Timestamp:

05/27/09 00:35:35 (3 years ago)

Author:

auchtemm

Message:

some large changes to the code for PID. disabling needs to be handled in a cleaner way yet, as do serial timeouts.

Location:

trunk/electronics/avr/2008-mcc

Files:

• main.m4 (modified) (2 diffs)
• mkfile (modified) (1 diff)
• pid.m4 (modified) (7 diffs)
• pins.m4 (modified) (1 diff)
• pwm.m4 (modified) (2 diffs)

trunk/electronics/avr/2008-mcc/main.m4

@@ -94 +94 @@
 
 
-void delay_ms(uint16_t t) {
+void delay_ms(uint16_t t)
+{
         while (t--) _delay_ms(1);
 }
+
+void calibrate()
+{
+        pwm_set(0);
+        delay_ms(250);
+        pwm_set(255);
+        delay_ms(1000);
+        pwm_set(-255);
+        delay_ms(1000);
+        pwm_set(0);
+        delay_ms(5000);
+}
+
 
 int main()
@@ -109 +123 @@
 
         pwm_init();
+        pwm_set(0);
+
+        //while (1)
+        //      calibrate();
+
+
 //      spi_init();
 //      mcp_init(Loopback, msg_handler1);

trunk/electronics/avr/2008-mcc/mkfile

@@ -1 +1 @@
 <$RHRTELEC/avr/mkhdr
 
-MCU_SPEED=20000000
+MCU_SPEED=20000000ULL
 TARG=pid
 OFILES=\

trunk/electronics/avr/2008-mcc/pid.m4

@@ -33 +33 @@
 int16_t set_point;
 int16_t ticks;
-int16_t distance;
 uint8_t disabled;
+int8_t enc_inc;
+
+#define MAX_I_TERM (INT16_MAX-1)
+#define PWM_MAX 255
 
 /* All gains scaled by 128 */
@@ -50 +53 @@
         int16_t error_sum;
         int16_t last_error;
+        int16_t max_error;
+        int16_t max_error_sum;
 
 } PID;
@@ -94 +99 @@
         PID.Ki = nKi;
         PID.Kd = nKd;
+        PID.max_error = INT16_MAX / (nKp + 1);
+        PID.max_error_sum = MAX_I_TERM / (nKi + 1);
+        PID.error_sum = 0;
 }
 
@@ -131 +139 @@
         PIN_INPUT(ENCA);
         PIN_INPUT(ENCB);
-        PIN_INPUT(KILL);
-        
+        PIN_INPUT(ENCPOL);
+        PIN_INPUT(DISI);
+        PIN_OUTPUT(DIS);
+        PIN_SET(DIS);
+
         pid_disable();
-        //pid_set_gain(190, 17, 120);
+
+        if (PIN_LEVEL(ENCPOL))
+                enc_inc = 1;
+        else
+                enc_inc = -1;
+
         /* Interrupt on rising edge */
         EICRA |= 0b00001100;
         EIMSK |= 0b00000010;
 
-        PID.Kp = eeprom_read_word((uint16_t *)EAkp);
-        PID.Ki = eeprom_read_word((uint16_t *)EAki);
-        PID.Kd = eeprom_read_word((uint16_t *)EAkd);
+        pid_set_gain(eeprom_read_word((uint16_t *)EAkp), eeprom_read_word((uint16_t *)EAki), eeprom_read_word((uint16_t *)EAkd));
+        PID.error_sum = 0;
 
         /* Use Timer0 to trigger PID update
@@ -150 +165 @@
 #if PIDUPDATE == PID50HZ 
         TCCR0B = 0b00000101;
-        OCR0A = 194;
+        OCR0A = 194; // Actually, this is 100Hz
 #elif PIDUPDATE == PID100HZ
         TCCR0B = 0b00000101;
@@ -160 +175 @@
 }
 
+
 /* INT1 is encoder channel A 
  * To enable interrupt on rising edge of INT1:
@@ -167 +183 @@
 ISR(INT1_vect)
 {
-        cli();
         if (PIN_LEVEL(ENCB))
-                enc_count++;
+                enc_count += enc_inc;
         else
-                enc_count--;
-        sei();
-}
+                enc_count -= enc_inc;
+}
+
 
 /* PID update loop */
 ISR(TIMER0_COMPA_vect)
 {
+        int32_t rv, tmp;
+
         ticks = enc_count;
-        PID.error = set_point - enc_count;
         enc_count = 0;
 
-        PID.p = PID.Kp * PID.error;
-
-        PID.error_sum += PID.error;
+        PID.error = set_point - ticks;
+
+        if (PID.error >= PID.max_error)
+                PID.p = INT16_MAX;
+        else if (PID.error < -PID.max_error)
+                PID.p = -INT16_MAX;
+        else
+                PID.p = PID.Kp * PID.error;
+
+
+        tmp = PID.error_sum + PID.error;
+        if (tmp >= PID.max_error_sum)
+                PID.error_sum = PID.max_error_sum;
+        else if (tmp < -PID.max_error_sum)
+                PID.error_sum = -PID.max_error_sum;
+        else 
+                PID.error_sum = tmp;
+
         PID.i = PID.Ki * PID.error_sum;
 
 
-        PID.d = PID.Kd * (PID.error - PID.last_error);
+        PID.d = PID.Kd * (PID.last_error - PID.error);
         PID.last_error = PID.error;
 
 
-        if (PIN_LEVEL(KILL) == 0 || disabled)
-                pwm_set(0);
-        else
-                pwm_set((PID.p + PID.i + PID.d) >> 7);
-
-
-}
-
+        rv = (PID.p + PID.i + PID.d) / 128;
+
+        if (rv >= PWM_MAX)
+                rv = PWM_MAX;
+        else if (rv < -PWM_MAX)
+                rv = -PWM_MAX;
+
+        pwm_set(rv);
+
+        if (PIN_LEVEL(DISI)) {
+                PIN_SET(DIS);
+        } else {
+                PID.error_sum = 0;
+                PIN_CLR(DIS);
+        }
+}
+

trunk/electronics/avr/2008-mcc/pins.m4

@@ -14 +14 @@
 define(TXD, `DDRD, PORTD, PIND, PD1')
 
-define(KILL, `DDRD, PORTD, PIND, PD5')
+define(DISI, `DDRD, PORTD, PIND, PD7')
+define(DIS, `DDRB, PORTB, PINB, PB2')
 
+define(ENCPOL, `DDRD, PORTD, PIND, PD5')

trunk/electronics/avr/2008-mcc/pwm.m4

@@ -12 +12 @@
 void pwm_init()
 {
-        PIN_OUTPUT(PWM);
-
         /* f_pwm = f_clk/(2 * n * TOP) */
-        TCCR1A = 0b10100000;
+        TCCR1A = 0b10000000;
         TCCR1B = 0b00010010;
 
-        ICR1 = 10000;
+        ICR1 = 12500;
         pwm_set(0);
+        PIN_OUTPUT(PWM);
 }
 
@@ -27 +26 @@
  *  1500 = neutral
  *  2000 = full forward
- *  sp should always be in the range [-256, 255]
  */
 void pwm_set(int16_t sp)
 {
-        if (sp >= 255)
-                sp = 255;
-        else if (sp <= -256)
-                sp = -256;
+        int16_t tmp;
 
-        OCR1A = 1500 + (sp << 1);
+        tmp = sp * 2;
+        if (tmp > 512)
+                tmp = 512;
+        else if (tmp < -512)
+                tmp = -512;
         
+        tmp += 1875;
+        OCR1A = tmp;
 }
 
 int16_t pwm_get()
 {
-        return (OCR1A - 1500) >> 1;
+        return (OCR1A - 1875) / 2;
 }