/* MotionControl.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" MODULE MOTIONCONTROL PUBLIC DoMotion StopMotion RSEG CODE /* DoMotion() Acceleration/velocity profiling code. Tries to emulate an L298 servo controller. Simplifies it by leaving out exception handling and shrinking the control values to 8.8 binary vs 16.16. Basic theory is that Acceleration and Velocity are represented as 8 bit signed numbers with 8 bits to the right of the decimal point. Acceleration is added to the Velocity with each call. Velocity is added to DesiredPosition with each call. When a roll over occurs between the fractional 8 bit part and the upper byte, then position (as represented by the encoder wheels) is advanced one count. In this way the velocity and the acceleration range is +/-127 to 1/127 counts per cycle. The resulting position information (DesiredPosition) has a trapazoidal velocity profile (S-curve positio) smoothly ramping up to the velocity setpoint, maintaining it until the appropriate moment and then smoothly ramping down to stop exactly on the PositionSetPoint. This code does not control the servos directly. It simply provides a series of target positions for the servo controller to match, thus providing the desired motion. DoMotion(MotorStruct *Z) { PositionErr = PositionSetPoint - DesiredPosition if (PositionErr > 0) { if (IntegralAcceleration < PositionErr) { CurrentVelocity += AccelerationSetPoint if (CurrentVelocity > VelocitySetPoint) CurrentVelocity = VelocitySetPoint else IntegralAcceleration += CurrentVelocity } else { IntegralAcceleration -= CurrentVelocity if (CurrentVelocity -= AccelerationSetPoint < 0) CurrentVelocity = 0 } } elseif (PositionError < 0) { if (IntegralAcceleration > PositionErr) { CurrentVelocity -= AccelerationSetPoint if (CurrentVelocity < VelocitySetPoint) CurrentVelocity = VelocitySetPoint else IntegralAcceleration -= CurrentVelocity } else { IntegralAcceleration += CurrentVelocity if (CurrentVelocity += AccelerationSetPoint > 0) CurrentVelocity = 0 } } DesiredPosition += CurrentVelocity } StopMotion(MotorStruct *Z, char bHard) { if (bHard) { PositionSetPoint = CurrentPosition IntegralAcceleration = 0 CurrentVelocity = 0 } else PositionSetPoint = CurrentPosition - IntegralAcceleration } } */ ;+ ;----------------------------------------- ; StopMotion ; ; PASSED: Z = Motor Control Struct, T = bHard ; Returned: ; USES: X & Y ; NOTES: This routine should use _Prolog/_Epilog to prevent ; race conditions with motor control ;- StopMotion: ldd Xl, Z+CurrentPosition+3 ldd Xh, Z+CurrentPosition+2 ldd Yl, Z+CurrentPosition+1 ldd Yh, Z+CurrentPosition+0 brtc Else00 clr Xl std Z+IntegralAcceleration+1, Xl std Z+IntegralAcceleration+0, Xl std Z+CurrentVelocity+1, Xl std Z+CurrentVelocity+0, Xl rjmp EndIf00 Else00: ldd R20, Z+IntegralAcceleration+1 ldd R21, Z+IntegralAcceleration+0 clr R22 sbrc R21, 7 ; Sign Extend IntegralAcceleration com R22 sub Xl, R20 sbc Xh, R21 sbc Yl, R22 sbc Yh, R22 EndIf00: std Z+PositionSetPoint+3, Xl std Z+PositionSetPoint+2, Xh std Z+PositionSetPoint+1, Yl std Z+PositionSetPoint+0, Yh ret ;+ ;----------------------------------------- ; DoMotion ; ; Module to take the encoder from one position to another with smooth ; acceleration and deceleration. ; ; PASSED: Z = Motor Control Struct ; RETURN: ; USES: R0, R18-29, Flags ; CALLS: ; NOTES: To be called with each control loop cycle. ; This routine should signal when a motion is completed ; Yh:X = IntegralAcceleration ; R20:R18 = AccelerationSetPoint ; R23:R22 = CurrentVelocity ; R25:R24 = VelocitySetPoint ;- DoMotion: ldd Xl, Z+IntegralAcceleration+2 ; 16.8 signed value ldd Xh, Z+IntegralAcceleration+1 ldd Yl, Z+IntegralAcceleration+0 clr Yh ; Sign Extend Integral Acceleration sbrc Yl, 7 com Yh ; ; PositionError = PositionSetPoint-DesiredPosition ; clr R0 ldd R22, Z+PositionSetPoint+3 ldd R23, Z+PositionSetPoint+2 ldd R24, Z+PositionSetPoint+1 ldd R25, Z+PositionSetPoint+0 ; 32 bit signed integer ldd R20, Z+DesiredPosition+4 ; 32.8 signed value sub R0, R20 ldd R20, Z+DesiredPosition+3 sbc R22, R20 ldd R20, Z+DesiredPosition+2 sbc R23, R20 ldd R20, Z+DesiredPosition+1 sbc R24, R20 ldd R20, Z+DesiredPosition+0 sbc R25, R20 ; R25:R22.R0 = PositionError bst R25, 7 ; Sign of results to T register cp Xl, R0 ; Compare IntergralAccel and PositionError cpc Xh, R22 cpc Yl, R23 cpc Yh, R24 cpc Yh, R25 ; Results in Flags. ; ldd R20, Z+VelocitySetPoint+1 ; Preload all other variables ldd R21, Z+VelocitySetPoint+0 ldd R22, Z+AccelSetPoint+1 ldd R23, Z+AccelSetPoint+0 ldd R24, Z+CurrentVelocity+1 ; 8.8 Signed Value ldd R25, Z+CurrentVelocity+0 #define VspH R21 #define VspL R20 #define AspH R23 #define AspL R22 #define cVH R25 #define cVL R24 ; ; if (PositionError > 0) If01: ; { brts Else01 ; T = sign of PositionError ; If02: ; if (IntegralAcceleration < PositionErr) ; { brge Else02 ; CurrentVelocity += AccelerationSetPoint add cVL, AspL adc cVH, AspH If03: ; if (CurrentVelocity > VelocitySetPoint cp VspL, cVL cpc VspH, cVH brge Else03 ; CurrentVelocity = VelocitySetPoint mov cVL, VspL mov cVH, VspH rjmp End03 Else03: ; else ; IntegralAcceleration += CurrentVelocity clr Yh ; Sign Extend Current Velocity into Yh sbrc cVH, 7 com Yh add Xl, cVL adc Xh, cVH adc Yl, Yh rjmp End02 ; } Else02: ; else ; { ; IntegralAcceleration -= CurrentVelocity clr Yh ; Sign Extend Current Velocity into Yh sbrc cVH, 7 com Yh sub Xl, cVL sbc Xh, cVH sbc Yl, Yh If04: ; if (CurrentVelocity -= AccelerationSetPoint < 0) sub cVL, AspL sbc cVH, AspH brge End04 ; CurrentVelocity = 0 clr cVL clr cVH ; rjmp End04 End04: End03: End02: ; } rjmp End01 Else01: ; else // PositionError < 0 ; { If05: ; if (IntegralAcceleration > PositionErr) ; { breq Else05 brlt Else05 ; brle ; CurrentVelocity -= AccelerationSetPoint sub cVL, AspL sbc cVH, AspH If06: ; if (CurrentVelocity < VelocitySetPoint cp cVL, VspL cpc cVH, VspH brlt Else06 ; CurrentVelocity = VelocitySetPoint mov cVL, VspL mov cVH, VspH rjmp End06 Else06: ; else ; IntegralAcceleration += CurrentVelocity clr Yh ; Sign Extend Current Velocity into Yh sbrc cVH, 7 com Yh add Xl, cVL adc Xh, cVH adc Yl, Yh rjmp End05 ; } Else05: ; else ; { ; IntegralAcceleration -= CurrentVelocity clr Yh ; Sign Extend Current Velocity into Yh sbrc cVH, 7 com Yh sub Xl, cVL sbc Xh, cVH sbc Yl, Yh ; if (CurrentVelocity += AccelerationSetPoint > 0) add cVL, AspL adc cVH, AspH brlt End07 ; CurrentVelocity = 0 clr cVL clr cVH ; rjmp End07 End07: End06: End05: ; } rjmp End01 End01: ; } ; DesiredPosition += CurrentVelocity ; clr Yh sbrc cVH, 7 ; Sign extend CurrentVelocity com Yh ldd R0, Z+DesiredPosition+4 add R0, cVL std Z+DesiredPosition+4, R0 ldd R0, Z+DesiredPosition+3 adc R0, cVH std Z+DesiredPosition+3, R0 ldd R0, Z+DesiredPosition+2 adc R0, Yh std Z+DesiredPosition+2, R0 ldd R0, Z+DesiredPosition+1 adc R0, Yh std Z+DesiredPosition+1, R0 ldd R0, Z+DesiredPosition+0 adc R0, Yh std Z+DesiredPosition+0, R0 std Z+CurrentVelocity+1, cVL std Z+CurrentVelocity+0, cVH std Z+IntegralAcceleration+2, Xl std Z+IntegralAcceleration+1, Xh std Z+IntegralAcceleration+0, Yl ret END