Mechatronics Laboratory Assignment 5 Motor Control and Straight-Line Robot Driving

Transcription

1 Mechatronic Laboratory Aignment 5 Motor Control and Straight-Line Robot Driving Recommended Due Date: By your lab time the week of March 5 th Poible Point: If checked off before your lab time the week of Mar. 2 th 4 point If checked off after your lab time the week of Mar. 2 th and before 4:00PM Mar. 6 th point If work not finihed before 4:00PM Mar. 6 th 0 point Goal for thi Lab Aignment:. Learn why feedback control i ued to complete a traight-line driving tak by comparing open-loop, decoupled loop, and coupled-loop control method. 2. Create a LABVIEW interface to communicate with the robot and diplay the current robot poition. DSP/BIOS Object Ued: PRD Library Function Ued: Global variable: enc_rad, enc2_rad, enc3_rad and enc4_rad, witchtate Function: void PWM_out(enum epwm ep, float u) Lecture Topic: PI Control and Integral Wind-up Prelab: A a prerequiite for thi cla, all tudent hould have completed a firt coure in control theory. We therefore expect that at ome time, the tudent ha already learned about baic feedback loop, pecifically PI control. Pleae review your note on thi ubject, a you are expected to implement a PI-type controller in lab. Read the lab and begin coding a PI peed controller for the right and left drive ide. Thi will help you finih the lab in a timely fahion. Laboratory Exercie: In thi lab, we will be attempting to control the motion of the robot and teting thi by driving the robot in a traight line. There are many different way to do thi; we will invetigate very tandard technique: Exercie : Open-Loop Robot Driving. Starting with a new project, implement open-loop control of the robot jut a you did in Lab 3. The value read from optical encoder 3 divided by 00 hould be ent to both motor a an open-loop torque command. We are going to compare the open-loop to the cloed-loop control deigned later in lab. Setup your program (in a witch-cae tatement) o that when all the dip-witche are FORWARD (toward the front of the robot) nothing i output to the motor. Alo diplay Robot Off to the LCD creen. When the firt dip-witch i back and all other forward, run the open-loop controller. Diplay omething like Open Loop to the LCD creen. Alway tet your code out firt on the bench before putting the robot on the ground. We ugget a certain organizational tructure to help you better debug your code. The following peudo-code will help guide you through the tep needed. Inide the void control(void) function jut after the call to updatedata() : GE423, Mechatronic Sytem Lab 5, Page of 5

