LED Control for VESC Express

led_vesc_express.lisp

; LED Control for VESC Express
; Copyright 2024 Syler Clayton <[email protected]>

; Debug flag for testing
(def debug 0)

; Features enabled
(def led-enable 1); Basic version works on previous VESC firmware
(def pubremote-enable 0); VESC 6.5 beta only. Also requires code-server
(def code-server-enable 0)  ; VESC 6.5 beta only. If timeout you probably don't have script on VESC which starts the code-server.
(def data-rx-enable 0) ; VESC 6.5 beta only. Needed for communication to float package for advanced LED support and pubremote telemetry.

;Pubremote config
(def esp-now-remote-mac '(132 252 230 80 168 12))

; VESC configuration
(def can-id 98); if can-id < 0 then it will scan for one and pick the first. Takes awhile on startup
(def cells-series 18)
(def idle-rpm 10.0)
(def led-delay 0.1)

; LED strip configuration
(def led-on 1)
(def led-highbeam-on 1)
(def idle-timeout 5)
(def idle-timeout-shutoff 120)
(def led-mode 3)
(def led-mode-idle 5)
(def led-mall-grab-enabled 1)
(def led-brake-light-enabled 0); TODO: "brake light" detection when current <-4.0A (regen)

;TODO: Need to verify logic for this
(def led-status-reversed 0)
(def led-front-reversed 0)
(def led-rear-reversed 0)

(def led-status-mode 0)
(def led-forward-color 0xFFFFFFFFu32)
(def led-backward-color 0x00FF0000u32)
(def led-brightness 1.0)
(def led-brightness-idle 1.0)
(def led-status-brightness 1.0)

; LED status configuration
(def led-status-pin 17)
(def led-status-num 10)
(def led-status-type 2)
(def led-status-duty-rpm 250.0)

; LED front configuration
(def led-front-pin 18)
(def led-front-num 19)
(def led-front-type 0)
(def led-front-has-laserbeam 1)

; LED rear configuration
(def led-rear-pin 19)
(def led-rear-num 13)
(def led-rear-type 2)
(def led-rear-has-laserbeam 0)

;GLOBAL_VARS_DONT_TOUCH
(def led-type 3)
(def led-current-brightness led-brightness)
(def vin-low (* cells-series 3.0));static
(def vin-high (* cells-series 4.2));static
(def direction 0)
(def previous-direction 0)
(def last-activity-time (systime))
(def idle-timeout-shutoff-event 0)
(def led-mall-grab 0)

(def fault-code -1)
(def pitch-angle -1)
(def roll-angle -1)
(def state -1)
(def switch-state -1)
(def rpm -1)
(def input-voltage-filtered -1)
(def speed -1)
(def tot-current -1)
(def duty-cycle-now -1)
(def distance-abs -1)
(def fet-temp-filtered -1)
(def motor-temp-filtered -1)
(def odometer -1)
(def battery-level -1)

(def rainbow-colors (array-create 24)) ; 6 colors * 4 bytes per color
(bufset-u32 rainbow-colors 0 0xFF0000u32)
(bufset-u32 rainbow-colors 4 0xFFFF00u32)
(bufset-u32 rainbow-colors 8 0x00FF00u32)
(bufset-u32 rainbow-colors 12 0x00FFFFu32)
(bufset-u32 rainbow-colors 16 0x0000FFu32)
(bufset-u32 rainbow-colors 20 0xFF00FFu32)
(def rainbow-index 0)

; Constants
(def BLACK 0x00)
(def WHITE 0xFF)

(def debug-val 1)
(def debug-timer (systime))

;Initialize the LED strips
(defun init-leds () {
    (if (>= led-status-pin 0)
        (def led-status-buffer (rgbled-buffer led-status-num led-status-type))
    )
    (if (>= led-front-pin 0)
        (def led-front-buffer (rgbled-buffer led-front-num led-front-type))
    )
    (if (>= led-rear-pin 0) {
        (def led-rear-buffer (rgbled-buffer led-rear-num led-rear-type))
    })
})

;Update the status LED bar
(defun update-status-leds () {
    (if (> rpm led-status-duty-rpm){
        (duty-cycle-pattern)
    }{;else
        (if (= switch-state 0){
            (battery-pattern led-status-buffer led-status-num)
        }{;else
            (footpad-pattern switch-state)
        })
    })
})

;Update the status LED bar based on duty cycle
(defun duty-cycle-pattern () {
    (var scaled-duty-cycle (* (abs duty-cycle-now) 1.1112))
    (var clamped-duty-cycle 0.0)

    (if (< scaled-duty-cycle 1.0) {
        (setq clamped-duty-cycle scaled-duty-cycle)
    } {;else
        (setq clamped-duty-cycle 1.0)
    })

    (var duty-leds (floor (* clamped-duty-cycle led-status-num)))
    (var duty-color 0x00FFFF00u32)

    (if (> (abs duty-cycle-now) 0.85) {
        (setq duty-color 0x00FF0000u32)
    } {;else if
        (if (> (abs duty-cycle-now) 0.7) {
            (setq duty-color 0x00FF8800u32)
        })
    })

    (looprange led-index 0 led-status-num {
        (rgbled-color led-status-buffer led-index (if (< led-index duty-leds) duty-color 0x00000000u32) led-status-brightness)
    })
})

;Update the status LED bar based footpad
(defun footpad-pattern (switch-state){
    (var color-status-half1 (if (or (= switch-state 1) (= switch-state 3)) 0xFF BLACK))
    (var color-status-half2 (if (or (= switch-state 2) (= switch-state 3)) 0xFF BLACK))
    
    (looprange led-index 0 led-status-num {
        (rgbled-color led-status-buffer led-index (if (< led-index (/ led-status-num 2)) color-status-half1 color-status-half2) led-status-brightness)
    })
})


; Battery LED strip based on the current voltage
(defun battery-pattern (led-buffer led-num) {
    (var voltage-range (- vin-high vin-low))
    (var voltage-per-led (/ voltage-range led-num))
    (var num-lit-leds (floor (* led-num (/ (- input-voltage-filtered vin-low) voltage-range))))
    (var percent-remaining (/ (- input-voltage-filtered vin-low) voltage-range))

    (looprange led-index 0 led-num {
        (var red 0)
        (var green 0)

        (if (or (< led-index num-lit-leds) (and (= led-index 0) (<= num-lit-leds 1))) {
            (if (or (< percent-remaining 0.2) (and (= led-index 0) (<= num-lit-leds 1))) {
                (setq red 255)
                (setq green 0)
            } {;else
                (setq red (floor (* 255 (- 1 (/ percent-remaining 0.8)))))
                (setq green (floor (* 255 (/ percent-remaining 0.8))))
            })
        } {;else
            (setq red 0)
            (setq green 0)
        })
        (var color BLACK)
        (setq color (bits-enc-int color 16 red 8))
        (setq color (bits-enc-int color 8 green 8))
        (rgbled-color led-buffer led-index color led-current-brightness)
    })
})


; Update the rainbow LED effect on the front and rear LED strips
(defun rainbow-pattern () {
    (var num-colors 6)
    (looprange led-index 0 led-front-num {
        (var color-index (mod (+ rainbow-index led-index led-status-num) num-colors))
        (var color (bufget-u32 rainbow-colors (* color-index 4)))
        (if (and (= led-index 0) (= led-front-has-laserbeam 1)) {
            (setq color BLACK)
        })
        (rgbled-color led-front-buffer led-index color led-current-brightness)
    })
    (looprange led-index 0 led-rear-num {
        (var color-index (mod (+ rainbow-index led-index led-status-num led-front-num) num-colors))
        (var color (bufget-u32 rainbow-colors (* color-index 4)))
        (if (and (= led-index 0) (= led-rear-has-laserbeam 1)) {
            (setq color BLACK)
        })
        (rgbled-color led-rear-buffer led-index color led-current-brightness)
    })
    (setq rainbow-index (mod (+ rainbow-index 1) num-colors))
})

; Clear all LED strips
(defun clear-leds () {
    (if (>= led-status-pin 0) {
        (looprange led-index 0 led-status-num {
            (rgbled-color led-status-buffer led-index BLACK led-status-brightness)
        })      
    })
    (if (>= led-front-pin 0) {
        (looprange led-index 0 led-front-num {
            (rgbled-color led-front-buffer led-index BLACK led-current-brightness)
        })      
    })
    (if (>= led-rear-pin 0) {
        (looprange led-index 0 led-rear-num {
            (rgbled-color led-rear-buffer led-index BLACK led-current-brightness)
        })      
    })
})

; Update the front and rear LED strips based on the current direction
(defun update-direction-leds (direction) {
    (var rear-color (if (< direction 0) led-forward-color led-backward-color))
    (var front-color (if (< direction 0) led-backward-color led-forward-color))
    (var front-color-laserbeam (if (< direction 0) BLACK WHITE))
    (var rear-color-laserbeam (if (< direction 0) WHITE BLACK))
    (if (= led-front-has-laserbeam 1) {
        (rgbled-color led-front-buffer 0 (if (= led-highbeam-on 1) front-color-laserbeam BLACK) led-current-brightness)
    })
    (looprange led-index led-front-has-laserbeam (- led-front-num led-front-has-laserbeam) {
        (rgbled-color led-front-buffer led-index front-color led-current-brightness)
    })
    (if (= led-rear-has-laserbeam 1) {
        (rgbled-color led-rear-buffer 0 (if (= led-highbeam-on 1) rear-color-laserbeam BLACK) led-current-brightness)
    })
    (looprange led-index led-rear-has-laserbeam (- led-rear-num led-rear-has-laserbeam) {
        (rgbled-color led-rear-buffer led-index rear-color led-current-brightness)
    })
})

; Update the last activity time if the VESC is moving
(defun update-last-activity-time (direction led-mall-grab) {
    (if (or (!= direction 0) (= led-mall-grab 1)) {
        (setq idle-timeout-shutoff-event 0)
        (setq last-activity-time (systime))
    })
})

;Update all leds
(defunret update-leds (last-activity-sec previous-direction direction) {
    (if (< last-activity-sec idle-timeout-shutoff) {
        (if (> led-status-pin 0){
            (if (= led-status-mode 0)
                (if (>= led-status-pin 0) {
                    (update-status-leds)
                })
            )
        })
        (var current-led-mode led-mode)
        (setq led-current-brightness led-brightness)
        (if (> last-activity-sec idle-timeout) {
            (setq current-led-mode led-mode-idle)
            (setq led-current-brightness led-brightness-idle)
        })
        (if (and (>= led-front-pin 0) (>= led-rear-pin 0)) {
            ;battery meter
            (if (or (= current-led-mode 1) (= led-mall-grab 1)){
                (battery-pattern led-front-buffer led-front-num)
                (battery-pattern led-rear-buffer led-rear-num)
                (return)
            })
            ;red/white
            (if (= current-led-mode 0){
                (setq led-forward-color 0xFFFFFFFFu32)
                (setq led-backward-color 0x00FF0000u32)
                (if (or (and (> direction 0) (<= previous-direction 0)) (and (<= direction 0) (> previous-direction 0))) {
                    (update-direction-leds direction)
                })
                (return)
            })               
            ;cyan/magenta
            (if (= current-led-mode 2){
                (setq led-forward-color 0xFF00FFFFu32)
                (setq led-backward-color 0x00FF00FFu32)
                (update-direction-leds direction)
                (return)
            })
            ;blue/green
            (if (= current-led-mode 3){
                (setq led-forward-color 0xFF0000FFu32)
                (setq led-backward-color 0x0000FF00u32)
                (update-direction-leds direction)
                (return)
            })
            ;yellow/green
            (if (= current-led-mode 4){
                (setq led-forward-color 0xFFFFFF00u32)
                (setq led-backward-color 0x0000FF00u32)
                (update-direction-leds direction)
                (return)
            })
            ;rgb led
            (if (= current-led-mode 5){
                (rainbow-pattern)
                (return)
            })
            ;Handle ERROR?
            (if (> current-led-mode 5){
                (clear-leds)
                (return)
            })
            ;TODO
            ;strobe
            (if (= current-led-mode 6){
                ;
                (return)
            })
            ;rave
            (if (= current-led-mode 7) {
                ;
                (return)
            })
            ;mullet
            (if (= current-led-mode 8) {
                ;
                (return)
            })
            ;knight rider
            (if (= current-led-mode 9) {
                ;
                (return)
            })
            ;felony
            (if (= current-led-mode 10) {
                ;
                (return)
            })
        })
    }
    {;else
        (if (< idle-timeout-shutoff-event 2) {
            (clear-leds)
            (setq idle-timeout-shutoff-event (+ idle-timeout-shutoff-event 1))
        })
    })
})

; Split the LED buffer based on pin configuration and update the physical LEDs
(defun led-update () {
    (if (>= led-status-pin 0) {
        (if (= led-status-reversed 1) {
            (var num-pairs (/ (- led-status-num (mod led-status-num 2)) 2))
            (looprange i 0 num-pairs {
                (var index1 (+ (* i 4) 2))
                (var index2 (- (+ (* (- led-status-num 1 i) 4) 2) 4))
                (var temp (bufget-u32 led-status-buffer index1))
                (bufset-u32 led-status-buffer index1 (bufget-u32 led-status-buffer index2))
                (bufset-u32 led-status-buffer index2 temp)
            })
        })
    })
    (if (>= led-front-pin 0) {
        (if (= led-front-reversed 1) {
            (var num-pairs (/ (- led-front-num (mod led-front-num 2)) 2))
            (looprange i 0 num-pairs {
                (var index1 (+ (* i 4) 2))
                (var index2 (- (+ (* (- led-status-num 1 i) 4) 2) 4))
                (var temp (bufget-u32 led-front-buffer index1))
                (bufset-u32 led-front-buffer index1 (bufget-u32 led-front-buffer index2))
                (bufset-u32 led-front-buffer index2 temp)
            })
        })
    })
    (if (>= led-rear-pin 0) {
        (if (= led-rear-reversed 1) {
            (var num-pairs (/ (- led-rear-num (mod led-rear-num 2)) 2))
            (looprange i 0 num-pairs {
                (var index1 (+ (* i 4) 2))
                (var index2 (- (+ (* (- led-status-num 1 i) 4) 2) 4))
                (var temp (bufget-u32 led-rear-buffer index1))
                (bufset-u32 led-rear-buffer index1 (bufget-u32 led-rear-buffer index2))
                (bufset-u32 led-rear-buffer index2 temp)
            })
        })
    })
    ;TODO: Verify all the combined buffer stuff. I think rgbled-buffer starts at index 2 but could be wrong.
    (if (and (= led-status-pin led-front-pin) (= led-front-pin led-rear-pin)) {
        ; All LED strips are chained on the same pin
        (var total-leds (+ led-status-num led-front-num led-rear-num))
        (var led-combined-buffer (rgbled-buffer total-leds led-status-type))
        (bufcpy led-combined-buffer 0 led-status-buffer 2 (* led-status-num 4))
        (bufcpy led-combined-buffer (* led-status-num 4) led-front-buffer 2 (* led-front-num 4))
        (bufcpy led-combined-buffer (* (+ led-status-num led-front-num) 4) led-rear-buffer 2 (* led-rear-num 4))
        (rgbled-init led-status-pin led-status-type)
        (rgbled-update led-combined-buffer)
    } {
        ; LED strips are on separate pins
        (if (>= led-status-pin 0) {
            (rgbled-init led-status-pin led-status-type)
            (rgbled-update led-status-buffer)
        })
        ;LED front/back are on same pin
        (if (= led-front-pin led-rear-pin) {
            (var total-leds (+ led-front-num led-rear-num))
            (var led-combined-buffer (rgbled-buffer total-leds led-front-type))
            (bufcpy led-combined-buffer 0 led-front-buffer 2 (* led-front-num 4))
            (bufcpy led-combined-buffer (* led-front-num 4) led-rear-buffer 2 (* led-rear-num 4))
            (rgbled-init led-front-pin)
            (rgbled-update led-combined-buffer)
        }{
            (if (>= led-front-pin 0) {
                (rgbled-init led-front-pin led-front-type)
                (rgbled-update led-front-buffer)
            })
            (if (>= led-rear-pin 0) {
                (rgbled-init led-rear-pin led-rear-type)
                (rgbled-update led-rear-buffer)
            })
        })
    })
})

; LED Loop
(defun led-loop () {
    (loopwhile-thd 100 t {
        (if (= data-rx-enable 1) {
            (float-cmd can-id (list (assoc float-cmds 'FLOAT_COMMAND_LIGHT_INFO)))
            (float-cmd can-id (list (assoc float-cmds 'FLOAT_COMMAND_GET_ALLDATA) 3))
        } {
            (get-vesc-values)          
        })
        (if (= led-type 3) (setq led-on 1) (setq led-on 0))
        (if (= led-on 1) {
            (setq direction 0)
            (var idle-rpm-darkride idle-rpm)
            (if (= state 4); RUNNING_UPSIDEDOWN 
                (setq idle-rpm-darkride (*idle-rpm-darkride -1))
            )
            (if (> rpm idle-rpm-darkride) {
                (setq direction 1)
            })
            (if (< rpm (* idle-rpm-darkride -1)) {
                (setq direction -1)
            })
            (if (= led-mall-grab-enabled 1){
                (if (> pitch-angle 70) (setq led-mall-grab 1) (setq led-mall-grab 0))
            })
            (if (= debug 1) {
                (setq direction 0)
                (setq idle-timeout 10)
                (setq idle-timeout-shutoff 20)
                (if (< (secs-since debug-timer) idle-timeout ) {
                    (setq direction debug-val)
                    (setq debug-val (* debug-val -1))
                })
            })
            (var last-activity-sec (secs-since last-activity-time))
            (update-leds last-activity-sec previous-direction direction)
            (setq previous-direction direction)
            (update-last-activity-time direction led-mall-grab)
            (if (< idle-timeout-shutoff-event 2) (led-update))
        } {
            (if (> led-on -2) {
                ;clear all LEDs twice if disabled
                (clear-leds)
                (led-update)
                (setq led-on (- led-on 1))
            })
        })
        (sleep led-delay)
    })
})

;Pubremote
(defun proc-esp-now-rx (src des data rssi) {
    ;(print (list "Received" src des data rssi))
    (var jsx (bufget-f32 data 0 'little-endian))
    (var jsy (bufget-f32 data 4 'little-endian))
    (var bt-c (bufget-u8 data 8))
    (var bt-z (bufget-u8 data 9))
    (var is-rev (bufget-u8 data 10))
    (rcode-run 25 0.5 (list 'set-remote-state jsy jsx bt-c bt-z is-rev))
    (esp-now-send '(132 252 230 80 168 12) data)
    ;battery, duty, speed, voltage, footpads, motor and controler temp, trip, remaining miles.
    ;(print "Responded")
})

(defun event-handler ()
    (loopwhile t
        (recv
            ((event-esp-now-rx (? src) (? des) (? data) (? rssi)) (proc-esp-now-rx src des data rssi))
            ((event-data-rx . (? data)) (proc-data data))
            (_ nil)
        )
    )
)

(defun init-remote () {
    (esp-now-start)
    (wifi-set-chan 1)
    (esp-now-add-peer esp-now-remote-mac) ; Add broadcast address as peer
    ;(print (list "starting" (get-mac-addr) (wifi-get-chan)))
    (event-enable 'event-esp-now-rx)
})

(defun get-vesc-config () {
    (setq cells-series (rcode-run can-id 0.5 '(conf-get 'si-battery-cells)))
    (setq vin-low (* cells-series 3.0));static
    (setq vin-high (* cells-series 4.2));static
})

; Retrieve VESC values from CAN bus
(defun get-vesc-values () {
    (setq direction 0)
    (var darkride 0) ;TODO: If darkride is on we'll want to (def idle-rpm (* idlerpm -1))
    (var idle-rpm-darkride idle-rpm)
    (if (= darkride 1)
        (setq idle-rpm-darkride (*idle-rpm-darkride -1))
    )
    (setq rpm (canget-rpm can-id))
    (if (> rpm idle-rpm-darkride) { ;Get this from float package
        (setq direction 1)
    })
    (if (< rpm (* idle-rpm-darkride -1)) { ;Get this from float package
        (setq direction -1)
    })
    (setq input-voltage-filtered (canget-vin can-id)) ;Get this from float package. Also set cells-series here
    (setq duty-cycle-now (canget-duty can-id))
    (var adc1 (canget-adc can-id 1))
    (var adc2 (canget-adc can-id 2))
    (def switch-state 0)
    (if (> adc1 2.5) 
        (setq switch-state 1)
    )
    (if (> adc2 2.5)
        (setq switch-state 2)
    )
    (if (and (> adc1 2.5) (> adc2 2.5))
        (setq switch-state 3)
    )
})

(defun init-can () { ;might have race condition with xlite D:
    ; Enable the CAN event for extended ID (EID) frames
    (if (< can-id 0) {
        (var can-devices (can-scan))
        (loopwhile (<= (length can-devices) 0) {
            (setq can-devices (can-scan))
        })
        (setq can-id (first (can-scan)))
    })
})

(defun proc-data (data) {
        ;(print (map (fn (x) (bufget-u8 data x)) (range (buflen data))))
        
        ; Only process data if data is long enough and magic number is correct
        (if (and (> (buflen data) 1) (= (bufget-u8 data 0) FLOAT_MAGIC)) {
                (match (cossa float-cmds (bufget-u8 data 1))
                    (FLOAT_COMMAND_LIGHT_INFO {
                            (setq led-type (bufget-u8 data 2))
                            (setq led-brightness (/ (bufget-u8 data 3) 100.0))
                            (setq led-brightness-idle (/ (bufget-u8 data 4) 100.0))
                            (setq led-status-brightness (/ (bufget-u8 data 5) 100.0))
                            (setq led-mode (bufget-u8 data 6))
                            (setq led-mode-idle (bufget-u8 data 7))
                            (setq led-status-mode (bufget-u8 data 8))
                            
                            ;(print (list "Light Info Received"
                            ;        (str-from-n led-type "led_type: %d")
                            ;        (str-from-n led-brightness "led_brightness: %d")
                            ;))
                    })
                    (FLOAT_COMMAND_GET_ALLDATA {
                        (var mode (bufget-u8 data 2))
                        (if (= mode 69) {
                            (setq fault-code (bufget-u8 data 3))
                        } { ;else
                            (setq pitch-angle (/ (to-float (bufget-i16 data 5)) 10))
                            ;(print pitch-angle)
                            (setq roll-angle (/ (to-float (bufget-i16 data 7)) 10))
                            (setq state (bufget-u8 data 10))
                            (setq switch-state (bufget-u8 data 11))
                            (setq input-voltage-filtered (/ (to-float (bufget-i16 data 22)) 10))
                            (setq rpm (to-float(bufget-i16 data 24)))
                            (setq speed (/ (to-float (bufget-i16 data 26)) 10))
                            (setq tot-current (/ (to-float (bufget-i16 data 28)) 10))
                            (setq duty-cycle-now (- (/ (bufget-u8 data 30) 100.0) 0.5))
                            (if (>= mode 2) {
                                (setq distance-abs (to-float (bufget-i32 data 34)))
                                (setq fet-temp-filtered (/ (bufget-u8 data 36) 2.0))
                                (setq motor-temp-filtered (/ (bufget-u8 data 38) 2.0))
                            })
                            (if (>= mode 3) {
                                (setq odometer (bufget-u32 data 41)) ;meters
                                (setq battery-level (/ (bufget-u8 data 53) 2.0))
                            })
                        })
                    })
                    (_ nil) ; Ignore other commands
                )
        })
})

(def FLOAT_MAGIC 101) ; Magic number used by float

; Float commands. Here we are using an association list, but there
; are many other ways to do the same thing.
(def float-cmds '(
        (FLOAT_COMMAND_LIGHT_INFO . 25)
        (FLOAT_COMMAND_GET_ALLDATA . 10)
))

(defun float-cmd (can-id cmd) {
        (send-data (append (list FLOAT_MAGIC) cmd) 2 can-id)
})

; Main
(defun main () {
    (if (!= (str-cmp (to-str (sysinfo 'hw-type)) "hw-express") 0) {
        (exit-error "Not running on hw-express")
    })
    ; Spawn the event handler thread and pass the ID it returns to C
    (event-register-handler (spawn event-handler))
    (init-can)
    (if (= code-server-enable 1) {
        (import "pkg@://vesc_packages/lib_code_server/code_server.vescpkg" 'code-server)
        (read-eval-program code-server)
        (get-vesc-config)
    })
    (if (= data-rx-enable 1) {
        (event-enable 'event-data-rx)
    })
    (if (= pubremote-enable 1)
        (init-remote)
    )
    (if (= led-enable 1){
        (init-leds)
        (led-loop)
    })
})

; Start the main
(main)