Quellcode-Bibliothek

^© Kompilation durch diese Firma

[Weder Korrektheit noch Funktionsfähigkeit der Software werden zugesichert.]

Datei: TestSnippetMarkup.java Sprache: VDM

Original von: 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.20 Sekunden (vorverarbeitet) ¤

Download des Quellennavigators
Download des sprechenden Kalenders
in der Quellcodebibliothek suchen

Haftungshinweis

Die Informationen auf dieser Webseite wurden nach bestem Wissen sorgfältig zusammengestellt. Es wird jedoch weder Vollständigkeit, noch Richtigkeit, noch Qualität der bereit gestellten Informationen zugesichert.

Bemerkung:

Die farbliche Syntaxdarstellung ist noch experimentell.