http://www.barello.net/Papers/Motion_Control/index.htm --------------------------------PID-------- int DoPid(MotorInfo *p) { int Perror, output; // Set point is an 24.8 number (+/- 1 mile@ 130/inch) Perror = (p->Setpoint>>8) - p->Encoder; // Derivative error is the delta Perror over Td output = (p->Kp*Perror + p->Kd*(Perror - p->PrevErr) + p->Ki*p->Ierror)/p->Ko; p->PrevErr = Perror; // Accumulate Integral error *or* Limit output. // Stop accumulating when output saturates if (output >= MAXOUTPUT) output = MAXOUTPUT; else if (output <= -MAXOUTPUT) output = -MAXOUTPUT; else p->Ierror += Perror; // Convert output to 0xFF-0x7F-0x00 (Fwd, neutral, Rev) // Locked anti-phase drive. return (output - (MAXOUTPUT+1)); } ------------------------------------pwm control------- typedef struct { long Setpoint; // 24.8 number long Encoder; // Current position int8_t Kp; int8_t Kd; int8_t Td; int8_t Ki; int8_t Ko; // Output Scale int16_t PrevErr; int16_t Ierror; int16_t Velocity; int16_t VelocitySetpoint; int16_t Acceleration; } MotorInfo, * pMotorInfo; #define MAXOUTPUT 0x7F -------------------------------------quad decode ;+ ;Left/RightEncoderInterrupt ; ;Called from PWM overflow interrupt. Does quadrature decoding of our ;encoder inputs. This gives us 4x resolution on our 45 segment encoder ;wheels (total counts/revolution = 180) ; ;There are a bunch of invalid state changes (state0 -> state2 for example) ; and this code completely ignores those possibilities. If the rate of ; change is lower than the polling rate of this code, then there can't ; be any invalid state changes. ;- A = 1<<0 ; Encoder A Line B = 1<<1 ; Encoder B line STATE0 = 0 ; Increment if 0->1, 1->2, 2->3 or 3->0 STATE1 = B ; Decrement if 0->3, 3->2, 2->1 or 1->0 STATE2 = A + B STATE3 = A ProcessEncoder: clr Xh sbic SWITCH, Right_Enc_A sbr Xh, A sbic SWITCH, Right_Enc_B sbr Xh, B ldi Zl, low(RightMotor) ldi Zh, high(RightMotor) rcall DecodeState clr Xh sbic SWITCH, Left_Enc_A sbr Xh, A sbic SWITCH, Left_Enc_B sbr Xh, B ldi Zl, low(LeftMotor) ldi Zh, high(LeftMotor) DecodeState: ldd Xl, Z+PreviousState ldd Yl, Z+Encoder+1 ldd Yh, Z+Encoder cp Xh, Xl breq DecodeExit State0: cpi Xl, STATE0 brne State1 cpi Xh, STATE1 breq Increment cpi Xh, STATE3 breq Decrement rjmp DecodeExit State1: cpi Xl, STATE1 brne State2 cpi Xh, STATE2 breq Increment cpi Xh, STATE0 breq Decrement rjmp DecodeExit State2: cpi Xl, STATE2 brne State3 cpi Xh, STATE3 breq Increment cpi Xh, STATE1 breq Decrement rjmp DecodeExit State3: cpi Xh, STATE0 breq Increment cpi Xh, STATE2 brne DecodeExit Decrement: sbiw Yl, 2 ; Save a word and a cycle by falling Increment: ; through adiw Yl, 1 DecodeExit: std Z+PreviousState, Xh std Z+Encoder+1, Yl std Z+Encoder, Yh ret -------------------------------------------------control code------- extern MotorInfo Left, Right; AVRX_TASKDEF(MotorTask, 28, 2) { pMotorInfo pLeft=&Left, pRight=&Right; int8_t LeftEnc, RightEnc; extern int8_t RightEncoder, LeftEncoder; MotorInit(); while(1) { AvrXStartTimer(&MotorTimer, MILLISECOND(10)); AvrXSetSemaphore(&MotorLoop); // Synchronize other motion tasks BeginCritical(); // Read encoders LeftEnc = LeftEncoder; LeftEncoder = 0; RightEnc = RightEncoder; RightEncoder = 0; EndCritical(); pLeft->Encoder += LeftEnc; // Update motor structures pRight->Encoder += RightEnc; DoMotion(pLeft); // Do motion control outp(DoPid(pLeft), LEFT_DRIVEL);// Do PID and output power level DoMotion(pRight); outp(DoPid(pRight), RIGHT_DRIVEL); AvrXWaitTimer(&MotorTimer); // Wait for timer to expire } }