MOBILE ROBOT CRUISE CONTROLLER

Transcription

1 University of Moratuwa B.Sc. Engineering Robotic Mini project 2006 MOBILE ROBOT CRUISE CONTROLLER By Cader M.F.M.A. (020046) Iynkaran N. (020153) Uthayasanker T. (020400) Department of electronic and telecommunication engineering University of Moratuwa Sri Lanka August 2006

2 Introduction Speed control takes important in many applications. The cruise controller is a special area that comes under this topic. The work carried out here is small mobile robot cruise controller which can cruise in and inclined plane with an angle less than 10 degree. The system contains the mobile robot platform which contains the mechanical and electrical parts and the controller which perform the controlling of the velocity and the pc interface which is used to set the set point velocity. Each of the sections are described separately in the following sections Mobile platform In order to implement the cruse control, we developed a mobile platform, in a more generic manner, since this concept of cruse control is very essential in many real life mobile robot applications. Bottom view of our platform Wheels Motor (DC) with encoder Dual (coupled) wheels Since this is designed to work on inclined surfaces (< 10 0 ), it needs higher torque. To avoid complicated mechanical system (as well as to reduce our cost), we went for (relatively) higher torque motors instead of using gears in the system. Note : Even though, it is much more easier to control the stepper motors, than DC motors, it doesn t make any real sense, when it comes to velocity control.

3 Although single motor is adequate for the testing of cruse control, as our primary focus is based only on velocity control, we used two motors. This because of following two reasons: 1. System should be more generalized (practically realizable) 2. Our final year project(which is an mobile) is going to use this differential drive control Like, many things in real life, this feature, introduces a new problem to our system. That is the straight line motion of the platform. We uncover certain causes for this problem. Two motors don t work in identical manner (especially the starting torques are different). For the mounting purposes, we are compelled to fix two motors in opposite directions. (i.e.: when the platform is linearly moving, two motors will operate in opposite directions.) But forward and reverse torque characteristics of each motors are different even though they are (almost) identical in the same direction. Friction (static as well as dynamic) varies significantly for each wheels in a randomized manner. The friction between the surface and the wheels is not constant, along the path. (All these problem are corrected through algorithms, up to a certain extent) Shaft encoding This is an important part in cruse control, since getting velocity feed back is very essential. Here it returns pulse stream proportional to actual velocity of the mobile platform. In our platform we used the Infrared Break beam sensors for shaft encoding, with a Single-Disk Shaft Encoder. A perforated disk is mounted on the shaft and placed between

4 the emitter detector pair. As the shaft rotates, the holes in the disk chop the infrared beam. Hardware and software connected to the detector keeps track of these infrared pulses, thereby monitoring the rotation of the shaft. Future enhancement of the system As mentioned earlier, straight line behavior is an important feature for a cruse controlled mobile platform. It has been observed in our system that, when one side of the platform suffers from a small obstacle on the surface, the straight line behavior is completely washed off. The following diagrams illustrate the proposed mobile platform. Bottom view of the platform Wheel(s) Friction-less rod (Free to rotate) Round rims (Mounted on the platform) Axis of rotation

5 Obstacle on surface (disturbing only one side of the mobile platform) It would be really significant if that platform is facilitated with some form of springs, in order to absorb sudden variation of forces acting on the platform. It ll reduce the stresses feel by the platform. Motor Controller In this platform we used DC motors. Hence we should develop a h-bridge cct that switches the motor as we want. We used L298 IC to develop this circuit. Figure 1 Motor Controller Circuit with L298 As shown in the figure a L298 IC contains two h bridges which can indepently drive two motors. We control this circuit using a pwm generated by pic IC which is given to the enable pin of these circuits. Pin 1. CURRENT SENSING A Used to control the current to the load(motor A). We can connect it to GND through a resistor and the current according to VS/R will be provided to the load. If we don t require to control the current, connect it to GND directly or via a small resistor (1 ohm). Pin 2. OUTPUT 1

