SDC2130 - Read encoder using CAN

option explicit
' Mecanum drive demonstrator
' Script is written for use in SDC2130 but will work on any other model by changing where commented.
' Script must be loaded and executed in controller that will serve as Master on a RoboCAN network.
' Second dual channel controller will act a Slave. Master node id=1, Slave node id=2
' Script is provided for demonstration purposes, as-is without warranty.
 
dim VD as integer ' Desired Robot Speed
dim ThetaD as integer ' Desired Angle
dim VTheta as integer ' Desired Rotation Speed
dim ThetaD45 as integer ' Desire Angle + 45o
 
' Previous values for change detection
dim PrevVD as integer ' Desired Robot Speed
dim PrevThetaD as integer ' Desired Angle
dim PrevVTheta as integer ' Desired Rotation Speed
 
dim V1 as integer ' Front Left motor
dim V2 as integer ' Front Right motor
dim V3 as integer ' Rear Left motor
dim V4 as integer ' Rear Right motor
 
dim c1 as integer
dim c2 as integer
dim c3 as integer
dim c4 as integer
 
dim cWait as integer
 
dim LR as integer ' Left/Right Command
dim FR as integer ' Forward/Reverse Command
dim CCW as integer ' Rotation command
 
dim RadioVD as integer ' VD from joystick
dim RadioTh as integer ' Theta from joystick
 
dim CANAlive as integer ' Alive Robocan nodes
 
dim c21 as boolean
dim c22 as boolean
dim c2send as boolean
 
ScanCANValue(2,_C,1,21,0)
ScanCANValue(2,_C,2,21,1)
 
c21 = false
c22 = false
c2send = false
cWait = 0
 
Top:
    ' Use code below to accept commands via RS232 or USB
    ' Send commands with !VAR nn value
    ' VAR 1 contains motion speed, +/-1000 range
    ' VAR 2 contains motion direction, 0-360 degree range
    ' VAR 3 contains rotation speed, +/-1000 range
    VD = getvalue(_VAR, 1)
    ThetaD = getvalue(_VAR, 2)
    VTheta = getvalue(_VAR, 3)
     
    ' Read list of alive RoboCAN nodes
    CANAlive = getvalue(_CL, 1)
     
    ' Check if Slave is present
    if(CANAlive <> 273)
             V1 = 0
             V2 = 0
             V3 = 0
             V4 = 0
             goto ApplyCommand ' Stop all motors if no radio or no slave
    end if
     
    ' To avoid unnecessary computation, evaluate formulas only if change occurred
    if (VD <> PrevVD or ThetaD <> PrevThetaD or VTheta <> PrevVTheta)
     
             ThetaD45 = ThetaD + 45 ' compute once angle + 45 for use in the 4 equations
           
             V1 = (VD * sin(ThetaD45))/1000 + VTheta ' sin takes degrees and returns result * 1000
             V2 = (VD * cos(ThetaD45))/1000 - VTheta
             V3 = (VD * cos(ThetaD45))/1000 + VTheta
             V4 = (VD * sin(ThetaD45))/1000 - VTheta
           
             ' Uncomment below to view computed speeds in console
             'print (V1,"\t",V2,"\t",V3,"\t",V4,"\r")
    end if
     
    ' Save for detecting change at next loop execution
    PrevVD = VD
    PrevThetaD = ThetaD
    PrevVTheta = VTheta
 
    if(IsScannedCANReady(0))
       c3 = ReadScannedCANValue(0)
       c21 = true
    end if
   
    if(IsScannedCANReady(1))
       c4 = ReadScannedCANValue(1)
       c22 = true
    end if
   
    if(c21 = true and c22 = true)
        c1 = getValue(_C,1)
        c2 = getValue(_C,2)
        print("&f:")
        print(c1,":")
        print(c2,":")
        print(c3,":")
        print(c4,"\r")
    end if
 
ApplyCommand:
    ' Apply to local motors
    SetCommand(_G, 1, V1)
    SetCommand(_G, 2, V2)
    ' Send command to Slave, node 2 on RoboCAN network
    SetCANCommand(2, _G, 1, V3)
    SetCANCommand(2, _G, 2, V4)
   
wait(20) ' Repeat loop every 10ms / 100Hz
 
goto top