Implementation of Real‐time control system using SHARK kernel
Contents
1. Parameters Previous encoder input Previous velocity Current velocity long k 0 int i 0 float suml 0 float sum2 0 float sum3 0 ifdef DEBUG char str 100 str1 100 endif TIME cmax csum int n Initialization prev trv c 0 prev pth c prev elv c gt U I prev_vel_trv 05 prev vel pth 0 prev vel elv 0 while 1 1 Read the encoders input travel c enc in CHN IN TRV pitch c enc in CHN IN PTH elevation c enc in CHN IN ELV Transform counter value to radius value travel travel c 8192 0 2 PI pitch pitch c 4096 0 2 PI elevation elevation c 4096 0 2 PI Compute the velocity of elevation pitch and travel vel elv elevation c prev elv c float PERIOD EVENT DET 1000000 0 4096 0 2 PI vel pth pitch c prev pth c float PERIOD EVENT DET 1000000 0 409 6 0 2 PI vel trv travel c prev trv c float PERIOD EVENT DET 1000000 0 81 92 0 2 PI Low pass filter if k LEN 1 suml 0 sum2 0 sum3 0 filtl k vel elv filt3 k vel pth filt2 k vel trv for i20 i k i t U suml filtl i sum2 filt2 i sum3 filt3 i curr vel elv suml k 1 curr vel pth sum2 k 1 curr vel trv sum3 k 1 else curr vel elv prev_vel_elv vel elv LEN filtl 0 LEN curr vel pth prev vel pth vel pth LEN filt2 0 LEN curr vel trv2. grx text s XMIN YMAX gray black endif TIMESPEC ASSIGN amp last amp now last dummy now dummy grx line JET DUMMY X x JET DUMMY Y JET DUMMY X x JET DUMMY Y height black grx_line JET_DUMMY_X x JET_DUMMY_Y height JET DUMMY X4x JET DUMMY Y JET DUMMY HEIGHT white grx line JET DUMMY X x 1 JET DUMMY WIDTH JET DUMMY Y JET DUMMY X x 1 JET DUMMY WIDTH JET DUMMY Y JET DUMMY HEIGHT 255 X X 1 JET DUMMY WIDTH task endcycle A jet task to collect control task and event detector task execution informati on and display on the screen TASK jet ctller void arg TIME sum max char st 50 st2 50 stl soy int n EDF level des lev EDF level des level table proc table controller PID ta Sk level for 77 Get the mean execution time max execution time In microseconds and nu mbers of missing deadlines for control task if jet getstat controller PID amp sum amp max amp n NULL 1 1 if n n 1 sprintf st 6d 6d 10s int sum n int max proc table controller PID name grx text st 300 YMIN 120 16 gray black sprintf st2 task has missed d deadlines lev gt tvec controller PID d1 miss grx text st2 300 YMIN 1204 16416 gray black Get the mean execution time max execution time in microseconds for ev ent detector task if jet getstat event PID 1 if n n 1 sprintf stl amp sum S6d 6d 10s name grx tex
3. Then you ll notice that the executing speed of the application is enhanced significantly cause you are reading the memory now instead of reading the floppy drive Implementation of Real time Control system using S Ha R K kernel Cong Chen cchen6 nd edu Software Part Il The S Ha R K kernel has made available device drivers of most computer hardware for application developers including keyboard input graphic card network card USB device etc The source files of these drivers reside in the driver folder under Shark kernel s root directory For more information about the device drivers and programming libraries see the S Ha R K User Manual Volume II for details which you can download at http shark sssup it In our implementation as we mentioned in previous section we use MultiQ3 data acquisition board to perform conversion between analog signal and digital signal We need to develop the driver for MultiQ3 board that is compatible with S Ha R K kernel The driver should be able to provide services containing analog to digital input digital to analog output encoder input etc For servo motor control experiment we only need the first two services to control the motor and collect data from the motor s sensor In the helicopter experiment since the sensor data are measured with encoders we ll further need to write the encoder input routine in the driver to properly control the helicopter In the following discussion w
4. 0 255 0 grx text The controller s in out state XMIN YMIN 60 rgb16 0 0 255 0 grx text system load XMIN YMIN 120 rgb16 0 0 255 0 grx text Mean Max Name 300 YMIN 120 gray black grx text Servo controller hard circle and soft box XMIN YMIN 10 rgbl6 2555 255 255 4 0 ys grx text press h create a hard circle XMIN YMIN 20 rgbl6 255 255 2 55 0 grx text press s create a soft box XMIN YMIN 30 rgbl6 255 255 255 0 grx text press ESC exit to DOS XMIN YMIN 40 rgbl6 255 255 255 0 Initialize and create Soft tasks including jet dummy task and jet controller task void init jet void SOFT TASK MODEL jl SOFT TASK MODEL jc PID pc soft task default model jl1 soft task def 1level j1 2 soft task def period jl PERIOD JETDUMMY soft task def met j1 WCET JETDUMMY soft task def group jl 1 soft task def usemath j1l soft task def ctrl jet j1 jet dummy PID task create jdmy jetdummy task amp jl NULL if jet dummy PID 1 sys shutdown message Could not create task jetdummy errno d errno exit 1 soft_task_default_model jc soft_task_def_level jc 2 soft task def period jc PERIOD JETCTRLLER soft task def met jc WCET JETCTRLLER soft task def ctrl jet jc soft task def group jc 1 pc task create jet ctrller jet ctller amp jc NULL if pe A d sys shutdown message Could n
5. K double K double K usefu int whit int blac int red int gray PID PID PID cont jet_ even hard soft hard soft pid Tni int int int PID desir float de float de XMIN 100 XMAX 540 YMIN 30 YMAX 450 YCTL 250 XMID 300 D 3 YMID 260 DUMMY PID 2 JET DUMMY WIDTH JET DUMMY HEIGHT JET DUMMY X JET DUMMY Y JET SLIDE WIDTH JET SLIDE X PI 3 1415926 LEN 25 in ep pp ed ei pd ll QC c qe TM o IDE l colors e k F roller PID dummy PID t PID period period weet 500 weet 500 e output sire out f sire out b 5000 10000 T 0 0 0 05 210 80 110 160 50 576 lowpass filter length di Mutex for accessing the print utility sem_t prt_mutex Count for int h_count int s_count graphic task numbers Hard graphic Soft graphic Hard graphic Soft graphic task period task period task WCET task WCET Task model HARD TASK MODEL m SOFT TASK MODEL n int travel c pitch c elevation c g elevation curr vel pth 0 79 float float float otor float tor float float float travel pitch curr vel trv Keep f ob Kb 5 1 69 travel offset 0 pitch offset 0 0 77 elevation offset 0 curr vel elv 20 97 L 09 0 65 i te 0 98 1 10 0 78 F Pd State feedback control gain float float float float float float ga
6. Servo motor and Helicopter using standard PC power amplifiers are needed to supply enough power to drive the plant The power mudules PA 0103 and UMP 2405 are used for driving DC motor and helicopter respectively See Figure 3 1 and 3 2 for a snapshot Figure 3 1 PA 0103 Figure 3 2 UMP 2405 4 DCServo motor The DC servo motor model SRV 02 is shown in Figure 4 The plant consists of a DC motor in a solid aluminum frame The motor is equipped with a gearbox The gearbox output drives external gears The basic unit is equipped with a potentiometer to measure the output load angular position It is also equipped with an optical encoder used to measure the load shaft angular position It offers high resolution 4096 counts in quadrature and measures the relative angle of the shaft D E gt d CO ANS 9NIL IQSNOO YISNYN Figure 4 SRV 02 Motor 5 3DOF Helicopter The 3DOF Helicopter consists of a base upon which an arm is mounted The arm carries the helicopter body on one end and a counterweight on the other The arm can pitch about an elevation axis as well as swivel about a vertical travel axis Encoders mounted on these axes allow for measuring the elevation and travel of the arm Two motors with propellers mounted on the helicopter body can generate a force proportional to the voltage applied to the motor The helicopter is shown in Figure 5 Figure 5 3DOF Helicopter Besides the hardware components that
7. amp heli state 2 0 pitch offset 0 3 heli state 2 1 If the user dosen t pull the joystick in the horizental dicrection X it maintains cruise control status else if JoyX gt 1 2 amp amp JoyX lt 2 8 amp amp heli_state_2 0 1 heli state 2 0 The state feedback control output computation for front motor and back mo tor separately ctrller f gain elevation Kf 0 curr_vel_elv Kf 1 elevation ele vation offset gain pitch Kf 2 curr vel pth Kf 3 pitch pitch off set gain travel Kf 4 curr vel trv travel offset ctrller b gain elevation Kb 0 curr vel elv Kb 1 elevation el evation offset gain pitch Kb 2 curr vel pth Kb 3 pitch pitch of fset gain travel Kb 4 curr vel trv travel offset If the current status of the helicopter is accelerating the control gain depends on the joystick input otherwise the control gains are updated in the pre vious computation if heli state 1 1 1 JoyY JoyY 1 3 0 ctrller f JoyY 4 ctrller b JoyY 4 else if heli state 1 1 1 JoyY JoyY 2 5 3 2 ctrller f JoyY 4 ctrller b JoyY 4 Make sure the absolute value of output voltage is not larger than 5 if ctrller lt 4 99 ctrller f 4 99 else if ctrller f gt 4 99 ctrller f 4 99 if ctrller b lt 4 99 ctrller b 4 99 else if cuirller b gt 4 99
8. ctrller b 4 99 Output the control signal to MultiQ board daout CHN OUT FRT ctrller f daout CHN OUT BCK ctrller b Print the control information in the screen sem wait amp prt mutex Sprintf strl travel 6d pitch 6d elevation evation c 6d travel c pitch c el Sprintf str2 the output of the travel rate controller is 8 6f 8 6f ctrl ler f ctriler b sprintf str3 curr vel elv 6 4f curr vel pth curr vel elv curr vel pth curr vel trv 6 4f curr vel trv 6 4f Sprintf str4 travel 8 5f pitch 8 5f elevation 8 5f travel pitch elevation grx text strl XMIN YMIN 70 gray black grx text str2 XMIN YMIN 80 gray black grx text str3 XMIN YMIN 90 gray black grx text str4 XMIN YMIN 100 gray black sem post amp prt mutex task endcycle Event detector task TASK event det void arg float QI2 2 1 0 9 0 9 0 81 float beta 0 5 float M 21 21 float N 2 2 M 0 0 1 beta beta 0 0 0 M 0 1 O 0 1 M 1 p0 Or1 0 M 1 1 1 beta beta 0Q 1 1 N 0 0 1 0 2 1 beta beta 9Q 0 0 N 0 1 0O 0 1 N 1 1 L0 O 1 I0 1 N 1 1 1 0 2 1 beta beta Q 1 1 int prev trv c prev pth c prev elv c float prev vel trv prev vel pth prev vel elv float vel elv vel pth vel trv Array for low pass filter implementation float filtl LEN float filt2 LEN float filt3 LEN
9. look like this Aifndef MY MULTIQ define MY MULTIQ include Il sys cdefs h BEGIN DECLS float itov int iv integer to voltage int vtoi float v voltage to integer int adin int ch analog to digital in int daout int ch float voltage digital to analog out void reset da void void reset ad void int digin void digital input void digout int dig value digital output END DECLS endif Then we should put these files in the drivers directory within the shark folder Go to the drivers directory and create a new folder named multiq Copy the multiq c file into the multiq folder and create a new folder called include Go to the include folder and create a new folder called drivers Then copy the multig h file into the drivers directory that is just created We ll then move on to compile the source code to produce the object file that can be used by application developers Create a makefile in multiq directory In this case our makefile has the following content The MultiQ library ifndef BASE BASE endif include BASE config config mk LIBRARY multiq OBJS_PATH BASE drivers multiq OBJS multiq o include BASE config lib mk After you finish editing the makefile type make command in the terminal make sure you are currently in the multiq directory The compiler should produce tw
10. 0 convert to signed 32 bit result unsigned int high word lt lt 16 low word return int result Reset Analog to digital input channels void reset ad void outpw control reg AD AUTOCAL CONTROL MUST outpw control reg CONTROL MUST while inpw status_reg amp 0x08 0x00 T Reset Digital to Analog output channels void reset da void float zero_v zero v 0 0 daout 0 zero v daout l zero v daout 2 zero v daout 3 zero v daout 4 zero v daout 5 zero v daout 6 zero v daout 7 zero v I reset encoder channels void enc reset int ch 1 loutpw enc regl ch if ch 0 ch 2 ch 4 outp enc_reg1 ch amp 0x07 0x8 Af ch 1 ch 3 ch 5 outp enc_reg1 ch amp 0x07 0x10 control word CONTROL MUST ch lt lt 3 outp control reg control word outp enc reg2 EFLAG RESET outp enc reg2 BP RESET outp enc reg l CLOCK DATA outp enc reg2 CLOCK SETUP outp enc reg2 INPUT SETUP outp enc reg2 QUAD X4 outp enc reg2 CNTR RESET Read digital input int digin void 1 return inpw digin port Perfom digital output void digout int dig value 1 outpw digout port dig value Compiling the driver Assume the driver source file is named multiq c we need a head file containing the function declarations for the source file Create a head file named multiq h and the content should
11. EY PRESSED keyb hook k zerofun FALSE k flag 0 k scan KEY S k ascii s k status KEY PRESSED keyb hook k hook func FALSE k flag 0 k scan KE k status KEY_PRESSED keyb_hook k hook_func FALSE k flag 0 k scan KEY O k ascii o k status KEY PRESSED keyb hook k hook func FALSE Activate the created tasks group activate 1 return 0
12. Implementation of Real time control system using SHARK kernel Hardware Setup The hardware setup of the real time control system consists of a PC computer an ISA MultiQ 3 data acquisition board amplifiers a DC servo motor and a 3DOF helicopter Each of these components will be briefly described below 1 Computer As shown in Figure1 A standard PC serves as the controller for the real time control system It means it is responsible for the computation of control update receiving the sampling data from the A D convertor and send the control output to the D A convertor It has a Pentium III processor 256M RAM and runs Free DOS system Figure 1 Computer 2 MultiQ 3 data acquisition board The MultiQ 3 is a general purpose data acquisition board with 8 analog inputs 8 analog output 16 bits of digital input 16 bits of digital output and up to 8 encoder inputs decoded in quadature The system is accessed through the PC ISA bus and is adressable via a 16 consecutive memory mapped locations which are selected through a DIP switch located on the board The MultiQ 3 board consists an main board which reside inside the PC by inserting it into the ISA slot and a terminal board which is an extensive part of the main board to interact with other components as shown in Figure 2 1 and 2 2 Figure 24 MultiQ 3 Main Board d Figure 2 2 MultiQ 3 terminal Board 3 Amplifiers Since we are dealing with controlling phsical plant including DC
13. are listed above the system cable connection scratches for the real time control system DC motor and Helicopter are shown in Figure 6 1 and Figure 6 2 6 Pil MINI DIN gnd 9nd OON S110 2 or 83 or 84 NO QZ 19 71 tis ds QUANSER CONSULTING QUICK CONNECT MODULE L C Q 2 Power Operational Arnplifier 1 2 3 4 to A D llow red TRS black ya So white S1 amp S2 FROM SENSORS red 2PIN trom DJA DIN gra x te rotor black GND QUC 1 Genera configuration uf QCM Figure 6 1 Cable Connection for Motor control system Power Supply avs GND Ns SRO ERO UU Ge T MD KAS 3 ag EE BLE of CN a jo 2 B RG Cv E IR c E Power Amplifier TN Pad g ped l l From DIA 2 sU 9 To Load UPM2405 1 MultiQ Analog Output ChO M as TM me Re el Power Amplifier i L ea l mre 0 _ Fm DIA l ake oak To Load as UPM2405 2 Analog Output Ch1 To Encoder 0 on MultiQ Joystick TST ToMultiQ Analog Inputs 82 3 Joystick X To A D 2 4 Joystick Y To A D 3 X Gain 3 Cable Gain 3 Cable Encoders Travel Pitch Elevation To Encoder 1 on MultiQ To Encoder 2 on MultiQ Figure 4 Wiring Diagram Figure 6 2 Cable Connection for Helicopter control system Implementation of Real time control system using S Ha R K kernel Software part I Cong Chen cchen6 nd edu The S Ha R K Soft and Hard Real time Kerne
14. ctor of task descriptors EDF level des Helicopter control task TASK servo ctrl void arg 1 float ctrller f ctrller b front and back propeller controller char str1 100 char str2 100 char str3 100 char str4 100 float JoyY float JoyX while 1 1 Read Joystick input JoyX itov short adin CHN JOYX JoyY itov short adin CHN JOYY Set the status of the helicopter to deccelerate if user pull the joystick down at the same time release the traval control loop if heli state 1 0 amp amp JoyY lt 2 5 4 heli state 1 1 gain travel 0 Set the status to accelerate if user push joystick up and realease the tra vel control loop else if heli state 1 0 amp amp JoyY gt 1 Q heli state 1 1 gain travel 0 Set the status to cruise control if user stop using the joystick reset th e travel gain and offset to maintain current state else if JoyY lt 0 5 amp amp JoyY gt 2 0 amp amp heli state 1 0 1 heli state 1 0 gain travel gain travel def travel offset curr vel trv elevation offset elevation If the user pulls the joystick to the left the desired pitch angle decrea ses by 0 3 radius if JoyX lt 1 amp amp heli state 2 0 1 pitch offset 0 3 heli state 2 1 If the user pulls the joystick to the right the desired pitch angle incr eases by 0 3 radius else if JoyX gt 3 amp
15. e ll list the MultiQ3 driver source code for S Ha R K kernel as well as the process to integrate the driver into the kernel so the application developer can access the driver function in a quite easy way 1 MultiQ driver source code multiq c driver of multiQ3 board for SHaRK y include math h include lt kernel kern h gt define base_port 0x320 define digin port base_port 0x00 define digout_port base_port 0x00 define dac_cs base_port 0x02 define ad_cs base_port 0x04 define status_reg base_port 0x06 define control reg base port 0x06 define clk reg base port 0x08 define enc regl base port 0x0c define enc reg2 base port 0x0e define AD SH 0x200 define AD AUTOCAL 0x100 define AD AUTOZ 0x80 define AD MUX EN 0x40 define AD CLOCK 4M 0x400 define CONTROL MUST AD SH AD CLOCK 4M define BP RESET 0X01 define TRSFRCNTR OL 0X10 define CLOCK DATA 0 define CLOCK SETUP 0x18 define INPUT_SETUP 0x41 define QUAD X4 0x38 define CNTR RESET 0x02 define EFLAG RESET 0x06 unsigned short control word CONTROL MUST Interger 16bit to voltages 5 5v conversion float itov int iv 1 return 5 float iv 4095 j voltages 5 5v to Interger 16bit conversion int vtoi float v return ceil v 2048 5 2047 j Read the analog to digital input int adin int ch i unsigned short hb Ib short toolong maxcnt maxcnt 30 nosound control
16. en horizontally and the only difference is the object shape and moving speed The hard task model is a moving box while the soft one is a moving circle In this uni processer real time system single CPU is shared by various tasks running at the same time Scheduling algorithms are used to allocate the CPU resource to different tasks and guarantee that the timing constraints can be meet In this application we use EDF Earliest Deadline First scheduling algorithm to schedule hard tasks and RRSoft Round Robin for soft tasks scheduling algorithm to schedule the soft tasks General RR Round Robin algorithm is used to schedule the non realtime tasks The scheduling algorithms can be identified in the initialization file and registered at run time when the program starts The main program is listed as followed The program is self explanatory since comments have been added for each block SHa A63 Unive Progr Z T ZT amp e OE include include include include include include include include include include include include include include define define define define define define define define define define define define 1 task define define define define define define define define define R K System amp Control Lab rsity of Notre Dame am Writer Cong Chen lt kernel kern h gt lt sem sem sem h gt lt stdlib h gt lt math h gt lt fs sysca
17. in elevation 0 gain pitch 0 gain travel 0 gain elevation def gain pitch def 0 gain travel def 0 Helicopter state 1 value 0 cruise control int heli state 1 0 Helicopter state 2 value 0 cruise control ue 1 int heli state 2 0 EDF task descriptor typedef struct int flags TIME period TIME rdeadline TIME offset struct timespec struct timespec int dl timer int eop timer int off timer int dl miss int wcet miss int act miss int nact EDF task des release Level descriptor typedef struct level des 1 int flags adeadline 0 status Value 1 accelerate Value 1 status Value 1 decrease pitch angle by 0 3 radius task flags period or inter arrival interval relative deadline release offset release time of current instance latest assigned deadline deadline timer end of period timer timer offset deadline miss counter WCET miss counter activation miss counter number of pending periodic jobs Standard level descriptor level flags control gain for increse pitch angle by 0 3 radius ty encoder input counts actual value in current velocity control gain for radius front m back mo deccelerate Val IQUEUE ready the ready queue bandwidth t U used bandwidth EDF task des tvec MAX PROC ve
18. l kernel is basically a set of libraries that the developers can statically link together to form an application bootable image The application is executed using the X EXE DOS memory extender under the DOS operating system or directly run when the system is booted using GRUB The S Ha R K kernel provides various kernel level services that application developer scan call at the development stage including task management context switching scheduling algorithms etc It has also provided drivers for most common computer hardware so that the developer can focus more on the application level instead of worrying about the interaction between the hardware and software For more details about the SHARK kernel please refer to the Document voll and vol4 which you can download at the homepage of S Ha R K project http shark sssup it Typically developing an S Ha R K application involves two major software components an initialization file and a program source file The initialization file is responsible for initializing the device drivers registering the modules needed both scheduling and resource modules including the headfiles etc The program sources file is where the classical main entrance function resides The main function will be automatically called after the application starts Here we list a simple application to briefly explain these two major components The application will simply print Hello world in the screen Commen
19. ll h gt lt drivers shark_linuxc26 h gt lt drivers shark_keyb26 h gt lt drivers shark_fb26 h gt drivers multiq h lt kernel func h gt lt kernel descr h gt lt edf edf edf h gt lt kernel model h gt lt kernel var h gt DEBUG CHN IN 4 CHN OUT 6 CHN IN TRV 0 CHN IN PTH 1 CHN IN ELV 2 CHN OUT FRT 0 CHN OUT BCK 1 CHN JOYX 2 CHN JOYY 3 PERIOD CTRLLER 100000 WCET CTRLLER 2000 PERIOD JETDUMMY 100000 WCET JETDUMMY 400 WCET JETCTRLLER 400 PERIOD JETCTRLLER 100000 PERIOD EVENT DET 5000 WCET EVENT DET 2000 HVEL 5 SVEL 3 MAX T 50 Department of Electrical Engineering application for real time 3DOF Helicpter control system with both periodic trigger and event trigger approach Encoder Input Channel for travel Encoder Input Channel for pitch Encoder Input Channel for elevation Output Channel for Front propeller Output Channel for Back propeller Input Channel for Joystick X Input Channel for Joystick Y Period for control task In microseconds WCET worst case execution time for contro Period for event detector task WCET for event detector task Velocity for Hard box task Velocity for Soft circle task Max number of addable tasks Parameters to draw the display senario define define define define define define define define Parameters for jet_dummy task define define define define define define define define define float ga double K double K double
20. minal under the application directory If the make process is successful an executable file will be generated within the current directory Otherwise some error messages generated by the compiler will appear in the terminal and you have to reexamine your source codes to fix the corresponding problem Since the application cannot run in the Linux Desktop we ll need a floppy to copy the executable file to the target machine running the FreeDOS system In Linux console type the following command to mount the floppy mount mnt floppy Before doing this make sure the floppy has already been inserted into the computer Then you can type cp AppDirectory my app mnt floppy AppDirectory represents the directory you build the application After finishing copying type umount mnt floppy to unmount the floppy Then you can insert the floppy to the target computer and access the floppy driver in FreeDOS by typing the command B and you should be in the Floppy directory Finally you can run your application by typing X my app That s basically all you need to do to compile and execute an S Ha R K application Really simple isn t it Note that there is an alternative way to execute the application in the target machine You can use the A drive which is a high speed RAM cache to run the executable Basically you only need to explicitly copy the executable file from the floppy drive B to A drive by typing A copy B my_app X my app
21. o new files called multiq o and libmultiq a The multiq o is an object file compiled from the original multiq c source file The libmultiq a is a library file can be added into the shark library and referred by the developer Now the driver source file has been successfully compiled we ll show how to integrate the driver into the shark library so that the application developer can call the driver routine without caring much about the driver itself Integrating the driver into the S Ha R K library In the previous section you probably have noticed that in the sample makefile for developing an S Ha R K application the device drivers is used by identify SHARKOPT LINUXC26 PCI INPUT FB This means that the application is going to use the Linux 2 6 compatibility layer which is always required when the drivers are used the PCI driver the Input driver to manage the input devices like keyboard mouse etc and the Frame Buffer driver for the graphical display The list of all the available libraries can be found into the Lib directory within shark folder In order to use the MultiQ driver in the similar meaning by add MULTIQ into the SHARKPORT string we have to do the following two steps Firstly copy the libmultiq a file we get after compiling into the lib directory within the shark folder to make the MultiQ library available to application developer Second go to the config director
22. ok like Then you are on your way to build your own application and we ll show you how to compile build and execute your application step by step Typically the development environment for S Ha R K application could be either Linux or DOS In Lab A63 we develop the application in those computers running RedHat Linux Workstation version There are several C source code editing tools that are available under Linux system including emacs vim gedit etc You can choose either editing tool based your own preference The GCC compiler will be used to compile your C source code under Linux It has already been installed and upgraded to the latest version which is GCC4 Before you start working on your own application make sure that the S Ha R K kernel libraries modules and common device drivers have been correctly built in the system Check out http shark sssup it for more details about how to build SHARK kernel After you have done editing your S Ha R K application the next step is to compile all the source files and link them together with the required libraries to form an executable image In Linux system using the GNU makefile utility you can complete this process by simply typing make in the command line The makefile is a special script file that can be invoked by the make command if you don t know makefile at all check out the official GNU site for make utility A typical makefile example for S Ha R K application is sh
23. opter model the encoders goes through the MultiQ3 board so that the computer is able to process and update the control computation The software program of the helicopter control application consists of several concurrently running tasks Typically the real time tasks can be divided into two categories hard task and soft task Hard tasks normally perform critical actions and have strict timing condition constraints deadline which might include control computation task event detector task etc Comparing to hard tasks soft tasks have relatively loose constraint in terms of deadlines which means that missing deadlines is tolerable to some extent and will not cause system failure These tasks usually include screen graphic tasks statistic data collection task keyboard input task and etc In our helicopter control application we ll have three hard tasks and three soft tasks The hard tasks are helicopter control task event detector task and a graphic task that can be triggered by user input The reason for having a graphic hard task here is that we can simulate multiple control loop environment by simply adding graphic hard task instead of introduce new control plant which is not necessary to conduct the experiment The soft tasks are task execution statistics collection task system load estimation task and a graphic soft task The graphic soft task is very similar to the graphic hard task Both of them will simply move an object on the scre
24. ot create task lt jet_ctrller gt errno d errno exit 1 Keyboard Callback function Terminate control task reset the AD DA board and exit the application void endfun KEY_EVT k t task kill controller PID sleep 1 reset ad reset da exit 0 Keyboard Callback function Clean the task execution statistic data void zerofun KEY_EVT k int i for i20 i MAX T i jet delstat i Keyboard Callback function create and activate new graphic task void hook_func KEY_EVT k switch k gt ascii 1 case h hard task default model m hard task def ctrl jet m hard task def arg m void h count hard task def wcet m hard wcet hard task def mit m hard period hard task def usemath m pid task create hardcircle cir amp m NULL if pid NIL sys_shutdown_message Could not create task lt hardcircle gt exit 1 task_activate pid h_count break case s soft_task_default_model n soft_task_def_ctrl_jet n soft_task_def_arg n void s_count soft_task_def met n soft_wcet soft_task_def_period n soft_period soft_task_def_usemath n pid task_create softbox box amp n NULL if pid NIL sys_shutdown_message Could not create task lt softbox gt exit 1 task_activate pid s_count break case p desire out_f 0 1 desire out b 0 1 break case o desire out f 0 1 desire o
25. own as followed makefile ifndef BASE BASE endif include BASE config config mk PROGS App Name include BASE config example mk 10 App Name 11 make f SUBMAKE APP App Name OTHEROBJS source2 0 source3 o 12 SHARKOPT LINUXC26 PCI INPUT FB co J G Lh S LA LA NO The example makefile is trying to build an application called App Name In this application the main function is included in the source file App Name c The other source files including source2 c and source3 c in this case should also be contained to build the App Name application To compile these other source files you should put the name of the source files with the o extension not c extension in the field after OTHEROBJS shown in the example The compiler will automatically compile all the c source file including App Name c So if you are going to develop an application called my app you should replace App Name in the example file at line 8 10 and 11 with your application name my app You should also replace the source2 o and source3 o with the name of the other source files appended with o extension For instance if you have only one other source file initfile c other than my app c you should replace source2 0 source3 o with initfile o After you modify the makefile in order to build your own application with the name you identify you can now type make command in the ter
26. prev vel trv vel trv LEN filt3 0 LEN for i20 i lt LEN 1 i 1 fizlti i fi LET eel TS filt2 i ile2 341 filts i filt3 if1 filtl LEN 1 vel elv filt2 LEN 1 vel pth filt3 LEN 1 vel trv kes Update the previous state value with current value prev_elv_c elevation_c prev pth pitch cj prev trv c travel c prev vel elv curr vel elv prev vel pth curr vel pth prev yel try curr vel trv task endcycle This task is responsible for collecting information of the current system load and displaying results on the screen TASK jetdummy task void arg 1 TIME now dummy last dummy diff dummy slice struct timespec now last diff float f now dummy f last dummy f diff dummy f slice int x 0 int height char s 50 NULL TIMESPEC amp last last dummy 0 for 73 1 task nopreempt jet getstat DUMMY PID NULL NULL NULL amp now dummy Sys gettime amp now task preempt SUBTIMESPEC amp now amp last amp diff slice diff tv sec 1000000 diff tv nsec 1000 diff dummy now dummy last dummy height int JET DUMMY HEIGHT float diff dummy float slice ifdef DEBUG f diff dummy float diff dummy f now dummy float now dummy f last dummy float last dummy f slice float slice sprintf s the height is d diff dummy is f last is f now is f slice i S f height f diff dummy f last dummy f now dummy f slice
27. t stl 300 task_endcycle Draw circle on the screen void draw cir int x GLX dise 2 yr D Oe int y Int e Draw box on the screen void draw box int x 1 grx box x y int y int c X de dy y tagi YMIN 120 16 8 amp max amp n int sum n gray NULL 1 int max proc_table event_PID black Graphic circle task moving horizontally on the screen TASK cir void arg int int int int int int i x Ox ax col outx int arg Nor oy red X ox XMIN y oy YMID 2 i dx HVEL red 100 10 i if red gt 255 red 255 col rgbl6 red 0 50 while 1 x dx outx x gt XMAX if outx x x dx dx dx x dx 1 color of x lt XMIN circle sem wait amp prt mutex draw cir ox oy 0 draw cir x y eol Ox Xj oy yi sem post amp prt mutex task endcycle Graphic box task moving horizontally on the screen TASK box void arg int x y int ox oy int dx int col blue int outx int i int arg X OX XMAX 4 y oy YMID 1 2 i dx SVEL blue 100 10 i if blue gt 255 blue 255 col rgbl6 50 0 blue color of box while 1 1 x dx outx x gt XMAX x lt XMIN if outx x X dx dx dx x dx sem wait amp prt mutex draw box o
28. te Dame cchen6 nd edu Software 3 In this section we are going to explain the helicopter control application with emphasis on the software program The purpose of developing the helicopter real time control application is that we can further perform experiment to explicitly evaluate the event trigger control approach and the traditional periodic trigger control approach on a 3DOF helicopter model based on the experimental platform we built The platform contains several hardware components we mentioned earlier in previous section as well as an S Ha R K application that runs on a computer serving as the real time controller In the following discussion we will briefly introduce the hardware components as well as the software function The hardware setup for the helicopter real time control application consists of a computer running DOS operating system a MultiQ3 data acquisition board two UMP 2405 power amplifiers and a 3DOF helicopter model The system diagram and relationship between each component is shown in Fig 4 1 Figure 4 1 Hardware diagram As figure 4 1 shows the control signal is produced by control task being executed by the Computer s CPU Through the MultiQ3 board the output digital signal is converted into analog signal In order to drive the motors in the 3DOF helicopter the outputs of the D A convertor are then applied to the helicopter using power amplifiers Similarly the sensor data collected from the helic
29. ts are included to help understand the functionality of these subroutines HelloWorld Application 1 Initialization file including necessary system head include kernel kern h include rr rr rr h include dummy dummy dummy h sysyem tick in us define TICK 300 RR tick in us define RRTICK 10000 The kernel calls this function to register the modules that will be used in this application TIME kernel register levels void arg 1 struct multiboot info mb struct multiboot info arg The round robin and dummy scheduling modules are registered RR register level RRTICK RR MAIN YES mb dummy register level a TICK value in microseconds is returned which is the time will be used for programming the periodic timer interrupt of the PC return TICK j This function is called to initialize the device drivers keyboard graphic etc and modules TASK init void arg 1 struct multiboot info mb struct multiboot info arg nitialize the Hartik Port Layer HARTPORT Init call main mb return void 0 2 Program source file Including system head include kernel kern h int main int argc char argv 1 The main function simply display string Hello world on the screen console print and switch to new line cprintf Hello world n return 0 T Now you might have the idea about what does an S Ha R K application lo
30. ut b 0 1 break int main int argc char argv short test_inp float ad_inp float test_outp int in_channel out_channel char ci char str 50 int i Read the program s command line arguments and set the default gain respeciti vely if argc 4 gain elevation def atof argv 1 gain pitch def atof argv 2 gain travel def atof argv 3 else exit 1 Initialize parameters gain travel 0 gain elevation gain elevation def gain pitch gain pitch def travel offset 0 pitch offset 0 elevation offset 0 h count 0 S count 0 char pids 100 Userful colors white rgb16 255 255 255 black rgb16 0 0 0 red rgb16 255 0 0 gray rgb16 128 128 128 test outp 0 in channel 4 out channel 6 Reset the MultiQ D A output A D input and Encoders reset da reset ad for 22051 8 144 1 enc reset i Initialize print mutex and call initilization function to draw frame graph a nd create hard and soft tasks sem_init amp prt_mutex 0 1 init_servo init_jet scenario Hookup keys to different Keyboad Callback functions KEY EVT k k flag 0 k scan KEY Hj k ascii h k status KEY PRESSED keyb hook k hook func FALSE k flag CNTL BIT k scan KEY C k ascii c k status KEY PRESSED keyb hook k endfun FALSE k flag ALTL BIT k scan KEY Z4 k ascii z k status K
31. word CONTROL MUST AD MUX EN ch lt lt 3 outpw control reg control word toolong 0 while inpw status_reg amp 0x8 0x00 amp amp toolong lt maxcnt toolong outpw control reg control word toolong 0 while inpw status reg amp 0x8 0x00 amp amp toolong lt maxcnt toolong hf toolong gt maxcnt sound 400 outp ad cs 0 while inpw status reg amp 0x10 0x00 hb inp ad cs amp Oxff Ib inp ad cs amp Oxff outpw control reg CONTROL MUST return hb 8 Ib l perform digital to analog output int daout int ch float voltage 1 int 1value if voltage gt 5 voltage 5 else if voltage 5 voltage 5 else ivalue vtoi voltage outpw control reg 0x1800 ch CONTROL MUST outpw dac cs ivalue outpw control reg CONTROL MUST return 0 Read encoder input int enc_in int ch unsigned char low byte mid byte high byte unsigned short low word high word unsigned int result control word CONTROL MUST AD MUX ENJ ch 3 select channel outp control reg control word outp enc reg2 BP RESET outp enc reg2 TRSFRCNTR OL low byte inp enc regl1 amp Oxff mid byte inp enc regli amp Oxff low word low byte mid byte 8 amp Oxffff high byte inp enc_regl amp Oxff high word high byte amp Oxffff if high word amp 0x80 high word high word Oxff0
32. x oy 0 draw box x y col Ox X oy Yr sem post amp prt mutex task endcycle Initialize and create helicopter control task and event detector task Hard ta sks void init_servo void HARD TASK MODEL sv ed Initialize the Hard task model with parameters for event detector task hard task default model ed hard task def ctrl jet ed hard task def wcet ed WCET EVENT DET hard task def mit ed PERIOD EVENT DET hard task def usemath ed hard task def group ed 1 Create a new event detector task event PID task create event detector event det amp ed NULL if event PID NIL sys shutdown message Could not create task event detector exit 1 Initialize the Hard task model with parameters for helicopter control task hard task default model sv hard task def ctrl jet sv hard task def wcet sv WCET CTRLLER hard task def mit sv PERIOD CTRLLER hard task def usemath sv hard task def group sv l Create a new helicopter control task controller PID task create servo controller servo ctrl amp sv NULL if controller PID NIL sys shutdown message Could not create task servo controller exit 1 Draw the frame scenario on the screen void scenario void 1 grx rect XMIN D 1 YCTL D 1 XMAX D 1 YMAX D 1 rgbl6 0 255 0 grx rect XMIN D 1 YMIN 60 D 1 XMAX D 1 YCTL 10 D 1 rgbl 6
33. y within the shark folder and open a file called libdep mk Add following content into the file 4 MultiQ ifeq findstring _ MULTIQ S USELIB MULTIQ INCL IS BASE drivers multiq include ifeq LIB PATH libmultiq a wildcard LIB PATH libmultiq a LINK LIB Imultiq LIB DEP LIB PATH libmultiq a endif Then save the file it s done Using the driver After all the previous work finally it s time to user the MultiQ driver Recall that we have discussed in the precious section typically an S Ha R K application has two major software component the initialization file and the program source file In order to use the driver properly we need to first explicitly include the driver head file in the program source file In this case for instance we use my app c as the source file the following content should be contained in the beginning of the program source code to include the driver head file include lt drivers multiq h gt In addition as mentioned previously we should also explicitly add MULTIQ to the SHARKPORT string to tell the compiler to locate the MultiQ library in the lib directory The result should look like this SHARKOPT LINUXC26 PCI INPUT FB MULTIQ Then you are free to call any driver function in your application Have fun Implementation of Real time control system using S Ha R K kernel Cong Chen Department of Electrical Engineering University of Nor
Download Pdf Manuals
Related Search