6 Connected to one of the terminals of the motor A, and the diodes are connected to this pin (look at the circuit for the connections). Pin 3. OUTPUT 2 Connected to the other terminal of the motor A. Connect the diodes as given in the circuit diagram. It is better to use the Shotkey diodes which helps to safeguard the circuit from pwm variations. Pin 4. SUPPLY VOLTAGE (VS) Voltage with which the motor is to be driven. Can be as high as 46V, typical value is 9V. Lower limit deps on your purpose. Pin 5. INPUT 1 TTL Compatible Inputs 1 to drive Motor A. Should be logic voltages (0V or 5V). Determines whether motor is running clockwise or anticlockwise. Pin 6. ENABLE A TTL Compatible Enable Input for Motor A. Should be 5V for running the motor and 0 for not running/stopping the motor. Pin 7. INPUT 2 TTL Compatible Inputs 2 to drive Motor A. Should be logic voltages (0V or 5V).This input determines whether motor is running clockwise or anticlockwise. Pin 8. GND Ground Pin 9. LOGIC SUPPLY VOLTAGE (VSS) 5-7 volts Pin10. INPUT 3 TTL Compatible Inputs 1 to drive Motor B. Should be logic voltages (0V or 5V) Pin 11. ENABLE B TTL Compatible Enable Input for Motor B. Should be 5V for running the motor and 0 for not running/stopping the motor. Pin 12. INPUT 4 TTL Compatible Inputs 2 to drive Motor B. Should be logic voltages (0V or 5V) Pin 13. OUTPUT 3

7 Connected to one of the terminals of the motor B, and the diodes are connected to this pin(look at the circuit for the connections). Pin 14. OUTPUT 4 Connected to the other terminal of the motor B. Connect the diodes as given in the circuit diagram. Pin 15. CURRENT SENSING B Used to control the current to the load(motor B). We can connect it to GND through a resistor and the current according to VS/R will be provided to the load. If we don t require to control the current, connect it to GND directly or via a small resistance. This current can be used as a feedback of motor output but here we used shaft encoding to get the motor output feedback. Algorithm Our main objective of this project is to develop a mobile platform that regulates its speed on its own. The speed it should run can be specified using the computer interfaced with it. The computer displays the speed portfolio of the mobile platform in real time. Feedback Shaft Encoder PC MRCC Specified Speed PIC PWM L298 Motor Feedback At pc a software named as MRCC (Mobile Robot cruse controller) runs. It can specify the reference speed of the system and can plot the current speed of the system. The firmware runs in the pic takes the speed reference when the start command is given via pc and runs the cruise controller algorithm. This cruise controller algorithm holds there stages named as, 1. Starting torque generation 2. Linear behavior

8 3. Cruse behavior 1. Starting Torque Generation What ever the speed specified via PC we should give enough starting torque to move the platform. So we give optimum starting torque for a while to start the platform. This is done by controlling the duty cycle of the pwm. 2. Linear behavior We calibrate the system to run in a linear line and this is done by deviating the duty cycle of pwm given to the motors. Since motors are hanged in opposite direction the have to be fed with opposite phase power to drive both motors in same direction relative to the platform. This caused the duty cycle to differ for the motors to run at same speed. + - Motor - + Motor 3. Cruse behavior We develop only the P-controller which regulates the speed proportional to the amount of error. We suggest adding D-controller and then I-controller to enhance the performance of this platform. The code implemented in the PIC Microcontroller is given in Appix 1. PC Interface The PC interface was used to specify the required velocity that the motor controller should maintain. The interface was built using Matlab. The main task done by the User interface was as follows. Carry out serial communication task Converting the velocity information in to pulse counts and vise versa Plotting the velocity feedback received from the Motor controller In the serial communication the pulse counts are sent as integers including the control information which contain the direction. The Matlab Code for configuring serial port and calculations are given in Appix 2.

