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
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
|
// Yep, this is actually -*- c++ -*-
//initialize our tools
void init_tools()
{
//do a scan of tools from address 0-255?
//with a 1 millisecond timeout, this takes ~0.256 seconds.
//we may also want to store which tools are available in eeprom?
/*
#ifdef SCAN_TOOLS_ON_STARTUP
for (int i=0; i<256; i++)
{
//are you out there?
if (ping_tool(i))
{
init_tool(i);
}
}
#endif
*/
}
//ask a tool if its there.
bool ping_tool(byte i)
{
slavePacket.init();
slavePacket.add_8(i);
slavePacket.add_8(SLAVE_CMD_VERSION);
slavePacket.add_16(FIRMWARE_VERSION);
return send_packet();
}
//initialize a tool to its default state.
void init_tool(byte i)
{
slavePacket.init();
slavePacket.add_8(i);
slavePacket.add_8(SLAVE_CMD_INIT);
send_packet();
}
//select a tool as our current tool, and let it know.
void select_tool(byte tool)
{
currentToolIndex = tool;
slavePacket.init();
slavePacket.add_8(tool);
slavePacket.add_8(SLAVE_CMD_SELECT_TOOL);
send_packet();
wait_for_tool_ready_state(tool, 0, 0);
}
void check_tool_ready_state()
{
//did we timeout?
if (millis() >= toolTimeoutEnd)
{
commandMode = COMMAND_MODE_IDLE;
return;
}
//is it time for the next ping?
if (millis() >= toolNextPing)
{
//is the tool ready?
if (is_tool_ready(currentToolIndex))
{
commandMode = COMMAND_MODE_IDLE;
return;
}
//ping every x millis
toolNextPing = millis() + 100;
}
}
//ping the tool until it tells us its ready
void wait_for_tool_ready_state(byte tool, int delay_millis, int timeout_seconds)
{
//setup some defaults
if (delay_millis == 0)
delay_millis = 100;
if (timeout_seconds == 0)
timeout_seconds = 60;
//check for our end time.
unsigned long end = millis() + (timeout_seconds * 1000);
//do it until we hear something, or time out.
while (1)
{
//did we time out yet?
if (millis() >= end)
return;
//did we hear back from the tool?
if (is_tool_ready(tool))
return;
//try again...
delay(delay_millis);
}
}
//is our tool ready for action?
bool is_tool_ready(byte tool)
{
slavePacket.init();
slavePacket.add_8(tool);
slavePacket.add_8(SLAVE_CMD_IS_TOOL_READY);
//did we get a response?
if (send_packet())
{
//is it true?
if (slavePacket.get_8(1) == 1)
return true;
}
//bail.
return false;
}
void send_tool_query()
{
//zero out our packet
slavePacket.init();
//load up our packet.
for (byte i=1; i<hostPacket.getLength(); i++)
slavePacket.add_8(hostPacket.get_8(i));
//send it and then get our response
send_packet();
//now load it up into the host. (skip the response code)
//TODO: check the response code
for (byte i=1; i<slavePacket.getLength(); i++)
hostPacket.add_8(slavePacket.get_8(i));
}
void send_tool_command()
{
//zero out our packet
slavePacket.init();
//add in our tool id and command.
slavePacket.add_8(commandBuffer.remove_8());
slavePacket.add_8(commandBuffer.remove_8());
//load up our packet.
byte len = commandBuffer.remove_8();
for (byte i=0; i<len; i++)
slavePacket.add_8(commandBuffer.remove_8());
//send it and then get our response
send_packet();
}
boolean send_packet()
{
//take it easy. no stomping on each other.
delayMicrosecondsInterruptible(50);
digitalWrite(TX_ENABLE_PIN, HIGH); //enable tx
//take it easy. no stomping on each other.
delayMicrosecondsInterruptible(10);
slavePacket.sendPacket();
digitalWrite(TX_ENABLE_PIN, LOW); //disable tx
rs485_packet_count++;
return read_tool_response(PACKET_TIMEOUT);
}
bool read_tool_response(int timeout)
{
//figure out our timeout stuff.
long start = millis();
long end = start + timeout;
//keep reading until we got it.
while (!slavePacket.isFinished())
{
//read through our available data
if (Serial1.available() > 0)
{
//grab a byte and process it.
byte d = Serial1.read();
slavePacket.process_byte(d);
rs485_rx_count++;
#ifdef ENABLE_COMMS_DEBUG
/*
Serial.print("IN:");
Serial.print(d, HEX);
Serial.print("/");
Serial.println(d, BIN);
*/
#endif
//keep processing while there's data.
start = millis();
end = start + timeout;
if (slavePacket.getResponseCode() == RC_CRC_MISMATCH)
{
slave_crc_errors++;
#ifdef ENABLE_COMMS_DEBUG
Serial.println("Slave CRC Mismatch");
#endif
//retransmit?
}
}
//not sure if we need this yet.
//our timeout guy.
if (millis() > end)
{
slave_timeouts++;
#ifdef ENABLE_COMMS_DEBUG
Serial.println("Slave Timeout");
#endif
return false;
}
}
return true;
}
void set_tool_pause_state(bool paused)
{
//TODO: pause/unpause tool.
}
|
