' MOGOTUT.BAS - Simple Single Channel DC Motor Example ' ' This program is not for commercial use and is copyrighted by Dan Creagan ' It is not a complete example of a motor controller. ' ' It is presented here for learning purposes only. ' ' Contact me at: dcreagan@scholars.bellevue.edu if you want to discuss it. ' ' ' *********************************************************************** ' ' * * ' ' * Delarations and Port Setup: * ' ' * locked anti-phase version * ' ' * This program expects a DC motor hooked up in locked anti phase * ' ' * configuration using an H bridge. There must be encoder feedback * ' ' * to B4. PWM out is from the hardware PWM and is on C2. This * ' ' * only shows proportional gain - integral and derivative gain * ' ' * are not addressed here (see main html for information on * ' ' * these). * ' ' * * ' ' * This is 8 bit anti-phase. The duty cycle can vary between * ' ' * 0 and 255. That means a duty cycle of 128 * ' ' * turns the motors on for rotation in each direction exactly * ' ' * half the time. To get a motor to rotate, you move the duty * ' ' * cycle of the pulse train to either side of 128. To get a * ' ' * motor to stop, set the duty cycle to 128. It will then be * ' ' * locked by opposing forces to turn. This technique discourages * ' ' * the motor from free spinning after a stop is requested (which * ' ' * is what you want for a robot.) * ' ' * * ' ' *********************************************************************** ' DEFINE OSC 20 ' use a pretty hot oscillator freq - not needed for demo, but ' useful later when you add in another channel and serial comms include "modedefs.bas" Lcounter var portB.4 DC2 var CCPR2L TLoop var word ' loop period (loosely 1 ms increments at 20 mHz) ErrorC var word ' error between requested and actual PGain var word ' Proportional gain x var word y var word ' x, y and z are scratch vars z var word ' ' variable to hold the current sensor counts ' LCurSpeed var word ' requested speed variable LeftReq var word ' direction variable dirLft var word ' setup motor DC2=128 ' set duty cycle to initial values here. 128 is 1/2 way ' and will result in equal duty cycle (motor is stopped) ' Set up PWM PR2 = $FF 'will give us 19.2 kHz with PS=1 @20MHz T2CON = %00000100 ' (currently div by 1=19.2 kHz) (div by 4 = 4.8 kHz) turn TMR2 on, pre-scaler set to 4 CCP2CON = %1100 'turns PWM mode on, clears lowest two bits of duty cycle ' setup B port for tracking TRISB = %11111101 ' make ports 1 of B output, rest input TRISC = %11111101 ' C2 out (PWM) 'Requested is set to a nominal value so we can check 'ramp up times, etc LeftReq=20 ' Requested speed - must be in range of motors! portb.1=1 ' turn on the enable bit for the L293D/SN754410 dirLft=1 ' 1=forward (128-255), 0=backwards (127-0) ' ' ' setup counters x=Lcounter ' get value of left counter LCurSpeed=0 TLoop=200 'Loop time in 1 ms. Keep it LOW for fast motors 'HIGH for slow motors (125 is for fast motors, 1000 slow??) ' *********************************************** ' ' * Suggested value for gains (to start) * ' ' * 10 * ' ' *********************************************** ' PGain=10 ' Proportional gain * 10. For gentle ramp ups, keep it low ' I would suggest 1 - 20 (that's a gain of .1 to 2) ' Right now the actual gain is 1 ' *********************************************** ' ' * * ' ' * Main Loop * ' ' * * ' ' *********************************************** ' loop: 'do z samples with a 1 ms delay 'between each test. 'Changing the sample size (TLoop) affects gain for z=1 to TLoop pause 1 ' need this puppy, it's way too fast without it if Lcounter = x then skip3 ' no change in encoder status, so skip counter LCurSpeed=LCurSpeed+1 ' encoder output changed, so tally it x=Lcounter ' set x to current encoder output skip3: next gosub adjLeft ' set left speed (request) LCurSpeed=0 ' reset current speed counter x=Lcounter ' set x to see if encoder changed goto loop ' go back for more ' ******************************************************* ' ' * * ' ' * Single subroutine * ' ' * that adjusts the power of the motor to match * ' ' * the feedback from the shaft encoders * ' ' * * ' ' ******************************************************* ' adjLeft: ' ' ' we're fighting a division problem with negative numbers ' here. Got to be careful not to divide 65534 since it would ' then be a very big positive number. Also have to be ' careful not to bring the PWM below 0 since that will ' roll over to a high byte number. This is all driven by ' PBASICs non-signed integer math ' ' Proportional control area. Proportional control is derived from ' the difference between requested speed of the drive and actual ' speed. This is a very effective method and the use of proportional ' control alone is probably sufficient for what we want to do. ' One disadvantage is that, under load, the system will settle into ' a slight offset from requested speed (especially if we are ' using fractional gains). A later project should be to add in ' Integral Feedback to stop the offset issue. ErrorC=LeftReq-LCurSpeed ' this is the fundamental feedback calc (easy, eh?) ' set the gain for Proportional feedback. ErrorC=ErrorC*PGain if (abs(ErrorC) = ErrorC) then ErrorC=ErrorC/10 ' take off the multiplier now else ErrorC=0-(abs(ErrorC)/10) endif ' limiter if (dirLft) then ' growing from 128 up if (abs(DC2+ErrorC) <> (DC2+ErrorC)) then ' we had a roll over DC2=255 return endif if(DC2+ErrorC) > 255 then DC2=255 return endif if (DC2+ErrorC) < 128 then ' went below artificial zero (128) DC2=128 return endif DC2=DC2+ErrorC endif if (dirLft=0) then ' growing from 128 down if (abs(DC2-ErrorC) <> (DC2-ErrorC)) then ' we had a roll under DC2=0 return endif if (DC2-ErrorC) > 128 then ' went above artificial zero (128) DC2=128 return endif DC2=DC2-ErrorC ' if to here, then put in the error ' correction and go back to the ' main program. endif return End