Programs/Code

Code for Moving Actuators in a Circle

This code is designed for running on Riptide's computers in Python to control the two linear actuators with IDs 3 and 4.

################################################################################

################################################################################

import can

import time

import os

import math

################################################################################

#################################**FUNCTIONS**##################################

################################################################################

def formatRcvMsg( message ):

    #Write position/current/temperature telemetry to terminal

    print( "Position Feedback: ", message.data[3] + (message.data[4] << 8) )

    print( "Current Feedback: ", message.data[5] + (message.data[6] << 8) )

    print( "Temperature Feedback: ", message.data[7] - 50.0, " deg_C" )

################################################################################

####################################**MAIN**####################################

################################################################################

#Configure the USB2CAN device

os.system("ip link set can2 up type can bitrate 250000 sample-point 0.875")

time.sleep(0.1)

bus = can.interface.Bus( bustype = 'pcan', channel = 'PCAN_USBBUS1', bitrate = 1000000 )

print('CAN port 1 configured.')

#set the Servo Cylinder's unitID

actuatorIDs = [0x3, 0x4]

pos0 = 65535

d_t = 0

period = 1.0

while 1:

    d_t += 1

    #Received Telemetry from the Actuator

    rcvMsg = bus.recv( timeout = 0.005 )

    if rcvMsg is not None: #if there's data available

        formatRcvMsg( rcvMsg )

    MT = 65535 #enter your current limiting function here

    currLowByte = MT & 0x00FF

    currHighByte = (MT & 0xFF00) >> 8

    #Generating the actuator commands to send over CAN

    #pos = 65535 #enter your position generating function here (ranges from pMin to pMax, mapped to spMin/spMax)

    pos = int(pos0*(0.5*math.sin(d_t/(2*math.pi*(period/0.1)))+0.5))

    posLowByte = pos & 0x00FF

    posHighByte = (pos & 0xFF00) >> 8

    #configure message to send to actuator (assumes <>() RX setting)

    msg1 = can.Message( arbitration_id = actuatorIDs[0], data = [posLowByte, posHighByte, currLowByte, currHighByte], is_extended_id = True )

    #pos = 65535 #enter your position generating function here (ranges from pMin to pMax, mapped to spMin/spMax)

    pos = int(pos0*(0.5*math.cos(d_t/(2*math.pi*(period/0.1)))+0.5))

    posLowByte = pos & 0x00FF

    posHighByte = (pos & 0xFF00) >> 8

    msg2 = can.Message( arbitration_id = actuatorIDs[1], data = [posLowByte, posHighByte, currLowByte, currHighByte], is_extended_id = True )

    #send the CAN message

    try:

        bus.send(msg1)

        bus.send(msg2)

    except Exception as e:

        print("CAN Transmit Error: ", e )

    time.sleep(0.001)

################################################################################

####################################**END**#####################################

################################################################################