2 . Remember that void control(void) i the function for the SWI, SWI_control. SWI_control i poted in the given code every m after all the optical encoder channel have been read. 2. Increment a long integer to keep track of elaped time; 3. Ue the drive-motor optical encoder (Encoder # & #2) to calculate the motor velocity in tile/econd; 4. Calculate the robot poition from the average of the two motor velocitie; 5. The global variable witchtate contain the tatu of the four white witche on the hield board jut to the left of the color LCD creen. Since there are 4 witche, the value of witchtate range from 0 to Ue a witch-cae code tructure to implement different control algorithm for different dip-witch etting; 7. Inide the open-loop cae tatement, et control effort u and u2 to Encoder #3 value divided by 00. In exercie 2, 3 and 4, encoder #3 value will be ued a the deired velocity et point for the control or the deired turn et point. 8. For thi open loop cae do not add friction compenation. Alo make ure NOT to apply friction compenation when the robot i in the OFF tate. The OFF tate hould end 0, with PWM_out, to both motor. 9. Output u and u2 to the motor with the PWM_out command; 0. Save pat variable a needed;. Print to LCD on line a text meage indicating which control algorithm i active and on line 2 the open-loop command (or reference velocity) and meaured motor velocitie with only decimal reolution. Remember not to print too fat to the LCD. Right now jut tet your code on the bench leaving the robot on the cake pan. Jut like in Lab 3, you hould be able to turn encoder #3 to command to robot wheel to pin forward or backward. After you have completed the 3 controller mode below, you will then place the robot on the floor and compare each of them. Exercie 2: Cloed-Loop Robot Driving with Decoupled Control Loop (See Figure ). Partner witch o that the other peron can do ome coding.. Add another cae tatement to check if the econd witch i back and all other forward. If o, your cae tatement will calculate u and u2 from two different PI control loop. You will be implementing the code equivalent to the diagram in Figure. For friction compenation ue 60% of the value you found in lab 3. A a tart, ue = 3.0 and = 5.0. Thee are good tarting gain auming that all velocitie are meaured in tile/econd. You will tune thee lightly in lab to achieve a more reponive PI control. Alo, print a meage to the LCD creen like PI. Before going onto tep 2, oberve integral windup. Start your code, but leave the motor amplifier off. After a few econd turn on the amp. The wheel hould take off pinning at maximum peed. After a few econd the wheel will die back down to the deired peed. 2. Check for integral wind-up. Add another if tatement inide your cae tatement that check if the control effort (u for motor and u2 for motor 2) are within the range of +/- 0. If the control command i outide thi region, make ure that the integral term doe not wind up by multiplying the integral um by Thi will force a decay of the integral term. GE423, Mechatronic Sytem Lab 5, Page 2 of 5

3 3. Build and run your code. Tet the robot on your bench. When you change the deired peed (you hould name thi variable vref ) by rotating encoder #3, the robot wheel velocity hould, after a mall tranient, match the deired peed. Tune the gain lightly to ee if you can achieve a quicker tranient repone in motor peed. deired velocity e2 u2 right_motor Integrator e u left_motor v v Integrator Figure : Independent PI control loop for the right and left motor. Exercie 3: Cloed-Loop Robot Driving with Coupled Control Loop (See Figure 2).. Add another cae tatement to ee if the third witch i back and all other are forward. In thi cae tatement, implement the coupled control algorithm hown in figure 2. Ue a _turn value of 3.0. Alo, print a meage to the LCD creen like Coupled PI. Ue the tuned gain you found i Exercie For thi cae, turn hould alway be zero and encoder 3 control the reference velocity. 3. Again, make ure that all integral term do not wind up uing the method given earlier. 4. Build and run your code. Tet thi code on the bench. Grab one of the wheel and ee what happen to the wheel on the oppoite ide. What i the advantage of thi control implementation? GE423, Mechatronic Sytem Lab 5, Page 3 of 5

4 deired velocity e2 u2 right_motor Integrator _turn e_teer 2 turn command e u left_motor v v Integrator Figure 2: Coupled PI control tructure. Exercie 4: Driving around.. Add another cae tatement to ee if the forth witch i back and all other are forward. In thi cae tatement you will again implement the coupled control algorithm hown in figure 2, but reference velocity hould be fixed at.0 tile/econd and turn updated by the value of optical encoder #3. Alo, print a meage to the LCD creen like Coupled PI Turn and on the other line print the value of turn. 2. Build and run your code. When you drive the robot around, you hould be able to teer it eaily with the attached encoder #3. 3. Alo at thi time try out your 3 other traight line mode on the floor. Can you ee a difference between open loop and cloed loop? In ummary, your final program hould ue the following witch logic: If witch 4 i back and all other are forward, run the coupled PI control law with Enc3 changing turn If witch 3 i back and all other are forward, run the coupled PI control law with Enc3 changing vref If witch 2 i back and all other are forward, run the two independent PI control law. If witch i back and all other are forward, run the open loop control law If all other cae, robot hould not move. Exercie 5: Create your initial LABVIEW interface. Same aignment a Lab 3 extended Prelab. GE423, Mechatronic Sytem Lab 5, Page 4 of 5

5 Add code to your DSP program o that your DSP end the PC the following information: it x-location (in tile), it y-location (in tile), it velocity (in tile/econd), and it orientation (in radian). See Ex 5 Step 3 below for the method of communicating thi data to LabView. Note: you have not been introduced to the rate gyro and the compa enor which will give you a bearing meaurement. For Lab 5 then y and orientation will alway be zero. In future lab additional enor reading may alo be ent. On part of your application window, the LABVIEW routine hould plot the poition and orientation of the robot. A your robot move around on the floor your depiction of the robot hould move around in your LABVIEW application. Ue Lab 4 LABVIEW aignment a a tarting point for thi aignment. Below are tep to guide your development of thi LABVIEW application. Each lab aignment from here on out will ak you to continue adding functionality to your LABVIEW application and demontrate it working at each of your lab check-off.. Create a LABVIEW program whoe window ue mot of the creen. In thi window create an area that will repreent the robot coure. Ue a 2D Picture object to create thi area. In the 2D picture draw grid line for a 6 by 6 tile coure. Ue a circle to indicate where the robot i located in the coure and draw a line to indicate the direction and velocity of the robot. The length of the line will indicate the velocity. 2. You will be able to pick a tarting location and orientation for you robot in the coure. 3. Uing the LABVIEW code from Lab 4, receive the X, Y, angle and velocity data ent from the DSP over WiFi. To end your variable to LABVIEW from the F28335 proceor, you imply aign the four given variable F28335_Extra, F28335_Extra2, F28335_Extra3, F28335_Extra4 to your x, y velocity and angle value. In Lab 6 we will be learning more about how all thi data tranfer i occurring. On the LABVIEW end pare the tring containing the four variable and convert the eparate tring into a number uing the Scan from String block. Thi dead reckoning method of navigation we are uing ha error that propagate the farther you travel (i.e. the error are integrated). While completing the remaining lab, tart thinking about how you might ue variou enor reading to meaure coure feature that will help correct your robot poition etimate. Example include meauring wall ditance to correct (x, y) poition, uing viual landmark to correct angular error, etc. Lab Check Off: Show your 4 control method and your LABVIEW interface to your intructor. Your LABVIEW interface hould indicate the robot X poition and velocity. You hould be able to anwer the following quetion:. Which method worked bet? Did you notice any ignificant behavioral difference in the controller? 2. What wa the point of the coupled loop approach veru the decoupled loop approach? Hint: Grab one of the wheel and hold it to prevent only one motor from moving. What happen to the other motor? What kind of robot driving ituation would caue one wheel to top moving? 3. How would you implement teering command uing the open-loop and independent PI control method? GE423, Mechatronic Sytem Lab 5, Page 5 of 5