RX_test.lisp

(print "Fast Electric Lifter & Drive")
(print (get-vin))
(print (sysinfo 'uuid))
(uart-start 115200)

; Globals
(define uart-buf (array-create type-byte 50))
(define chan-buf (list 0 0 0 0 0 0 0 0))
(define error-cnt 0)
(define loop-start 0)

; Constants → better way to define them?
(def loop-rate 20000)
(def mot-left 118) ; CAN bus id for left motor VESC
(def mot-right 17) ; CAN bus id for right motor VESC

; Are the other 2x VESCs connected?
(print (can-list-devs)); ideally not `nil`
(print (canget-temp-fet mot-left)) ; should only be "0.000" if no VESC
(print (canget-temp-fet mot-right)) ; should only be "0.000" if no VESC

(loopwhile t
        (progn
	   (setvar 'loop-start (systime))
            (def last-read (uart-read uart-buf 50)) ; RX packet is 16 bytes
            (if (= last-read 16)
                (progn
                    ;(print "valid packet!")
                    (if (< error-cnt 5) (setvar 'error-cnt 0) (setvar 'error-cnt (- error-cnt 5)))
                    
                    (looprange i 1 8 ; skip the first pair as it's RX ID
                        (let (
                            (byte-pair (bufget-u16 uart-buf (* i 2)))
                            (buf-ind (bits-dec-int byte-pair 11 14)) ; 14:11 is channel
                            (buf-val (bits-dec-int byte-pair 0 11))) ; 10:0 is value
                        (progn
                            (setix chan-buf buf-ind buf-val) ; save to chan-buf
                        ))
                    )

                    (print chan-buf) ; before conversions
                    (define out-buf (map (lambda (x) (/ (- x 1023.5) 682.5)) chan-buf)) ; convert to ±1.00
		  (print out-buf) ; after conversions
                        
                  (if (< error-cnt 11)
                    (progn
		      (canset-current-rel mot-left (ix out-buf 1) 0.04)
                        (canset-current-rel mot-right (ix out-buf 2) 0.04)  
                        ; do 'fast electric lifter' things from ch1 `(ix out-buf 0)`
                                               
                        ; -150% = 0
                        ; -100% = 341    or -1.0
                        ;    0% = 1023.5 or  0.0
                        ; +100% = 1706   or +1.0
                        ; +150% = 2047
                        
                        ; so (/ (- (ix out-buf `index`) 1023.5) 682.5)
                        ; maps to -1.0 to +1.0

                        )
                        nil
                  )

                )
                (progn
                    (print "Invalid packet!")
                    (if (< error-cnt 30) 
                        (setvar 'error-cnt (+ error-cnt 1))
                        (progn
                            ; if it happens a few times lock out the lifter & slave VESCs
                            (print "Failing safe!")
                            (setvar 'error-cnt 30)
                            (print "clamp error")
                            (set-current-rel 0)
                            (print "set canbus")
   			 (canset-current-rel mot-left 0)
			  (canset-current-rel mot-right 0))
                    )
                )
            )
            (yield (- loop-rate (* 1000000 (secs-since loop-start))))
))