NXTway -GS Controller Model based on Rate Monotonic Scheduling

Transcription

1 NXTway -GS Controller Model based on Rate Monotonic Scheduling This model consists of four parts : Device Inputs, Device Outputs, Task Scheduler, and Application Task Subsystem. Disclaimer : LEGO (R) is a trademark of the LEGO Group of companies which does not sponsor, authorize or endorse this demo. LEGO (R) and Mindstorms (R) are registered trademarks of The LEGO Group. 7 head_m 3 theta_m_l 4 theta_m_r 6 sonar 5 gyro 2 bluetooth _rx In A Revolution Sensor Interface 2 Port = A Priority = - In C Revolution Sensor Interface Port = C Priority = - BIn Revolution Sensor Interface Port = B Priority = - S2 Ultrasonic Sensor Interface Port = S2 Priority = - S4 Gyro Sensor Interface Port = S4 Priority = - BTRx Bluetooth Rx Interface Priority = - ### OSEK Tasks ### task_init: Init task_ts: [sec] task_ts3: 0. [ sec] task_ts4: [sec] ExpFcnCalls Scheduler Priority = 0 Added task _ts4 - main control task for controling logic of robot, with sample time ts4=0.0 defined in param _controller ## Requires only MATLAB products ## Generate code from task subsystem using RTW ## Requires additional 3rd party tools ## Generate code and build the generated code Download (NXT enhanced firmware ) Click the annotations to generate task_init task_ts task _ts3 task _ts4 task _init_fc task _ts_fc task _ts3_fc task _ts4_fc nxtway _app -EC Do All Download (SRAM) /build code and download it into NXT A Servo Motor Interface 2 Port = A Priority = C Servo Motor Interface Port = C Priority = B Servo Motor Interface Port = B Priority = BTTx Bluetooth Tx Interface Priority = Freq Dur Sound Tone Interface Priority = leave.wav Vol Sound WAV Interface 3 pwm _head pwm _l 2 pwm _r 4 pwm _r battery Battery Voltage Interface Priority = - Model rebuilded by Integra Lego Segway -Team Added extra interfaces for driving /reading third motor - motor A - head position K. Wesolowski => Added Extra "Do All" button, for conveniant reloading params, rebuildoing model and uploading rxe file to nxtway via USB System Clock Interface Priority = -