9 The flow chart of the algorithm as follows Initialize the program Wait for user input Wait for feedback Convert User Input into Pulse count Convert the Feedback in to Velocity S through Serial Port Plot the velocity vs. Time Graph

10 Appix 1 Pic code DEFINE HSER_RCSTA 90h DEFINE HSER_TXSTA 20h DEFINE HSER_BAUD 2400 DEFINE HSER_SPBRG 25 DEFINE HSER_CLROERR symbol AselectF = PORTB.3 symbol AselectR = PORTB.5 symbol BselectF = PORTB.6 symbol BselectR = PORTB.7 symbol EncoderA = PORTB.0 symbol EncoderB = PORTB.4 i var byte ref var word VelocityA var byte VelocityB var byte datain var byte DirectionA var bit DirectionB var bit DutyA var byte DutyB var byte FeedbackA var word FeedbackB var word FeedbackAD var word FeedbackBD var word high AselectF high AselectR high BselectF high BselectR HPWM 1,255,250 HPWM 2,255,250 VelocityA = 0 VelocityB = 0 datain = 0 DirectionA = 1 DirectionB = 1

11 DutyA = 255 DutyB = 255 i = 0 ref = 0 loop: pause 10 i = i+1 FeedbackA = 0 FeedbackB = 0 FeedbackAD = 0 FeedbackBD = 0 (feedback is s to pc) PULSIN EncoderB,1,FeedbackBD FeedbackB = FeedbackBD / 255 FeedbackB.6 = 0 hserout [FeedbackB] PULSIN EncoderA,1,FeedbackAD FeedbackA = FeedbackAD / 255 FeedbackA.6 = 1 hserout [FeedbackA] (p controller) if i > 255 then if FeedbackB!= ref then DutyB = DutyB - (FeedbackB-ref)/2 if if feedbacka!= ref then DutyA = DutyA - (feedbacka-ref)/2 if if HPWM 1,DutyA,250 HPWM 2,DutyB,250 '=============================================================== ============ (get the input from pc) HSERIN 10,continue, [datain] continue:

12 if datain > 0 then if datain.6 =1 then if datain.5 =1 then high AselectF low AselectR DirectionA = 1 VelocityA = datain high BselectF low BselectR DirectionB = 1 VelocityB = datain else low AselectF high AselectR DirectionA = 0 VelocityA = datain low BselectF high BselectR DirectionB = 0 VelocityB = datain if if (assign the reference speed and bias the duty cycle to run linearly) ref = 6 + (3- (VelocityB / 3)) dutya = 214 dutyb = 141 HPWM 1,DutyA,250 HPWM 2,DutyB,250 pause 250 (first step of cruising) dutya = (VelocityB-8) * 5 dutyb = (VelocityB-8) * 5 HPWM 1,DutyA,250 HPWM 2,DutyB,250 if '=============================================================== ============ goto loop

13 Appix 2 1. Serial Port Initialization code s=serial('com1'); s.baudrate = 2400; s.databits = 8; s.parity = 'none'; s.stopbits = 1; s.bytesavailablefcnmode = 'byte'; s.bytesavailablefcncount=1; s.bytesavailablefcn fopen(s); 2. Code which take the user input and calculate the pulse count and transmit through Serial port velocitya=get(handles.edit1,'string'); velocityb=get(handles.edit2,'string'); velocityax=(-1)*str2num(velocitya); velocitybx= (-1)*str2num(velocityb); inputa = str2double(velocitya); inputb = str2double(velocityb); if isnan(inputa) isnan(inputb) errordlg('you must enter a numeric value','bad Input','modal') if isnan(inputa) str2num(velocitya)> 25 str2num(velocitya)< 0 set(handles.edit1,'string',[]); if isnan(inputb) str2num(velocityb)> 25 str2num(velocityb)< 0 set(handles.edit2,'string',[]); else if str2num(velocitya)> 25 str2num(velocityb)> 25 errordlg('you must enter a value less than 25','Bad Input','modal') if str2num(velocitya)>25 str2num(velocitya)< 0 set(handles.edit1,'string',[]); if str2num(velocityb)> 25 str2num(velocityb)< 0 set(handles.edit2,'string',[]);

