Подсветка поворотов OBDII Arduino

Всем привет, нужна помощь в написании кода!
Есть идея реализовать на focus2 функцию подсветки поворотов противотуманками.
Принцип работы следующий, при вкл. зажигании и срабатывании датчика света, срабатывает реле и выдает нз контакт на ардуино. После этого аруино начинает считывать данные с датчика положения руля и при достижении заданного порога срабатывает реле для левой или правой ПТФ. Инфа о положении руля гуляет по HS CAN, так же инфу можно взять из PID 223201.
Пытался понять как мне все это дело реализовать на примере соседней темы "OBD-II диагностика на Arduino", но к сожалению не хватает мозгов...

Steering Wheel Angle PID: 223201 min: -800 max: 800 unit: deg equation: (A*6.25)-800 header: ABS
HS-CAN, CAN-ID 0x080 первые два байта (0-15 биты, считая слева направо) как раз искомый угол, (-1000) - (1000) град., разрешение 0.1 град.

Steering Wheel Angle PID: 223201 работает в програмке для диагностики torque. Данные с него идут следующие: при отклонении руля от 0 точки показания начинают меняться от 800 до 500, в другую сторону от -800 до -500 (500,800,0,-800,-500).