PID Motor control
CRST1
Posts: 103
I have searched and can't seem to find anything on motor control that looks like I need. I'm looking for a PID routine that just controls the speed of a PWM controller for a DC motor. The ones I have found seem to be for robots and are based on position control. The controller I have uses 50% width for 0 speed and 100% duty for full speed forward and 0% duty for full speed reverse. The pulse width object I'm using takes 0 to 2400 for the control of the pulse width to the controller. 0 for reverse 1200 for stop and 2400 for foward. I'm looking for something to get me started on a PID formula. I don't need any position control just speed regulation. I have a simple proportional one written that works roughly but I need better control.
Can anyone give ne something that I can get started with and try to understand or a link to a simple example. I have studied the algorithms and am not getting the idea.
Thanks
Can anyone give ne something that I can get started with and try to understand or a link to a simple example. I have studied the algorithms and am not getting the idea.
Thanks
Comments
I have the proportional part working but I am not grasping the ID part of the equation.
Basically you want to create a PID engine that outputs a value to the motor controller. Your engine constantly watches the encoder and does it's speed calculation to determine speed. I cannot say what will happen when the object counting the pulses rolls over 4.4 billion, since I have never used the quadrature encoder object for a motor that may spin past that amount of pulses.
You proportion is an abrupt calculation, and in many cases never is able to settle. The integral helps smooth out the correction since it is based on TIME, or the accumulation of error over time. With integral, you have a factor called integral windup, which means that your output may need to be limited with an integral maximum value. There is no real need for derivative in this case.
Can you post your code so far?
You tell the output to go to some speed. You then subtract what the actual speed is from what you are telling the motor to be. If the motor is exceeding the value you want, the PID loop reduces the output. If the motor is running slower than your desired speed, the PID loop increases the output to correct the error. The roll of the PID loop is to make the error become as close to zero as possible. In the real world, the goal is to make the error get close to zero but at the same time accomplish the error reduction within an amount of time (a desired speed of correction) in a smooth manner. This is where it gets tricky. The integral value accumulates by some value over time. You can change how fast it accumulates, and you can set how far it can accumulate. A slower rate of accumulation makes for a more graceful attempt at correction. A far rate of accumulation makes for a faster (potentially abrupt) correction. The balancing act is to avoid oscillation and overshooting the error, while getting the error corrected in a timely manner.
Ok, here's what I have so far. It's just in the starting rough stage.
Like I said, it does work so far but its rough because its only proportional.
CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 LCD_PIN = 27 LCD_BAUD = 19_200 LCD_LINES = 4 LCD_COLS = 20 ENCODER_PIN = 0 OBJ L_quad : "QuadDecoder" db : "FullDuplexSerial" VAR long L_set_Speed, R_set_Speed 'speeds to send to controller long L_cont_speed, L_Error long L_Speed, R_Speed 'actual speed of motor from encoder long L_offset, R_offset 'actual count from encoder long L_offset_old, L_offset_new 'used for calculating number of counts in 100uS long R_offset_old, R_offset_new 'used for calculating number of counts in 100uS long Time_start 'start of 100uS timer ' example variable that will be accumulated to PUB go | x dira[14..16]~~ outa[14]~ outa[15]~~ outa[16]~ L_offset := 0 ' initialize the accumulator You can set it to any desired value L_set_Speed := 1200 R_set_Speed := 1200 L_quad.start(ENCODER_PIN, @L_offset) ' start the encoder reader cognew(@entry, @L_set_Speed) 'start assembly cog db.start(31,30,0,115_200) repeat '' Compute speed from encoder pulses Time_start := cnt 'put current count into new time L_offset_old := L_offset 'put current encoder count into new left offset R_offset_old := R_offset 'put current encoder count into new right offset waitcnt(Time_start + 80000) 'wait for 1mS L_offset_new := L_offset 'put current encoder count into new left offset R_offset_new := R_offset 'put current encoder count into new right offset L_Speed := (L_offset_new - L_offset_old) * 8 'Left speed = tach counts/Sec R_Speed := (R_offset_new - R_offset_old) * 8 'Right speed = tach counts/Sec '' Get speed from controller L_cont_speed := 1600 '' Compute error from speed and set speed L_Error := L_cont_speed - (L_speed + 1200) '' Compute control for output to motor L_set_Speed := L_set_Speed + (L_Error/6) #> 1200 <# 2400 db.str(string("Value = ")) db.dec(L_Speed) db.str(string(" ")) db.dec(L_set_Speed) db.str(string(" ")) db.tx(1) waitcnt(clkfreq/10 + cnt) DAT ''assembly cog fetches the value in parameter for PWM percentage org 0 entry mov dira, diraval 'set A and B PIN to output mov ctra, ctraval 'establish counter A mode and APIN mov ctrb, ctrbval 'establish counter B mode and BPIN mov frqa, #1 'set counter a to increment 1 each cycle mov frqb, #1 'set counter b to increment 1 each cycle mov time, cnt 'record current time add time, period 'establish next period :loop mov index, par 'put the variable address into index rdlong valuea, index 'get an up to date pulse a width add index, #4 'add four to the index address rdlong valueb, index 'get an up to date pulse b width waitcnt time, period 'wait until next period neg phsa, valuea 'back up phsa so that it trips "value" cycles from now neg phsb, valueb 'back up phsb so that it trips "value" cycles from now jmp #:loop 'loop for next cycle diraval long %00000000_00000000_00110000_00000000'APIN=output BPIN=output ctraval long %00100 << 26 + 12 'NCO/PWM APIN=12 dirbval long |< 20 'BPIN=0 ctrbval long %00100 << 26 + 13 'NCO/PWM BPIN=13 period long 2400 ' time res 1 index res 1 valuea res 1 valueb res 1 fitIt has some entrys for left and right motors but it's only using the left motor now.
Kp, Ki, and Kd are your basic tuning parameters. You can set them by hand initially as an educated guess then refine them in a limited real environment before letting the machine lose in the wild.
DirectError, as you've already got, is a direct subtraction of measured speed from target speed.
IntegratedError is just an accumulation of the direct errors. Be wary of this one, it can run away on you.
And, surprise, DifferentialError is the subtraction of previous direct error from the latest direct error.
Not too hard but, that said, you aren't really bound to just this one exact description. You can add filtering - generally always a good idea, each term has it's own filter. Filters are particularly effective on the differential term. You can add extra terms. You can take terms away. You can split and combine terms depending on feedback and feedforward sources. You can put bounds on terms and have auto-reset features. You can add offsets and predictive profile generation to the target speed. You can add auto-tune features that hunt for stable tuning parameters.
Hope that brief run down helps.
VAR long intergralaccumulatorvalue , I , imax, proportional , deadband PUB Init intergralaccumulatorvalue := 5 'randon number used imax := 10000 'random proportional := 9 'random deadband := 20 Pub PIDengine Repeat If direction <> prevdirection 'reset all if new direction command received P~ I~ Output~ P := proportional * error 'basic proportional value I := I + intergralaccumulatorvalue 'accumulate while there is an error present If I > imax ' long version of I <#= imax I := imax repeat while target - actualspeed =< deadband 'set a dead band (example not for signed values) P~ 'reset if no error I~ 'reset if no error output~ 'kill output if in range of deadband, probably used only for stopped condition so more code is need to be sure output := p + i 'set the output prevdirection = direction 'add code to detect directionIt takes a lot of tweaking to find what works in your case. The code I pulled this from is much more complicated for a specific use.
http://forums.parallax.com/showthread.php/105978-PID-Enhanced-is-finally-done!?highlight=Craig Weber
Worthwhile reading:
http://www.pmdcorp.com/downloads/Mathematics_of_Motion_Control_Profiles.pdf
P.S. Never understood why he didn't stick with integers, though (PID object)