2 Application Task Subsystems Each function -call subsystems are drived by function -call signals of the task scheduler and generated as OSEK task functions in the generated code using Export functions feature of RTW -EC task_init_fc task_ts_fc task_ts3_fc task_ts4_fc function () function () function () function () task_init Initialization task_ts Balance & Drive Control task_ts3 Time Check & Battery Average Task K. Wesolowski => Added above task 4 and separated control logic from balance mechanism task_ts4 Main control Task Shared Data Data Store Memory is used as a shared data between tasks. Time Check & Battery Average flag_start DataType = boolean start time flag : gyro calibration is finished 0 : gyro calibration is not finished battery DataType = single average battery voltage gyro _offset gyro offset (zero point value ) DataType = single start_time program start time DataType = uint 32 head_initial_pos initial position - for positioning Servo DataType = int32 K. Wesolowski => Positioning head shared variables head_pos Sets target head angle DataType = int32 initial_theta_difference DataType = int32 turn _angle DataType = int32 Sets INITIAL wheels angle difference Sets body horizontal angle difference K. Wesolowski = > Controling robot turn - angle difference beetween wheels, remember that robot use absolute value, rather increment angle than make absolut (if you change 000 to 0 no matter f full circles robot will do them all forward _cmd Sets robot speed (use negative for going back ) K. Wesolowski => Shared command, -00 to 00 DataType = single

3 Initialization Task Set initial values and drive mode flag. f() start_time System Clock Read single battery Battery Voltage Read Round = Simplest S4 single gyro _offset Gyro Sensor Read Port = S4 Round = Simplest Out A head_initial_pos Revolution Sensor Read Port = A Out B initial_theta_difference Revolution Sensor Read 2 Port = B Out C Revolution Sensor Read Port = C

4 Task "ts" - Balance & Drive Control Control NXTway-GS to balance itself and drive autonomously or by remote control after gyro calibration. f() flag_start Balance & Drive Control NOT Gyro Calibration

5 "Balance & Drive Control " forward _cmd forward _cmd K. Wesolowski = > Shared control inputs - modified in task 4 here used turn _angle turn _angle pwm _l int8 Round = Simplest C Servo Motor Write Port = C Out C single theta _m _l Revolution Sensor Read Port = C Round = Simplest Out B Revolution Sensor Read Port = B S4 single Round = Simplest single theta _m _r gyro pwm _r int8 Round = Simplest B Servo Motor Write Port = B Gyro Sensor Read Port = S4 Round = Simplest Balance Controller K. Wesolowski = > Head positioning with simple P regulator Out A head _angle pwm _out A Revolution Sensor Read 2 Port = A Head Positioning Servo Motor Write 2 Port = A

6 NXTway -GS "Balance Controller " Calculate PWM duty to minimize the difference between reference and measured value. K. Wesolowski = > Anti windup Anti Windup forward _cmd thetadot _cmd x_ref Set Vector Control vol antiwindup value Cal Reference State - SP Process Vector LQ Regulator 5 gyro gyro 3 theta_m_l theta _m _l x battery battery pwm _l pwm _l 4 theta_m_r theta _m _r Cal x - current State - PV 2 turn _angle Target Theta diff New Angle Current Theta difference Sensor Read pwm _out pwm _turn pwm _r 2 pwm _r initial_theta_difference Initial angle Servo _Motor Controller K. Wesolowski = > P regulator for positioning left / right. It also keeps angle constant so it synchronizes whees when moving straightforward Calculate PWM

7 "Cal Reference State - SP" thetadot_cmd gp_max DataType = int8 Divide command values by maximum of game pad input. k_thetadot Low Path Filter Smooth velocity command using Low Path Filter to suppress rapid input change. in out in out theta_ref Discrete Integrator (forward euler ) 0 psi_ref DataType = single thetadot _ref x_ref 0 psidot_ref DataType = single K. Wesolowski = > removed extra theta output (doubling data from mux )

8 "Discrete Integrator (forward euler)" in ts z out

9 "Low Path Filter" -a_r in out a_r z Low Path Filter Function -a_r a_r*z^(-)

10 "Cal x - current State - PV" 2 theta_m_l pi/ 80 deg2rad theta_l 3 theta_m_r pi/ 80 deg2rad theta_r /2 theta gyro gyro gyro _offset Cal gyro_offset pi/ 80 Calculate gyro offset for removing gyro drift. deg2rad2 psidot in out psi Discrete Integrator (forward euler) Smooth measured velocity value using Low Path Filter to reduce the noise because it makes extra control input. in out Low Path Filter Discrete Derivative (backward difference) in out psi thetadot x psidot K. Wesolowski => removed extra theta output (doubling data from mux)

11 "Cal gyro_offset" gyro _offset a_gd gyro _offset gyro _offset Priority = - Priority = 0 Priority = gyro _offset gyro -a_gd Low Path Filter Function -a_gd a_gd*z^(-)

12 "Discrete Derivative (backward difference)" in / ts out z

13 "Discrete Integrator (forward euler)" in ts z out

14 "Low Path Filter" -a_d in out a_d z Low Path Filter Function -a_d a_d*z^(-)

15 "Calculate PWM" vol 2 battery batteryvol _max Cal vol_max Calculate maximum of DC motor voltatege from battery using an experimental expression pwm_turn Turn NXTway-GS by applying pwm offset to left and right motor in the counter direction. Friction Compensator gain = pwm _gain offset = pwm _offset Compensate coulombic and viscous friction of drive system max = 00 min = - 00 antiwindup value int8 Round = Simplest int8 PWM in left anti backslash PWM in PWM out PWM out right anti backslash 2 pwm_l max = 00 min = pwm_r max = 00 min = - 00 Round = Simplest K. Wesolowski => pwm_turn would be restricted, dependant on current robot stability (probably control output )

16 "Cal vol_max" battery vol _max DataType = single

17 "Left anti backslash" z PWM in 0 DataType = single direction change detection 0.05 DataType = single Switch in out Discrete Integrator (forward euler) DataType = int8 ts DataType = int8 Switch Sign PWM out

18 "Discrete Integrator (forward euler)" in z Unit Delay out

19 "Right anti backslash" z PWM in 0 DataType = single direction change detection 0.05 DataType = single Switch in out Discrete Integrator (forward euler) DataType = int8 ts DataType = int8 Switch Sign PWM out

20 "Discrete Integrator (forward euler)" in z Unit Delay out

21 "LQ Regulator" Anti Windup ts Feedback Gain 2 Set Vector in out k_i* u Discrete Integrator (forward euler ) Integral Gain Control 3 Process Vector k_f* u Feedback Gain K. Wesolowski => Separated LQ regulator from model, added anti windup feature to integral part Antiwindup reset integral when output is saturated on motors

22 "Discrete Integrator (forward euler)" in ts z Unit Delay out

23 "Servo_Motor Controller" New Angle single Round = Simplest Gain Saturation pwm_out 3 Initial angle 2 Sensor Read

24 "Head Positioning" head_pos New Angle Sensor Read pwm _out head_angle pwm_out Initial angle Servo_Motor Controller head_initial_pos

25 "Servo_Motor Controller" New Angle single 2 int8 Round = Simplest Round = Nearest pwm_out Gain Saturation 3 Initial angle 2 Sensor Read

26 "Gyro Calibration" Calculate gyro offset (zero point value). gyro _offset a_gc gyro _offset S4 Gyro Sensor Read Port = S4 single Round = Simplest -a_gc Low Path Filter Function -a_gc a_gc*z^(-)

27 Time Check & Battery Average Task ts3 Check start time (gyro calibration time) and autonomous start time. Smooth measured battery value using Low Path Filter to reduce the noise. f() System Clock Read time_start > = flag_start start_time DataType = uint32 ~= z Make Sound Make a sound when the gyro calibration is finished battery a_b battery single Round = Simplest Battery Voltage Read -a_b Low Path Filter Function -a_b a_b*z^(-)

28 "Make Sound" sound_freq DataType = uint32 sound_dur DataType = uint32 Freq Dur Sound Tone Write

29 Motion Task Controls motion and obstacle check of NXTWay f() W / R int 32 turn _angle System Clock Read start _time Turn Angle Round = Simplest Calculate wheels angle head _pos Head POS??? forward _cmd Forward Cmd S2 < = 0 Ultrasonic Sensor Read Compare To Constant Triggered Subsystem

30 "Triggered Subsystem" Trigger 00 Vol leave.wav Constant Sound WAV Write