/*****************************************************************
  file   : cruise.lsd
  date   : 22.08.91
  author : I.Bridge (with minor revision by WMB 26/03/08)
  notes  : description of agents for a `cruise control' system
*****************************************************************/


TYPEDEF
  pushBtn_Type      = ENUM (pbUp, pbDown)
  cruiseStts_Type   = ENUM (csOn, csMaintain, csOff)
  throttleStts_Type = ENUM (tsOff, tsMan, tsAuto)
  engineStts_Type   = ENUM (esOn, esOff)

  accelPos_Type     = REAL(0.0, 1.0)             /* normalised position */
  brakePos_Type     = REAL(0.0, 1.0)             /* normalised position */
  throttlePos_Type  = REAL(0.0, 1.0)             /* normalised position */
  gradient_Type     = REAL(-100.0, 100.0)        /* percent             */


AGENT control_panel {
CONST
  minCruiseSpeed = 30      /* [km h^-1] */
  maxCruiseSpeed = 100     /* [km h^-1] */
STATE
  cruiseStts  : cruiseStts_Type = csOff
  cruiseSpeed : INT             = minCruiseSpeed
  onBtn       : pushBtn_Type    = pbUp /* switch on cruise controller      */
  offBtn      : pushBtn_Type    = pbUp /* switch off cruise controller     */
  incrBtn     : pushBtn_Type    = pbUp /* increment cruise speed           */
  decrBtn     : pushBtn_Type    = pbUp /* decrement cruise speed           */
  setBtn      : pushBtn_Type    = pbUp /* set cruise speed = current speed */
  resBtn      : pushBtn_Type    = pbUp /* resume cruise speed              */
  manBtn      : pushBtn_Type    = pbUp /* revert to manual operation       */
ORACLE
  onBtn, offBtn, incrBtn, decrBtn, setBtn, resBtn, brakePos, speed, engineStts
HANDLE
  cruiseStts, cruiseSpeed
DERIVATE
  braking is (brakePos != 0)
  press_btn(pushBtnSt) is ((pushBtnSt' == pbUp) && (pushBtnSt == pbDown))  
PROTOCOL
  (press_btn(onBtn) && (engineStts == esOn))
    --> cruiseStts = csOn
  (press_btn(offBtn) || (engineStts == esOff))
    --> cruiseStts = csOff
  ((press_btn(incrBtn) && (cruiseStts != csOff) && (cruiseSpeed < maxCruiseSpeed))
    --> cruiseSpeed++
  ((press_btn(decrBtn) && (cruiseStts != csOff) && (cruiseSpeed > minCruiseSpeed))
    --> cruiseSpeed--
  ((press_btn(setBtn) && (cruiseStts == csOn)) 
    --> cruiseSpeed = speed; cruiseStts = csMaintain;
  ((press_btn(resBtn) && (cruiseStts == csOn))
    --> cruiseStts = csMaintain
  ((braking  || pressBtn(manBtn)) && (cruiseStts == csMaintain))
    --> cruiseStts = csOn
}


AGENT throttle_manager {
CONST
  GainK = 0.5  /* auto throttle controller gain          */
  TimeK = 2.0  /* auto throttle controller time constant */
STATE
  throttleStts      : throttleStts_Type
  throttlePos       : throttlePos_Type
  deltaAutoThrottle : REAL
ORACLE
  speed, cruiseSpeed, cruiseStts, engineStts, accelPos
HANDLE
  throttlePos
DERIVATE
  speedErr is cruiseSpeed - measSpeed 
  deltaAutoThrottle is ((GainK * speedErr) - throttlePos') / TimeK
  throttlePos is 
    (throttleStts == tsOff)  ? 0.0 :
    (throttleStts == tsMan)  ? accelPos :
    (throttleStts == tsAuto) ? integ_wrt_time(deltaAutoThrottle, accelPos)
PROTOCOL
  (engineStts == esOff)
    --> throttleStts = tsOff  
  ((cruiseStts != csMaintain) && (engineStts == esOn))
    --> throttleStts = tsMan
  ((cruiseStts == csMaintain) && (engineStts == esOn))
    --> throttleStts = tsAuto
}


AGENT engine {
CONST
  maxEngineTorque = 74500
STATE
  engineStts   : engineStts_Type = esOff
ORACLE
  engineStts, throttlePos
HANDLE
  engineTorque, engineStts
DERIVATE
  engineTorque is 
    (engineStts == esOn)  ? maxEngineTorque * throttlePos :
    (engineStts == esOff) ? 0;
}


AGENT vehicle_dynamics {
CONST
  mass   = 2500    /* total mass of car & contents [kg]            */
  windK  = 50.0    /* wind resistance factor [N m^2 s^2]           */
  rollK  = 10.0    /* rolling resistance factor [N m^-1 s]         */
  gravK  = 9.81    /* acceleration due to gravity [m s^-2]         */
  brakK  = 1500.0  /* viscous friction braking constant [N m^-1 s] */
  forcK  = 40.0    /* torque to force conversion                   */
  sticK  = 100.0   /* static friction                              */
STATE
  actSpeed : REAL := 0.0  /* actual speed             */
  accel    : REAL 
  windF    : REAL         /* wind resistance force    */
  rollF    : REAL         /* rolling resistance force */
  gradF    : REAL         /* gradient force           */
  tracF    : REAL         /* engine traction force    */
ORACLE
  gradient, engineTorque, brakePos
HANDLE
  speed
DERIVATE
  windF is windK * pwr(actSpeed',2)
  rollF is rollK * actSpeed'
  gradF is gravK * mass * sin(gradient * pi / 200)
  brakF is brakK * actSpeed' * brakePos
  tracF is forcK * engineTorque;
  sticF is sticK * sgn(actSpeed') * bound(actSpeed', -0.01, 0.01)
  accel is (tracF - brakF - gradF - rollF - windF - sticF) / mass
  actSpeed is integ_wrt_time(accel, 0)
}


AGENT speed_transducer {  
CONST 
  wheelDiam   = 0.45           /* wheel diameter [m]           */ 
  wheelCirc   = pi * wheelDiam /* wheel circumference [m]      */ 
  wheelPuls   = 8;             /* pulses per wheel revolution  */
  countPeriod = 0.2            /* counter/timer period [s]     */
  maxCountVal = 65535          /* assumes 16-bit counter       */   
STATE
  measSpeed : REAL  
  pulseRate : REAL             /* wheel revs/sec [s^-1]       */
  countVal  : REAL             /* timer/counter value [s^-1]  */
ORACLE    
  actSpeed
HANDLE
  measSpeed
DERIVATE    
  pulseRate is int(actSpeed * wheelPuls / wheelCirc)         
  countVal is int(pulseRate * countPeriod) % maxCountVal
  measSpeed is (countVal * wheelCirc) / countPeriod 
} 
 

AGENT driver {
STATE
  accelPos : accelPos_Type
  brakePos : brakePos_Type
ORACLE
  engineStts, cruiseSpeed
HANDLE
  engineStts, accelPos, brakePos, onBtn, offBtn, incrBtn, decrBtn, setBtn, resBtn      
/* comment required */
}
  

AGENT environment {    
STATE
  gradient : gradient_Type  /* gradient (%) */
HANDLE
  gradient
/* comment required */
}