Safe AI Control for Robotics

reflex avoid @(rate(500Hz), wcet(60us)) {
  input:  ranges: i16[m][8], v_meas: i16[mps]
  output: v_cmd: i16[mps], w_cmd: i16[radps]
  
  loop {
    let min_d: i16[m] = 32767
    for i in 0..7: { 
      if (ranges[i] < min_d) { min_d = ranges[i] } 
    }
    
    v_cmd = clamp(min_d - 200, 0, 300)
    if (min_d < 150) { w_cmd = 120 }
    elif (min_d < 250) { w_cmd = 60 }
    else { w_cmd = 0 }
  }
  
  tests {
    test near_wall 
      inputs: { ranges = [100,100,100,100,100,100,100,100] }
      expect: { v_cmd = 0, w_cmd = 120 }
  }
}