| Quellcodebibliothek Statistik Leitseite products/Sources/formale Sprachen/VDM/VDMRT/ChessWayRT/ (Wiener Entwicklungsmethode ©) Datei vom 13.4.2020 mit Größe 6 kB |
|
Quelle RightController.vdmrt Sprache: VDM |
|||||||||
|
% RightController.vdmrt
\subsubsection{Creating the controllers -- the \texttt{RightController} class} The right controller in the ChessWay controls the right wheel and monitors the safety switch and the direction switch. The \texttt{RightController} is created by subclassing the generic \texttt{Controller} class and by overloading the operation prototypes for \texttt{CtrlLoop}, \texttt{PowerUp} and \texttt{printDiagnostics}. \begin{vdm_al} class RightController is subclass of Controller values -- values for P controller (velocity) KP2 : real = 20; KI2 : real = 78.431372549; KD2 : real = 0.0031875; BETA2 : real = 0.1 instance variables -- controller public ctrl : DTControl; -- sensors connected to the right controller public mSafetySwitch : SafetySwitch; public mOnOffSwitch : OnOffSwitch; public mDirectionSwitch : DirectionSwitch; operations -- constructor for the left motor controller public RightController: () ==> RightController RightController () == ( -- create the co-sim shared variables ChessWay`vel_in := new Sensor(); ChessWay`vel_out := new Actuator(); -- create the controller ctrl := new P(KP2); ctrl.SetSampleTime(SAMPLETIME); -- create the sensors mSafetySwitch := new SafetySwitch(self); mOnOffSwitch := new OnOffSwitch(self); mDirectionSwitch := new DirectionSwitch(self); -- call the controller base constructor Controller ("RIGHT") ); instance variables -- maintain a link to the other controller private mLeft : [LeftController] := nil operations -- auxiliary operation to hook controller models together public setLeftController: LeftController ==> () setLeftController (pLeft) == mLeft := pLeft pre mLeft = nil \end{vdm_al} Two auxiliary variables are introduced that will aid diagnostics. \texttt{mLoopCnt} will be incremented every time the control loop is executed and \texttt{mDebug} will influence the amount of diagnostics messages shown in the output log of the Overture tool. \begin{vdm_al} instance variables -- loop count variable private mLoopCnt : nat := 0; -- time at control loop entry private mTimeEntry : nat := 0; -- enable debug logging private mDebug : nat := 0 \end{vdm_al} The constructor of the \texttt{RightController} initialises the safety switch, the on/off switch and the direction switch. The operation \texttt{setLeftController} is used by the environment to link the left and right controllers. \begin{vdm_al} operations public CtrlLoopEntry: () ==> () CtrlLoopEntry () == duration (0) ( -- first increase the loop counter mLoopCnt := mLoopCnt + 1; -- capture the current time mTimeEntry := time; -- diagnostics if mDebug >= DEBUGCTRLLOOP then IO`printf("RightController.mainLoop (S) = %s (%s)\n", [mTimeEntry / 1E9, mLoopCnt]) ); public CtrlLoopBody: () ==> () CtrlLoopBody () == duration (0) ( --dcl hall : bool * bool * bool := -- mMotorSensor.getHallSensorData(), -- safe : bool := mSafetySwitch.getStatus(), -- onoff : bool := mOnOffSwitch.getStatus(), -- dir : DirectionSwitch`tDirectionStatus := -- mDirectionSwitch.getStatus(); -- execute the control loop -- mMotorActuator.SetValue(ctrl.Output(mMotorSensor.GetValue())); ChessWay`vel_out.SetValue(ctrl.Output(ChessWay`vel_in.GetValue())); -- -- execute the controller -- let pwm = computeResponse(hall, safe, onoff, dir) in -- mMotorActuator.setPWM(pwm); -- -- local diagnostics -- duration (0) -- if ChessWay`debug then -- ( -- IO`print("R-HAL = "); -- -- IO`print(hall); IO`print("\n"); -- -- IO`print("R-SAFE = "); -- -- IO`print(safe); IO`print("\n"); -- -- IO`print("R-ONOFF = "); -- -- IO`print(onoff); IO`print("\n"); -- -- IO`print("R-DIR = "); -- -- IO`print(dir); IO`print("\n"); skip ); public CtrlLoopExit: () ==> () CtrlLoopExit () == duration (0) ( dcl mTimeExit : nat := time; -- diagnostics if mDebug >= DEBUGCTRLLOOP then IO`printf("RightController.mainLoop (F) = %s (%s)\n", [mTimeExit / 1E9, mLoopCnt]); if mDebug > DEBUGCTRLLOOP then IO`printf("RightController execution time was %s\n", [(mTimeExit - mTimeEntry) / 1E9]) ) \end{vdm_al} The \texttt{CtrlLoop} operation obtains the value of all connected sensors and passes these to the \texttt{computeResponse} operation, which will return a real value that is in turn passed to the \texttt{setPWM} operation of the \texttt{MotorActuator}. Note that the execution time of the control loop is 100~msec. The \texttt{PowerUp} operation is called once at startup. It initialises the actuator and then starts the periodic control loop. \begin{vdm_al} operations public PowerUp: () ==> () PowerUp () == duration (100) ( mMotorActuator.initActuator(); mMotorActuator.printDiagnostics() ) pre mLeft <> nil operations -- prototype used for simulation diagnostics public printDiagnostics: () ==> () printDiagnostics () == duration (0) Controller`printDiagnostics(mLoopCnt) \end{vdm_al} The \texttt{computeResponse} operation processes all the input parameters obtained from the sensors in the environment. As an initial model, we abstract away from the complexity of the actual algorithm by simply looking at the on/off switch only. If the on/off switch is set, then both motors will be actuated and set to run forward at 10~percent of its maximum power. If the on/off switch is reset then the motor is set to idle (\texttt{<FREERUNNING>}). \begin{vdm_al} operations public computeResponse: (bool * bool * bool) * bool * bool * DirectionSwitch`tDirectionStatus ==> real computeResponse (-, -, onoff, -) == ( if onoff then ( mMotorActuator.setActuated(); mLeft.mMotorActuator.setActuated() ) else ( mMotorActuator.setFreeRunning(); mLeft.mMotorActuator.setFreeRunning() ); return 0.1 ) end RightController \end{vdm_al} ¤ Dauer der Verarbeitung: 0.13 Sekunden (vorverarbeitet) ¤*© Formatika GbR, Deutschland |
|
2026-03-28