14 else if str2num(velocitya)<0 str2num(velocityb)< 0 errordlg('you must enter a positive value','bad Input','modal') if str2num(velocitya)< 0 set(handles.edit1,'string',[]); if str2num(velocityb)< 0 set(handles.edit2,'string',[]); else counta=192; countb=128; set(hobject,'enable','off'); directiona=get(handles.directiona,'value'); directionb=get(handles.radiobutton5,'value'); %counta=counta + ceil((str2num(velocitya)*2)/pi); %countb=countb + ceil((str2num(velocityb)*2)/pi); counta=counta + ceil(str2num(velocitya)); countb=countb + ceil(str2num(velocityb)); if directiona==1 counta=counta+32; velocityax= (-1)*velocityax; if directionb==1 countb=countb+32; velocitybx=(-1)*velocitybx; serialport2=handles.serialport; fwrite(serialport2,counta,'uint8','async'); while(1) if ~(strcmp(serialport2.transferstatus,'write')) fwrite(serialport2,countb,'uint8','async'); display('out') break; set(hobject,'enable','on'); 3. code which receive the feedback and convert it to velocity and plot the velocity set(axisobject1,'visible','on');

15 axis(axisobject1,[ ]) set(get(axisobject1,'xlabel'),'string','time') set(get(axisobject1,'ylabel'),'string','velocity (cm/s)') set(axisobject1,'xminortick','on') grid( axisobject1,'on') set( axisobject2,'visible','on'); axis(axisobject2,[ ]) set(get(axisobject2,'xlabel'),'string','time') set(get(axisobject2,'ylabel'),'string','velocity (cm/s)') set(axisobject1,'xminortick','on') grid( axisobject2,'on') info = fread(hobject,1,'uint8'); display(info) if info > 64 if info > 96 resultvaluea= info-96; else resultvaluea= (-1)*(info-64); resultvaluea=(resultvaluea*pi)/2; set(text1object,'string',['current Velocity Reading: ' num2str(resultvaluea)]); velocityvectora(velocitycounta)=resultvaluea; velocityvectorax(velocitycounta)=velocityax; widtha=1:velocitycounta; if velocitycounta <100 velocitycounta =velocitycounta +1; else velocitycounta =1; velocityvectora=zeros(1,100); velocityvectorax=zeros(1,100); plot( axisobject1,widtha,velocityvectorax(widtha),'-r') hold( axisobject1,'on') plot( axisobject1,widtha,velocityvectora(widtha)) hold(axisobject1,'off') axis(axisobject1,[ ]) set(get(axisobject1,'xlabel'),'string','time') set(get(axisobject1,'ylabel'),'string','velocity (cm/s)') set(axisobject1,'xminortick','on') grid( axisobject1,'on') if info < 64 if info > 32 resultvalueb= info-32; else resultvalueb= (-1)*info;

16 resultvalueb=(resultvalueb*pi)/2; set(text2object,'string',['current Velocity Reading: ' num2str(resultvalueb)]); velocityvectorb(velocitycountb)=resultvalueb; velocityvectorbx(velocitycountb)=velocitybx; widthb=1:velocitycountb; if velocitycountb <100 velocitycountb =velocitycountb +1; else velocitycountb =1; velocityvectorb=zeros(1,100); velocityvectorbx=zeros(1,100); plot( axisobject2,widthb,velocityvectorbx(widthb),'-r') hold(axisobject2,'on') plot( axisobject2,widthb,velocityvectorb(widthb)) hold( axisobject2,'off') axis(axisobject2,[ ]) set(get(axisobject2,'xlabel'),'string','time') set(get(axisobject2,'ylabel'),'string','velocity (cm/s)') set(axisobject1,'xminortick','on') grid( axisobject2,'on')