1 / 62

Chapter 16 Feedback control of motors, b ased on PIDRobotDemo.c

Chapter 16 Feedback control of motors, b ased on PIDRobotDemo.c. CEG2400 - Microcomputer Systems. Reference : www.tic.ac.uk/ micromouse /PRESENTATIONS/Heretic.ppt http://www.cse.cuhk.edu.hk/khwong/ceg2400/PIDRobotDemo093.c. Objectives. Study DC motors

carsyn
Download Presentation

Chapter 16 Feedback control of motors, b ased on PIDRobotDemo.c

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Chapter 16Feedback control of motors, based on PIDRobotDemo.c CEG2400 - Microcomputer Systems Reference : www.tic.ac.uk/micromouse/PRESENTATIONS/Heretic.ppt http://www.cse.cuhk.edu.hk/khwong/ceg2400/PIDRobotDemo093.c CEG2400 Ch16: feedback control V3b

  2. Objectives • Study DC motors • Study open-loop and closed-loop control • Control methods • I) Proportional feedback control • II) PID (proportional-integral-derivative) control CEG2400 Ch16: feedback control V3b

  3. 1) DC motors For robots CEG2400 Ch16: feedback control V3b

  4. Motors • DC motors: Direct current motor, easy to control and use. For making wheeled robots • Servo motors for making robot legs http://www.lynxmotion.com/ CEG2400 Ch16: feedback control V3b

  5. Small Direct Current D.C. motors • Speed (~ 1200-2000 rpm). • Operates on a 3~5Volt, Can use gear box (e.g. ratio 58:1) to increase torque • Use H-bridge circuit to boost up current from the TLL level to motor driving level. 田宫四驱车配件15351 PRO版专用双头马达 , picture from http://item.taobao.com/item.htm?id=1606576457&tracelog=newcardfavirate CEG2400 Ch16: feedback control V3b

  6. 2 (1A) 1Y(3) 1(EN1/2) 7(2A) (2Y)6 10(3A) (3Y)11 9(EN3/4) 15(4A) (4Y)14 LEN LDIR REN RDIR Left-motor Right-motor Motor control chip • H-bridge Chips • L293D: H-bridge circuit, up 2A • LDIR: left motor direction; RDIR: right motor direction • LEN : left motor enable; REN : right motor enable CEG2400 Ch16: feedback control V3b

  7. 2) open-loop and closed-loop control Feedback control PID theory and implementation CEG2400 Ch16: feedback control V3b

  8. Open-loop motor control and its problems • Change motor supply power change speed • Problem: How much power is right? • Ans: don’t know , depends on internal/external frictions of individual motors. • Problem: How to make the robot move straight? • How to control power (Ton) by ISR & an MCU? • Solution: Use feedback control to read actual wheel: • Slower, increase power (+ Ton) • Faster, reduce power (- Ton) CEG2400 Ch16: feedback control V3b

  9. Exercise • When using the open-loop control method with a constant PWM signal for both wheels, explain why the robot would slow down when climbing up hill. • Ans: When climbing up hill, energy is used to act against gravity, hence the robot is running slower. CEG2400 Ch16: feedback control V3b

  10. Feedback control • The real solution to real speed control is feedback control • Require speed encoder to read back the real speed of the wheel at real time. CEG2400 Ch16: feedback control V3b

  11. First you need to have speed encoders • Read wheel speed. • Use photo interrupter • Use reflective disk to save space • Based on interrupts CEG2400 Ch16: feedback control V3b

  12. Wheel encoder Our motor and speed encoder Each wheel rotation= 88 on/off changes IR receiver Darkened part blocks light IR light source CEG2400 Ch16: feedback control V3b

  13. CEG2400 Ch16: feedback control V3b

  14. 田宫四驱车配件15351 PRO版专用双头马达 , picture from http://item.taobao.com/item.htm?id=1606576457&tracelog=newcardfavirate New speed encoder Demo movie CEG2400 Ch16: feedback control V3b

  15. Student ID: ___________,Date:_____________Name: _______________CENG2400 , Ch 16 PID Exercise 1: (Fill in ?__) 66mm • The Gear ratio =48, and the DC motor is rotating at 200 Rotation/Second • So the wheel is rotating at • 200/48  ?___ rotations per second • If the disk has 1 hole, the pulse frequency is ?__Hz. • If the disk has 4 holes, the pulse frequency is 200x4=?_____Hz. • Wheel radius is r=0.033m, perimeter is 2*pi*0.033 m • The car is moving at 4 rotations per second=?__________ meters per second (at full speed, quite fast) • The speed encoder gives out ?_______Hz pulses. This is when the disk has 1 hole, The waveform is 200 Hz Data comes from http://item.taobao.com/item.htm?spm=2013.1.0.116.KbMxbr&id=15172908917 CEG2400 Ch16: feedback control V3b

  16. 3) Control methods I) Proportional feedback control II) PID (proportional-integral-derivative) control CEG2400 Ch16: feedback control V3b

  17. I) Proportional closed-loop feed back control system • Show the left motor control only if (leftErr >deadband) leftPWM increase by (Pgain * leftErr) leftPWM Required speed =leftRPMset leftErr Motor Alter PWM for driver L293 + - leftRPM CEG2400 Ch16: feedback control V3b

  18. II) PID (proportional-integral-derivative) control A more formal and precise method Used in most modern machines CEG2400 Ch16: feedback control V3b

  19. control method:PID (proportional-integral-derivative) control Motor integral control Igain* leftErr dt Required speed= leftRPMset IR wheel Speed encoder leftPWM generates PWM for driver L293 Proportional control Pgain*leftErr + - leftErr sum Derivative control Dgain*[d(leftErr)/dt] leftPWM LeftRPM leftRPM CEG2400 Ch16: feedback control V3b

  20. Introduction • Control for better performance • Use PID, choose whatever response you want Too much overshoot/undershoot, not stable Motor speed (w) Good performance Criteria depends on users and applications required Response too slow time CEG2400 Ch16: feedback control V3b

  21. Values to evaluate a control systemExercise 2: Describe the terms n the following diagrams Steady state error overshoot Target value Typically value=10% Depends on application undershoot time 0 Settling time Rise time CEG2400 Ch16: feedback control V3b

  22. Use of PIDcontrol terms are intertwinedhttp://en.wikipedia.org/wiki/PID_controller • Kp (Pgain): Proportional Gain - Larger Kp typically means faster response since the larger the error, the larger the Proportional term compensation. An excessively large proportional gain will lead to process instability and oscillation. • Ki (Igain): Integral Gain - Larger Ki implies steady state errors are eliminated quicker. The trade-off is larger overshoot: any negative error integrated during transient response must be integrated away by positive error before we reach steady state. • Kd (Dgain): Derivative Gain - Larger Kd decreases overshoot, but slows down transient response and may lead to instability due to signal noise amplification in the differentiation of the error. CEG2400 Ch16: feedback control V3b

  23. Effects of increasing parametershttp://en.wikipedia.org/wiki/PID_controller CEG2400 Ch16: feedback control V3b

  24. Software PIDrobotdemo.c (at course webpage) http://www.cse.cuhk.edu.hk/khwong/ceg2400/PIDRobotDemo.c CEG2400 Ch16: feedback control V3b

  25. IR receiver Speed Encoder sensor interrupts 1000 interrupts per second time PID control algorithm using interrupt Main( ) { Setup( ); : : } _IRQ( )//1000Hz { : read wheel speed PID : } CEG2400 Ch16: feedback control V3b

  26. Overview • //////////////main ////////////////////////////////////// • Main() • { setup() • forward (Lstep, Rstep, Lspeed, Rspeed)….. • } • ////////////subroutine ////////////////////////////////////////// • forward (Lstep, Rstep, Lspeed, Rspeed) • { lcount=0 • if (lcount>=Lstep) • Stop left motor • …same for right motor… • .} • //////////timer triggered , interrupt service routine, 1000Hz/////// • __irq exception// Interrupt() running at 1000HZ • { lcount++ • read wheel speeds • PID control to achieve L/Rspeed • ….same for right motor…. • } Interrupt rate 1000Hz _IRQ see next slide CEG2400 Ch16: feedback control V3b

  27. Speed encoder interfacing(show left motor only) • Block diagram ARM7-microcontroller LPC2131 1000Hz timer interrupt /int0 CPU GPIO Parallel interface IR transmitter IR receiver (speed encoder sensor) In our robot 88 pattern changes in one rotation CEG2400 Ch16: feedback control V3b

  28. Exercise 3_irq interrupt programming method for the main PID loop, using a counter (intcount) • __irq() exception //timer interrupt 1000HZ, • { intcount++; //intcount • //increases at 1000 times/sec • //read motor speed: • //update the motor speed in every 1/4 seconds • if(intcount==250) // happens at every ¼ seconds • { • read wheel speed • Control the PWM using PID • intcount=0; • } • } • Question: If “if(intcount==250) “ is changed to “if(intcount==500) “ • What happens? PID CORE CEG2400 Ch16: feedback control V3b

  29. The interrupt service routine enables the loop to run 4 times in a second Loop once in ¼ seconds CEG2400 Ch16: feedback control V3b

  30. Speed control interrupt core _irq()part 1: Find motor speed, leftRPM Speed encoder sensor lefttimeval • void __irq IRQ_Exception() • {ms++; intcount++; • //get the current wheel sensor values • lcur=IO0PIN & LWheelSen;rcur=IO0PIN & RWheelSen; • if(lcur!=lold) { lcount++;lold=lcur;lefttimeval++;} • : • //update motor speed by PID feed back control in every ¼ Seconds • if(intcount==250) { //calculate the speed of left motor in RPM • leftRPM=lefttimeval; • : • intcount=0; • lefttimeval=0; • } • } Part1 Read Wheel speeds time Interrupts 1000 Hz PID core : See following slides Part 2 PID control • lcount :steps of wheel • lcur : sensor read 1 or 0 • lefttimeval: time lapsed since sensor last change of state • leftRPM : left wheel RPM CEG2400 Ch16: feedback control V3b

  31. Part 2 :Algorithm for PID core • For every ¼ seconds • Find error=(desired value - measured value) • {If (error>dead band ) • { find • Error, • Accumulated error (add up all previous errors) • Derivative error (current error – previous error) • PWM+=Kp* Error+ • Ka*(Accumulated error)+ • Kd* Derivative error ; • } • Else • Error <= dead_band, error too small do nothing Set_point In our experiment leftPWM=276000 at the beginning and 192800 at steady state CEG2400 Ch16: feedback control V3b

  32. part 2: PID core PID core P=Proportional I Integral • if(intcount==250) {//for every ¼ seconds • //caculate the speed of left motor in RPM • leftRPM=lefttimeval; • leftErr = leftRPMset - leftRPM; //caculate left error • if((leftErr<DeadBand*(-1))||(leftErr>DeadBand)) //see next slide • { //if |left error| > deadband • leftP = Pgain * leftErr; //calculate P Proportional term • leftI = Igain * leftaccErr; //calculate I Integral term • leftD = Dgain * (leftErr - leftlastErr); //calculate D Derivative term • leftPWM += (leftP + leftI + leftD);//update left motor PWM using PID • if(leftPWM>PWM_FREQ) • leftPWM=PWM_FREQ;//prevent over range (max.=PWM_FREQ) • if(leftPWM<0) leftPWM = 0; // (min. = 0) • leftaccErr += leftErr; // accumulate error • leftlastErr = leftErr; //update left last error • : • : // handle right motor similarly……. • current_leftRPM = leftRPM*240/88; • current_leftPWM = leftPWM;// • lefttimeval = 0; • } D Derivative Pgain, Igain, Dgain are constants found by a trial and error method, here we have Pgain = 8000; Igain = 6000; Dgain = 5000; because each rotation has 88 counts, the ISR loop is in ¼ seconds, each minutes 60 seconds. see line 18 CEG2400 Ch16: feedback control V3b

  33. control method:PID (proportional-integral-derivative) control Motor integral control Igain* leftErr dt Required speed= leftRPMset IR wheel Speed encoder leftPWM generates PWM for driver L293 Proportional control Pgain*leftErr + - leftErr sum Derivative control Dgain*[d(leftErr)/dt] leftPWM LeftRPM leftRPM CEG2400 Ch16: feedback control V3b

  34. Inside the PID core, we will study these lines • 5) if((leftErr<DeadBand*(-1))||(leftErr>DeadBand)) • : • : • 7) leftP = Pgain * leftErr; //calculate P Proportional term • 8) leftI = Igain * leftaccErr; //calculate I Integral term • 9) leftD = Dgain * (leftErr - leftlastErr);//calculate D Derivative term • 10) leftPWM += (leftP + leftI + leftD);//update motorPWM by PID • : • : • 14) leftaccErr += leftErr; // accumulate error CEG2400 Ch16: feedback control V3b

  35. Dead bandline5) if((leftErr<DeadBand*(-1))||(leftErr>DeadBand)) • Dead-band : A Dead-band (sometimes called a neutral zone) is an area of a signalrange or band where no action occurs : • only enable motor when leftErr> a small value (deadband, ie =1 in our robot ) • Otherwise may oscillate when leftErr is small leftErr = leftRPM - leftRPMset; //calculate left error if(leftErr>DeadBand ) { activate motor} Dead-band CEG2400 Ch16: feedback control V3b

  36. Exercise 4: Discuss what will happen if dead-band is changed, say (a) 0.5 or (b) 2.Example of a dead band ;do nothing “if 10-1<leftPRM <10+1 In our experiment leftPWM=276000 at the beginning and 192800 at steady state Steady state error Dead band +/- 1 overshoot Target speed= leftRPMset =10 Typically value=10% Depends on application undershoot time Settling time 0 Rise time When leftRMPset=10, the real RPM is RPM*240/88=27.3., because each rotation has 88 counts, the ISR loop is in ¼ seconds, each minutes 60 seconds. see line 18 CEG2400 Ch16: feedback control V3b

  37. Parameters for evaluating a control system near steady state See next slide Steady state error overshoot Target value Typically value=10% Depends on application undershoot time 0 Settling time Rise time CEG2400 Ch16: feedback control V3b

  38. (line 7) Effects of increasing Kp(P)http://en.wikipedia.org/wiki/PID_controller CEG2400 Ch16: feedback control V3b

  39. Example: t0t1, The proportional term when the measurement is below the set point (leftRPMset), proportional P term is +ve Usage of the P proportional term Each ¼ seconds a new left RPM (speed of wheel) is measured (leftErr = leftRPMset-leftRPM ) overshoot Target speed= leftRPMset =10 8 e.g. Pgain = 8000; Igain = 6000; Dgain = 5000; 6 undershoot leftErr= leftRPMset– leftPRM =30-28=2 leftErr= leftRPMset– leftPRM =10-6=4 P is +ve P is +ve In our experiment leftPWM=276000 at the beginning and 192800 at steady state t1 t2 t3 t4 0 time Rise time leftP = Pgain * leftErr; leftP= 8000* (10-6)= 8000*4 is positive This term pushing up “leftPWM” (more energy delivered to the wheel). leftP = Pgain * leftErr; leftP= 8000* (0-8)= 8000*2 is positive This term pushing up “leftPWM” (more energy delivered to the wheel). CEG2400 Ch16: feedback control V3b

  40. Exercise 5: Fill in ?__: t0t1, The proportional term when the measurement is below the set point (leftRPMset), proportional P term is +ve Usage of the P proportional term Each ¼ seconds a new left RPM (speed of wheel) is measured overshoot Target speed= leftRPMset =10 8 e.g. Pgain = 8000; Igain = 6000; Dgain = 5000; 6 undershoot leftErr= leftRPMset– leftPRM =10-8=2 leftErr= leftRPMset– leftPRM =10-6=4 P is +ve P is +ve In our experiment leftPWM=276000 at the beginning and 192800 at steady state t1 t2 t3 t4 0 time Rise time leftP = Pgain * leftErr; leftP=?___________ ,is +ve or -ve? “leftPWM” is increasedor decreased?__ more/less energy delivered to wheel?__ leftP = Pgain * leftErr; leftP= 8000* (10-6)= 8000*4 is positive This term pushing up “leftPWM” (more energy delivered to the wheel). CEG2400 Ch16: feedback control V3b

  41. Example: t1t3,The proportional termwhen the measurement is below the set point (leftRPMset), the proportional P term is +ve P is -ve Usage of the P proportional term Each ¼ seconds a new left RPM (speed of wheel) is measured overshoot 13 Target speed= leftRPMset =10 leftErr= leftRPMset– leftPRM =10-13= -3 e.g. Pgain = 8000; Igain = 6000; Dgain = 5000; undershoot P is +ve In our experiment leftPWM=276000 at the beginning and 192800 at steady state t1 t2 t3 t4 0 time Rise time leftP = Pgain * leftErr; leftP= 8000* (10-13)= 8000*(-3) is negative This term lowering down “leftPWM” (less energy delivered to the wheel). CEG2400 Ch16: feedback control V3b

  42. Understanding PID –a little summary for the P proportional term • When the measurement is below the set point (leftRPMset) • The motor is slow • The P proportional term is +ve, pushing the speed higher • When the measurement is above the set point (leftRPMset) • The motor is running too fast • The P proportional term is -ve, lowering down the speed. • Increase in Pgain (Kp) will decrease rise time (meaning faster to reach set point) , decrease steady state error (study it later) • But increase overshoot CEG2400 Ch16: feedback control V3b

  43. dx/dt (line 9 ) Effects of increasing Kd (D)http://en.wikipedia.org/wiki/PID_controller dx/dt CEG2400 Ch16: feedback control V3b

  44. Derivative term • Derivative control • Dgain*[d(leftErr)/dt] • d(leftErr)/dt = • =Derivative term • =current_Err - last_Err • =leftErr – leftlastErr ; in our program CEG2400 Ch16: feedback control V3b

  45. Example: time 0 t1, the Derivative term (=current-previous) When the measurement is rising, the Derivative term is -ve Target Value= leftRPMset =10 Usage of the D Derivative term Each ¼ seconds a new left RPM (speed of wheel) is measured ¼ seconds overshoot leftRPMset =10 leftErr= leftRPMset– leftPRM =10-7=3 e.g. Pgain = 8000; Igain = 6000; Dgain = 5000; 7 undershoot 3 leftlastErr= leftRPMset– eftlastPRM =10-3=7 D is -ve In our experiment leftPWM=276000 at the beginning and 192800 at steady state t1 t2 t3 t4 0 time Rise time leftD = Dgain * (leftErr – leftlastErr); leftD= 5000* (3-7)= 5000*( -4) is negative This term lowering down “leftPWM” (less energy delivered to the wheel). CEG2400 Ch16: feedback control V3b

  46. Example: time t1t2, the Derivative termWhen the measurement is rising, the Derivative term is -ve leftErr= leftRPMset-leftPRM =10-15= -5 Usage of the D Derivative term Each ¼ seconds a new left RPM (speed of wheel) is measured ¼ seconds overshoot leftPRM 15 12 leftRPMset =10 leftlastErr= leftRPMset-leftlastPRM =10-12= -2 e.g. Pgain = 8000; Igain = 6000; Dgain = 5000; undershoot D is -ve D is -ve In our experiment leftPWM=276000 at the beginning and 192800 at steady state t1 t2 t3 t4 time 0 Rise time leftD = Dgain * (leftErr – leftlastErr); leftD= 5000* (-5- (-2))= 5000*(-3) is negative This term lowering down “leftPWM” (less energy delivered to the wheel). CEG2400 Ch16: feedback control V3b

  47. Little Summary: Negative D Derivative term • When the measurement (leftRPM) is rising, the change (change= current-previous) of error (error = setpoint-leftRPM ) is -ve = d(Err/dt)=-ve • D Derivative term= Kd*d(Err/dt) is –VE • Decrease energy to motor  decrease overshoot overshoot Setpoint 目標 undershoot D is -ve D is -ve t1 t2 t3 t4 time 0 Rise time CEG2400 Ch16: feedback control V3b

  48. Example: time t2t3, the Derivative termWhen the measurement is falling, the Derivative term is +ve Usage of the D Derivative term Each ¼ seconds a new left RPM (speed of wheel) is measured ¼ seconds overshoot leftPRM 14 11 leftRPMset =10 leftlastErr= leftRPMset-leftlastPRM =10-14= -4 leftErr= leftRPMset-leftPRM =10-11= -1 undershoot D is -ve D is +ve e.g. Pgain = 8000; Igain = 6000; Dgain = 5000; D is -ve t1 t2 t3 t4 time 0 In our experiment leftPWM=276000 at the beginning and 192800 at steady state Rise time leftD = Dgain * (leftErr – leftlastErr); leftD= 5000* (-1- (-4))= 5000*3 is positive This term pushing up “leftPWM” (more energy delivered to the wheel). CEG2400 Ch16: feedback control V3b

  49. Exercise 6:Fill in ?_ time t2t3, the Derivative termWhen the measurement is falling, the Derivative term is +ve Usage of the D Derivative term Each ¼ seconds a new left RPM (speed of wheel) is measured ¼ seconds overshoot leftPRM leftRPMset =10 9 7 undershoot D is +ve leftErr= leftRPMset-leftPRM =10-7= 3 D is +ve leftlastErr= leftRPMset-leftlastPRM =10-9= 1 D is -ve e.g. Pgain = 8000; Igain = 6000; Dgain = 5000; D is -ve t1 t2 t3 t4 time 0 Rise time In our experiment leftPWM=276000 at the beginning and 192800 at steady state leftD = Dgain * (leftErr – leftlastErr); leftD= ?___________, which is +ve or –ve?____ “leftPWM” increased or decreased?____ More/less energy delivered to the wheel?___ CEG2400 Ch16: feedback control V3b

  50. Little Summary: Positive D Derivative term • When the measurement (leftRPM) is falling, the change (change= current-previous) of error (error = setpoint-leftRPM ) is +ve • D Derivative term= Kd*d(Err/dt) is +VE • Increase energy to motor  decrease undershoot overshoot Setpoint 目標 undershoot D is +ve D is +ve t1 t2 t3 t4 time 0 Rise time CEG2400 Ch16: feedback control V3b

More Related