/* MotorControl.Asm Copyright ©1998, 1999 Larry Barello Author: Larry Barello lbarello@accessone.com You are allowed to use this file for personal use only. You may use, embed, modify or distribute the contents of this file in any way you see fit as long as you include this copyright notice in the resulting file. Any commercial or industrial use including publishing or distributing in any form requires the written authorization of the copyright holder(s). No guarantees are made about fitness, functionality, correctness or performance of this file. The file is supplied on a "AS IS" basis, with no responsibility assumed by the author(s) for bug fixes, support, or damages of any sort arising from the use of this file. Contact the author with problem reports or fixes, feature requests or additions and questions. They are welcomed and will be incorporated as time and energy permits. */ #include "registers.inc" #include "AvrX.inc" #include "MotorControl.inc" PUBLIC MotorCtlTcb RightMotor LeftMotor PUBLIC SetLeftMotor SetRightMotor PUBLIC MOTOR LEFT_DIRa RIGHT_DIRa EXTERN mult16x16s LimitWord Limit32x16s EXTERN AvrXStartTimer AvrXWaitTimer EXTERN AvrXReadEEProm EXTERN _ReturnFromRestore _ReturnFromSave AvrXBreakpoint EXTERN SysCycleTime SysFilterSize EXTERN DoMotion ;+ ; ; PWM control bit and port assignments ; ;#define ALWAYSENABLED ; MOTOR = PORTA MOTOR_DDR = DDRA MOTOR_DDR_INIT = 0x0F ; PA0-3 = motor control ; ; Right Motor ; PD5 = OC1A ; RIGHT_DRIVEH = OCR1AH RIGHT_DRIVEL = OCR1AL RIGHT_DIRa = PA1 RIGHT_DIRb = PA0 ; ; Left Motor ; Pin 29 (OC1B) ; LEFT_DRIVEH = OCR1BH LEFT_DRIVEL = OCR1BL LEFT_DIRa = PA3 LEFT_DIRb = PA2 ; ;- RSEG AVRXDATA:DATA(0) MotorCtlPid: DS PidSz MotorCtlClk: DS TcbSz RSEG SSEG:DATA(1) ; ; Stacks grow down, so make their labels at the end ; DS 10 ; StandardContext DS 8 ; Additional User context DS 4 ; User Stack (two calls deep) DS 1 ; plus one interrupt MotorCtlStack: DS 1 RSEG DATA:DATA(0) iSysTime: DS 2 LeftMotor: DS MotorStruct DS 1 RightMotor: DS MotorStruct RSEG CODE ;---------------------------------------------- ; MotorCtlTcb: DW MotorCtlStack ; Stack DW MotorCtl>>1 ; Start Address DB MotorCtlPid, MotorCtlContexSize DW MotorCtlContext>>1 DB 02 ; Priority EVEN ; ; Additional registers saved for the motor control task. ; These are used primarily for the math package ; MotorCtlContexSize = 8 MotorCtlContext: sbiw Yl, (MotorCtlContexSize-1) brts SysSave ld R18, Y+ ld R19, Y+ ld R20, Y+ ld R21, Y+ ld R22, Y+ ld R23, Y+ ld R24, Y+ ld R25, Y rjmp _ReturnFromRestore SysSave: st Y+, R18 st Y+, R19 st Y+, R20 st Y+, R21 st Y+, R22 st Y+, R23 st Y+, R24 st Y , R25 rjmp _ReturnFromSave ; ; Motor Control Task ; MotorCtl: ; ; Enable control bits and PWM output A ; ldi Zl, MOTOR_DDR_INIT out MOTOR_DDR, Zl sbi DDRD, PD5 #ifdef ALWAYSENABLED sbi MOTOR, LEFT_DIRb ; Force driver enable high (BRAKE) sbi MOTOR, RIGHT_DIRb #endif ldi Zl, low(SysCycleTime) ldi Zh, High(SysCycleTime) ldi Xl, low(iSysTime) ldi Xh, high(iSysTime) ldi Yh, 2 rcall InitializeData ldi Xl, low(LeftMotor+Kproportional) ldi Xh, high(LeftMotor+Kproportional) ldi Yh, SysFilterSize rcall InitializeData ldi Xl, low(RightMotor+Kproportional) ldi Xh, high(RightMotor+Kproportional) ldi Yh, SysFilterSize rcall InitializeData ; ; Main Control Loop. The cycle time determines the ; response limits of the overall system ; CtlLoop: lds Yl, iSysTime+1 lds Yh, iSysTime ldi Xl, low(MotorCtlClk) rcall AvrXStartTimer ldi Zl, low(LeftMotor) ; Point to data struct ldi Zh, high(LeftMotor) rcall DoMotion mov R0, LeftEncoder ; Grab and reset counter sub LeftEncoder, R0 ; rcall _DoControl ; Update struct rcall SetLeftMotor ; ldi Zl, low(RightMotor) ldi Zh, high(RightMotor) rcall DoMotion mov R0, RightEncoder ; Grab and reset counter sub RightEncoder, R0 rcall _DoControl rcall SetRightMotor ldi Xl, low(MotorCtlClk) rcall AvrXWaitTimer rjmp CtlLoop ;+ ;---------------------------------------------- ; _DoControl ; ; Perform power calculation ; ; Passed: Z = Motor Control Struct ; R0= Current encoder value (Velocity) ; Returns: Y = abs(powerlevel) limited to 8, 9 or 10 bits ; T = Forward Flag ; USES: R0, R18-29, Flags ;- _DoControl: ; ; Add incremental encoder value (essentially velocity) ; to current position (32bit) ; mov R18, R0 clr R19 ; Sign Extend to 32 bits sbrc R18, 0x07 ser R19 mov R20, R19 mov R21, R19 ldd Xl, Z+CurrentPosition+3 ldd Xh, Z+CurrentPosition+2 ldd Yl, Z+CurrentPosition+1 ldd Yh, Z+CurrentPosition add R18, Xl adc R19, Xh adc R20, Yl adc R21, Yh std Z+CurrentPosition+3, R18 std Z+CurrentPosition+2, R19 std Z+CurrentPosition+1, R20 std Z+CurrentPosition, R21 ; ; Calculate the proportional error ; ldd Xl, Z+DesiredPosition+3 ldd Xh, Z+DesiredPosition+2 ldd Yl, Z+DesiredPosition+1 ldd Yh, Z+DesiredPosition sub Xl, R18 ; Y:X = Ep = (Desired - Current) sbc Xh, R19 sbc Yl, R20 sbc Yh, R21 ; ; Limit error to 16 bits, signed ; ldi Wl, low(0x7FFF) ldi Wh, high(0x7FFF) rcall Limit32x16s ; Y:X = Ep (Only X significant) mov R18, Xl ; R19:R18 = Ep mov R19, Xh ; ; Integrate Ep ; ldd R20, Z+IntegralError+1 ldd R21, Z+IntegralError+0 clr R22 sbrc R21, 7 com R22 add Xl, R20 adc Xh, R21 adc Yl, R22 adc Yh, R22 ; ; Limit Ei to Il bits, signed ; ldd Wl, Z+IntegralLimit+1 ldd Wh, Z+IntegralLimit rcall Limit32x16s ; ; Store ; std Z+IntegralError+1, Xl std Z+IntegralError+0, Xh ; ; Multiply Ei * Ki ; ldd Wl, Z+Kintegral+1 ldd Wh, Z+Kintegral rcall mult16x16s ; ; Divide (Ei * Ki)/256 ; mov R20, Xh mov R21, Yl ; R20:R21 = Ei * Ki limited by Il mov R22, Yh clr R23 sbrc Yh, 7 ser R23 ; ; Check derivative sampling period: ; 0-255 control cycles per derivative sample period ; ldd R0, Z+DerivativeCounter tst R0 breq DoDerivative dec R0 rjmp Continue DoDerivative: ldd Xl, Z+PrevPositionError+1 ; Recover Ep[n'-1] ldd Xh, Z+PrevPositionError std Z+PrevPositionError+1, R18 ; Store Ep[n'] std Z+PrevPositionError, R19 sub Xl, R18 sbc Xh, R19 ; X = Ed = Ep[n'-1] - Ep[n'] ldd Wl, Z+Kderivative+1 ldd Wh, Z+Kderivative rcall mult16x16s ; Y:X = Ed * Kd ldi Wl, low(0x7FFF) ; Limit Ed * Kd to 16 bits, signed ldi Wh, high(0x7FFF) rcall Limit32x16s std Z+DerivativeError+1, Xl std Z+DerivativeError, Xh ldd R0, Z+DerivativeSampleRate ; Reload Sample Counter Continue: std Z+DerivativeCounter, R0 ; ; Subtract derivative term from Integral term ; ldd Xl, Z+DerivativeError+1 ; Reload: only calculated ldd Xh, Z+DerivativeError ; at the derivative sample rate clr Yl ; Sign extend Ed term sbrc Xh, 7 ser Yl sub R20, Xl ; R24:R22 = Ei * Ki - Ed * Kd sbc R21, Xh sbc R22, Yl sbc R23, Yl ; ; Multiply Ep * Kp ; mov Xl, R18 mov Xh, R19 ldd Wl, Z+Kproportional+1 ldd Wh, Z+Kproportional rcall mult16x16s add Xl, R20 ; Add in previous terms adc Xh, R21 adc Yl, R22 adc Yh, R23 ldi Wl, low(0x7FFF) ldi Wh, high(0x7FFF) rcall Limit32x16s ; ; Limit control signal 8, 9 or 10 bits ; bst Xh, 7 brtc _dc02 com Xl com Xh adiw Xl, 1 ; Y = abs(y), T=sign (Velocity Error) _dc02: lsr Xh ror Xl lsr Xh ror Xl lsr Xh ror Xl lsr Xh ror Xl lsr Xh ror Xl ; Divide our results by 32 ldd Wl, Z+ControlFlags sbrc Wl, PWM10BIT ; Shift out 10th bit? rjmp Try9Bits lsr Xh ror Xl Try9Bits: sbrc Wl, PWM9BIT ; Shift out 9th bit? rjmp Done lsr Xh ror Xl Done: ret ;+ ;----------------------------------------- ; InitializeData ; ; Copy initialization data from EEPROM to ram ; ; PASSED: Z = EEPROM, X = Destination address, Yh = Size ; RETURNS: ; USES: Z, X and Y ; NOTES: Z points to next location in EEPROM ;- InitializeData: rcall AvrXReadEEProm st X+, Yl adiw Zl, 1 dec Yh brne InitializeData ret ;---------------------------------------------- ; Limit8bits ; ; PASSED: X = 16 bit value to limit ; RETURN: X = limited value, T=1 if limited ; USES: R0, Wl, Wh, Flags ; Limit8bits: ldi Wl, low(0x007F) ldi Wh, high(0x007F) rjmp LimitWord ;+ ;---------------------------------------------- ; SetLeftMotor ; ; Passed: X = power level ; T = Reverse Flag ; Returns: ; USES: ; NOTES: Uses internal OCR in PWM mode ;- SetLeftMotor: brtc slm10 sbi MOTOR, LEFT_DIRa cbi MOTOR, LEFT_DIRb rjmp slm11 slm10: cbi MOTOR, LEFT_DIRa sbi MOTOR, LEFT_DIRb slm11: out LEFT_DRIVEH, Xh ; Note hardware is little endian out LEFT_DRIVEL, Xl ret ;+ ;---------------------------------------------- ; SetRightMotor ; ; Passed: X = power level ; T = Reverse Flag ; Returns: ; USES: ; NOTES: Uses internal OCR in PWM mode. ;- SetRightMotor: brtc swm21 sbi MOTOR, RIGHT_DIRa cbi MOTOR, RIGHT_DIRb rjmp swm22 swm21: cbi MOTOR, RIGHT_DIRa sbi MOTOR, RIGHT_DIRb swm22: out RIGHT_DRIVEH, Xh out RIGHT_DRIVEL, Xl ret END