1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
|
local device = _G
local WRITE_INFO = 0x01
local READ_INFO = 0x02
local GET_RAW_POS = 0x03
local char000 = string.char(0,0,0)
local mode='wheel'
--byte id,byte regstart, int value
api={}
api.write_info = {}
api.write_info.parameters = {[1]={rname="id", rtype="number", min=0, max=255},[2]={rname="regstart", rtype="number", min=0, max=255},[3]={rname="value", rtype="number", min=0, max=65536}} ----byte id,byte regstart, int value
api.write_info.returns = {[1]={rname="write_info_return", rtype="number"}} --one return
api.write_info.call = function (id, regstart, value)
id, regstart, value = tonumber(id), tonumber(regstart), tonumber(value)
local write_info_payload = string.char(WRITE_INFO, id, regstart, math.floor(value / 256),value % 256)
device:send(write_info_payload)
local write_info_response = device:read(2) or char000
local raw_val = (string.byte(write_info_response, 2) or 0)
return raw_val
end
--- Set wheel mode.
--Set the motor to continuous rotation mode.
api.wheel_mode = {}
api.wheel_mode.parameters = {[1]={rname="id", rtype="number", min=0, max=255}}
api.wheel_mode.returns = {}
api.wheel_mode.call = function (id )
id = tonumber(id)
local ret = device:send(string.char(WRITE_INFO,id,0x06,0x00,0x00))
local write_info_response = device:read(1) or string.char(0,0)
local ret = device:send(string.char(WRITE_INFO,id,0x08,0x00,0x00))
local write_info_response = device:read(1) or string.char(0,0)
mode='wheel'
end
--- Set joint mode.
-- Set the motor to joint mode. Angles are provided in degrees,
-- in the full servo coverage (0 - 300 degrees arc)
-- @param min the minimum joint angle (defaults to 0)
-- @param max the maximum joint angle (defaults to 300)
api.joint_mode = {}
api.joint_mode.parameters = {[1]={rname="id", rtype="number", min=0, max=255},[2]={rname="minimo", rtype="number", min=0, max=1023},[3]={rname="maximo", rtype="number", min=0, max=1023}}
api.joint_mode.returns = {}
api.joint_mode.call = function (id ,minimo, maximo)
id = tonumber(id)
minimo=tonumber(minimo)
maximo=tonumber(maximo)
local ret = device:send(string.char(WRITE_INFO,id,0x06,math.floor(minimo / 256),minimo % 256))
local write_info_response = device:read(1) or string.char(0,0)
local ret = device:send(string.char(WRITE_INFO,id,0x08,math.floor(maximo / 256),maximo % 256))
local write_info_response = device:read(1) or string.char(0,0)
mode='joint'
end
--- Set motor position.
-- Set the target position for the motor's axle. Only works in
-- joint mode.
-- @param value Angle in degrees, in the 0 .. 300deg range.
api.set_position = {}
api.set_position.parameters = {[1]={rname="id", rtype="number", min=0, max=255},[2]={rname="pos", rtype="number", min=0, max=1023}}
api.set_position.returns = {}
api.set_position.call = function (id, pos )
id = tonumber(id)
pos = tonumber(pos)
local ret = device:send(string.char(WRITE_INFO,id,0x1E,math.floor(pos / 256),pos % 256))
local write_info_response = device:read(1) or string.char(0,0)
end
--- Get motor position.
-- Read the axle position from the motor.
-- @return The angle in deg. The reading is only valid in the
-- 0 .. 300deg range
api.get_position = {}
api.get_position.parameters = {[1]={rname="id", rtype="number", min=0, max=255},[2]={rname="pos", rtype="number", min=0, max=1023}}
api.get_position.returns = {[1]={rname="motor_position", rtype="number"}} --one return
api.get_position.call = function(id)
local send_response = device:send(string.char(GET_RAW_POS,id))
local value = device:read(3) or string.char(0,0,0)
local h_value = string.byte(value, 2)
local l_value = string.byte(value, 3)
local raw_angle = 256 * h_value + l_value
local ang=0.29*(raw_angle) -- deg ?
return raw_angle
end
--- Set motor speed.
-- @param value If motor in joint mode, speed in deg/sec in the 1 .. 684 range
-- (0 means max available speed).
-- If in wheel mode, as a fraction of max torque (in the -1 .. 1 range).
api.set_speed = {}
api.set_speed.parameters = {[1]={rname="id", rtype="number", min=0, max=255},[2]={rname="speed", rtype="number", min=-1, max=684}}
api.set_speed.returns = {} --no return
api.set_speed.call = function(id, speed)
--if mode=='joint' then
-- 0 .. 684 deg/sec
local vel=math.floor(speed * 1.496)
local lowb = math.floor(vel/ 256)
local highb = vel % 256
--local lowb, highb = get2bytes_unsigned(vel)
local ret = device:send(id,0x20,string.char(lowb,highb))
print ("ret= ",ret, " lowb= ", lowb, " highb= ", highb)
if ret then return ret:byte() end
--else --mode=='wheel'
-- -1 .. +1 max torque
--local vel=math.floor(speed * 1023)
--local lowb, highb = get2bytes_signed(vel)
--local ret = device:send(id,0x20,string.char(lowb,highb))
--if ret then return ret:byte() end
--end
end
|
