-
Notifications
You must be signed in to change notification settings - Fork 0
/
RCR-to-SmartyCam.lua
166 lines (145 loc) · 6.67 KB
/
RCR-to-SmartyCam.lua
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
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- Message 1056
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
--
-- RPM = (High Byte * 256 + Low byte)
-- Speed = KPH * 10, [01 00] = 0.1kph, [FF 00] = 25.5kph, [00 01] = 25.6kph
-- Gear
-- Engine Temp
--
-- Byte Offsets
--
-- 0 = RPM low byte
-- 1 = RPM high byte
-- 2 = Speed low byte (10ths of KPH)
-- 3 = Speed high byte (10ths of KPH)
-- 4 = Gear low byte
-- 5 = Gear high byte (probably unused)
-- 6 = Engine temp low byte
-- 7 = Engine temp high byte
--
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- Message 1058
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- Brake Pressure in Bar = (High Byte * 256 + Low byte ) / 100
-- TPS = (High Byte * 256 + Low byte )
-- Brake position = (High Byte * 256 + Low byte )
-- Cluth position = (High Byte * 256 + Low byte )
--
-- Bar to PSI = Bar / 14.504
-- PSI to BAR = PSI * 0.0689476
--
-- Byte Offsets
--
-- 0 = brake pressure low byte
-- 1 = brake pressure high byte
-- 2 = TPS low byte (0% =0, 100% = 100)
-- 3 = TPS high byte (unused)
-- 4 = Brake position low byte
-- 5 = Brake position high byte
-- 6 = Clutch position low byte
-- 7 = Clutch position high byte
tick_rate = 200 -- Update frequency in Hz. Moved from 30Hz to 150Hz while testing message dropouts at SmartyCam
channel = 0 -- CAN channel on the RCP. Either 0 or 1, depending on CAN bus chosen.
ext = 0 -- CAN ID is extended (0=11 bit, 1=29 bit)
timeout = 10 -- Milliseconds to attempt to send CAN message for
bitrate = 1000000 -- CAN bitrate (SmartyCam = 1megabit)
gear_tick_update = 3 -- Start at this number and tick down to 0. Upon hitting 0 calculateGear again
gear_tick_count = 3 -- Current tick approaching 0
current_gear = 0 -- The most recent gear that's been detected
mph_to_kph = 1.60934 -- Set this to 1 if speed already in kph, set to 1.60934 if in MPH
-- Init
initCAN(channel, bitrate) -- Initialize CAN
setTickRate(tick_rate) -- Specify the onTick callback frequency
function onTick()
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- Check if we need to sample the gear again
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
gear_tick_count = gear_tick_count - 1
if (gear_tick_count <= 0) then
calculateGear()
current_gear = getChannel("Gear")
gear_tick_count = gear_tick_update
end
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- sample inputs
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
tps_value = getAnalog(0)
brake_pressure_value = getAnalog(2)
rpm_value = getTimerRpm(0)
speed_value = getGpsSpeed()
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- override inputs (for testing) comment these out with a --
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- set engine to 750 RPM
-- rpm_value = 750
-- set speed to doc brown's specifications
-- speed_value = 88
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- perform conversions
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
brake_pressure_value = brake_pressure_value * 0.0689476 -- convert brake pressure to bar
brake_pressure_value = math.floor(brake_pressure_value * 100) -- convert 1 = 1 bar to 1 = 0.01 bar
brake_pressure_high_byte = math.floor(brake_pressure_value / 256)
brake_pressure_low_byte = brake_pressure_value % 256 -- - (brake_pressure_high_byte * 256)
rpm_high_byte = math.floor(rpm_value / 256)
rpm_low_byte = rpm_value % 256
speed_tenths_of_kph = math.floor(speed_value * mph_to_kph * 10)
speed_high_byte = math.floor(speed_tenths_of_kph / 256)
speed_low_byte = speed_tenths_of_kph % 256
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- override conversions (for testing) comment these out with a --
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- speed_low_byte = 255
-- speed_high_byte = 0
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- Compile message data
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
data_1056 = { rpm_low_byte, rpm_high_byte, speed_low_byte, speed_high_byte, current_gear, 0, 0, 0}
data_1058 = { brake_pressure_low_byte, brake_pressure_high_byte, tps_value, 0, 0, 0, 0, 0}
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- Transmit messages
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
response = txCAN(channel, 1056, ext, data_1056, timeout)
if response ~= 1 then
println "ID 1056: Transmission failed"
end
response = txCAN(channel, 1058, ext, data_1058, timeout)
if response ~= 1 then
println "ID 1058: Transmission failed"
end
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-- Enable Logging - Log above 10mph, stop logging below 10mph.
-- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if getGpsSpeed() > 10 then
startLogging()
else
stopLogging()
end
end
function calculateGear() -- tick rate on this can be greatly reduced from function onTick, 10hz is adequate
local gear1 = 3.321
local gear2 = 1.902
local gear3 = 1.308
local gear4 = 1.000
local gear5 = 0.759
local finalDrive = 4.083
local tireDia = 25.03
local gearErr = 0.1
local rpmSpeedRatio = 0
local gearPos = 0
local speed = getGpsSpeed()
local rpm = getTimerRpm(0)
gearId = addChannel("Gear",5,0,0,5)
if speed > 10 then
rpmSpeedRatio = (rpm/speed)/(finalDrive*1056/(tireDia*3.14159))
if ((gear1 - rpmSpeedRatio)^2) < (gearErr^2) then gearPos = 1 end
if ((gear2 - rpmSpeedRatio)^2) < (gearErr^2) then gearPos = 2 end
if ((gear3 - rpmSpeedRatio)^2) < (gearErr^2) then gearPos = 3 end
if ((gear4 - rpmSpeedRatio)^2) < (gearErr^2) then gearPos = 4 end
if ((gear5 - rpmSpeedRatio)^2) < (gearErr^2) then gearPos = 5 end
else
gearPos = 0
end
setChannel(gearId, gearPos)
end