ardupilot/libraries/AP_Scripting/examples/CAN_MiniCheetah_drive.lua

227 lines
5.0 KiB
Lua

-- Control MiniCheetah motor driver over CAN
-- https://os.mbed.com/users/benkatz/code/HKC_MiniCheetah/docs/tip/CAN__com_8cpp_source.html
-- Load CAN driver with a buffer size of 20
local driver = CAN.get_device(20)
local target_ID = uint32_t(1)
local pos_max = 12.5
local vel_max = 65
local Kp_min = 0
local Kp_max = 500
local Kd_min = 0
local kd_max = 5
local torque_max = 18
local position_des = 0
local position_inc = 0.01
-- convert decimal to int within given range and width
function to_uint(val, min, max, bits)
local range = max - min
local int_range = 0
for i = 0, bits - 1 do
int_range = int_range | (1 << i)
end
return math.floor((((val - min)/range) * int_range) + 0.5)
end
-- convert int to decimal within given range and width
function from_uint(val, min, max, bits)
local range = max - min
local int_range = 0
for i = 0, bits - 1 do
int_range = int_range | (1 << i)
end
return ((val / int_range) * range) + min
end
-- send a motor command
function send(position, velocity, Kp, Kd, torque)
-- 16 bit position command, between -4*pi and 4*pi
-- 12 bit velocity command, between -30 and + 30 rad/s
-- 12 bit kp, between 0 and 500 N-m/rad
-- 12 bit kd, between 0 and 100 N-m*s/rad
-- 12 bit feed forward torque, between -18 and 18 N-m
-- range check
assert(math.abs(position) <= pos_max, "position out of range")
assert(math.abs(velocity) <= vel_max, "velocity out of range")
assert((Kp >= Kp_min) and (Kp <= Kp_max), "Kp out of range")
assert((Kd >= Kd_min) and (Kd <= kd_max), "Kd out of range")
assert(math.abs(torque) <= torque_max, "torque out of range")
-- convert from decimal to integer
position = to_uint(position, -pos_max, pos_max, 16)
velocity = to_uint(velocity, -vel_max, vel_max, 12)
Kp = to_uint(Kp, Kp_min, Kp_max, 12)
Kd = to_uint(Kd, Kd_min, kd_max, 12)
torque = to_uint(torque, -torque_max, torque_max, 12)
msg = CANFrame()
msg:id(target_ID)
-- 0: [position[15-8]]
msg:data(0, position >> 8)
-- 1: [position[7-0]]
msg:data(1, position & 0xFF)
-- 2: [velocity[11-4]]
msg:data(2, velocity >> 4)
-- 3: [velocity[3-0], kp[11-8]]
msg:data(3, ((velocity << 4) | (Kp >> 8)) & 0xFF)
-- 4: [kp[7-0]]
msg:data(4, Kp & 0xFF)
-- 5: [kd[11-4]]
msg:data(5, Kd >> 4)
-- 6: [kd[3-0], torque[11-8]]
msg:data(6, ((Kd << 4) | (torque >> 8)) & 0xFF)
-- 7: [torque[7-0]]
msg:data(7, torque & 0xFF)
-- sending 8 bytes of data
msg:dlc(8)
-- write the frame with a 10000us timeout
driver:write_frame(msg, 10000)
end
-- send command to enable motor
function enable()
msg = CANFrame()
msg:id(target_ID)
msg:data(0, 0xFF)
msg:data(1, 0xFF)
msg:data(2, 0xFF)
msg:data(3, 0xFF)
msg:data(4, 0xFF)
msg:data(5, 0xFF)
msg:data(6, 0xFF)
msg:data(7, 0xFC)
msg:dlc(8)
driver:write_frame(msg, 10000)
end
-- send command to disable motor
function disable()
msg = CANFrame()
msg:id(target_ID)
msg:data(0, 0xFF)
msg:data(1, 0xFF)
msg:data(2, 0xFF)
msg:data(3, 0xFF)
msg:data(4, 0xFF)
msg:data(5, 0xFF)
msg:data(6, 0xFF)
msg:data(7, 0xFD)
msg:dlc(8)
driver:write_frame(msg, 10000)
end
-- send command to zero motor
function zero()
msg = CANFrame()
msg:id(target_ID)
msg:data(0, 0xFF)
msg:data(1, 0xFF)
msg:data(2, 0xFF)
msg:data(3, 0xFF)
msg:data(4, 0xFF)
msg:data(5, 0xFF)
msg:data(6, 0xFF)
msg:data(7, 0xFE)
msg:dlc(8)
driver:write_frame(msg, 10000)
end
-- receive data from motor
function receive()
-- Read a message from the buffer
frame = driver:read_frame()
-- noting waiting, return early
if not frame then
return
end
-- 8 bit ID
-- 16 bit position, between -4*pi and 4*pi
-- 12 bit velocity, between -30 and + 30 rad/s
-- 12 bit current, between -40 and 40;
-- 0: [ID[7-0]]
-- 1: [position[15-8]]
-- 2: [position[7-0]]
-- 3: [velocity[11-4]]
-- 4: [velocity[3-0], current[11-8]]
-- 5: [current[7-0]]
local ID = frame:data(0)
local position = (frame:data(1) << 8) | frame:data(2)
local velocity = (frame:data(3) << 4) | (frame:data(4) >> 4)
local current = ( (frame:data(4) << 8) | frame:data(5)) & 0xFFF
-- from integer to decimal
position = from_uint(position, -pos_max, pos_max, 16)
velocity = from_uint(velocity, -vel_max, vel_max, 12)
current = from_uint(current, -torque_max, torque_max, 12)
return ID, position, velocity, current
end
function update()
send(position_des, 0, 100, 1, 0)
local ID, position, velocity, current = receive()
if ID then
gcs:send_named_float('POS',position)
gcs:send_named_float('VEL',velocity)
gcs:send_named_float('CUR',current)
end
position_des = position_des + position_inc
if position_des > pos_max then
position_inc = -math.abs(position_inc)
position_des = pos_max
end
if position_des < -pos_max then
position_inc = math.abs(position_inc)
position_des = -pos_max
end
return update, 10
end
function init()
enable()
return update, 100
end
return init, 1000