RSeries astromech firmware
RoboteQController.h
Go to the documentation of this file.
1 // Copyright (C) 2013 Andy Kipp <kipp.andrew@gmail.com>
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a
4 // copy of this software and associated documentation files (the
5 // "Software"), to deal in the Software without restriction, including
6 // without limitation the rights to use, copy, modify, merge, publish,
7 // distribute, sublicense, and/or sell copies of the Software, and to
8 // permit persons to whom the Software is furnished to do so, subject to
9 // the following conditions:
10 //
11 // The above copyright notice and this permission notice shall be included
12 // in all copies or substantial portions of the Software.
13 //
14 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
15 // OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
16 // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
17 // IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
18 // CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
19 // TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
20 // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
21 
22 #ifndef ROBOTEQCONTROLLER_H
23 #define ROBOTEQCONTROLLER_H
24 
25 #include <Stream.h>
26 
35 {
36 public:
37  enum
38  {
45 
54 
62  };
63 
67  RoboteQController(Stream *serial) :
68  fSerial(serial),
69  fTimeout()
70  {
71  }
72 
78  int isConnected(void)
79  {
80  if (fSerial == NULL)
81  return kROBOTEQ_ERROR;
82 
83  uint8_t inByte = 0;
84  uint32_t startTime = millis();
85 
86  // send QRY
87  fSerial->write(kROBOTEQ_QUERY_CHAR);
88  fSerial->flush();
89 
90  while (millis() - startTime < (uint32_t)fTimeout)
91  {
92  // wait for ACK
93  if (fSerial->available() > 0)
94  {
95  inByte = m_Serial->read();
96  if (inByte == kROBOTEQ_ACK_CHAR)
97  return kROBOTEQ_OK;
98  }
99  }
100  return kROBOTEQ_TIMEOUT;
101  }
102 
110  int commandMotorPower(uint8_t ch, int16_t p)
111  {
112  char command[kROBOTEQ_COMMAND_BUFFER_SIZE];
113  sprintf(command, "!G %02d %d\r", ch, p);
114  return sendCommand(command);
115  }
116 
124  {
125  char command[kROBOTEQ_COMMAND_BUFFER_SIZE];
126  sprintf(command, "!EX\r");
127  return sendCommand(command);
128  }
129 
135  int queryFirmware(char *buf, size_t bufSize)
136  {
137  // Query: ?FID
138  // Response: FID=<firmware>
139  memset(buf, NULL, bufSize);
140  return sendQuery("?FID\r", (uint8_t*)buf, 100);
141  }
142 
149  {
150  // Query: ?BA
151  // Response: BA=<ch1*10>:<ch2*10>
152  int ch1, ch2;
153  char buffer[kROBOTEQ_BUFFER_SIZE];
154  int res;
155 
156  // Send Query
157  if ((res = sendQuery("?BA\r", (uint8_t*)buffer, kROBOTEQ_BUFFER_SIZE)) < 0)
158  return res;
159  if (res < 4)
160  return kROBOTEQ_BAD_RESPONSE;
161 
162  // Parse Response
163  if (sscanf((char*)buffer, "BA=%i:%i", &ch1, &ch2) < 2) {
164  return kROBOTEQ_BAD_RESPONSE;
165  }
166 
167  // Return total amps (ch1 + ch2)
168  return ch1+ch2;
169  }
170 
177  int queryBatteryAmps(uint8_t ch);
178  {
179  // Query: ?BA [ch]
180  // Response: BA=<ch*10>
181  int amps;
182  char command[kROBOTEQ_COMMAND_BUFFER_SIZE];
183  char buffer[kROBOTEQ_BUFFER_SIZE];
184  int res;
185 
186  // Build Query Command
187  sprintf(command, "?BA %i\r", ch);
188 
189  // Send Query
190  if ((res = this->sendQuery(command, (uint8_t*)buffer, kROBOTEQ_BUFFER_SIZE)) < 0)
191  return res;
192  if (res < 4)
193  return kROBOTEQ_BAD_RESPONSE;
194 
195  // Parse Response
196  if (sscanf((char*)buffer, "BA=%i", &amps) < 1) {
197  return kROBOTEQ_BAD_RESPONSE;
198  }
199 
200  return amps;
201  }
202 
209  {
210  // Query: ?V 2 (2 = main battery voltage)
211  // Response: V=<voltage>*10
212  uint8_t buffer[kROBOTEQ_BUFFER_SIZE];
213  int voltage = -1;
214  memset(buffer, NULL, kROBOTEQ_BUFFER_SIZE);
215  int res;
216  if ((res = this->sendQuery("?V 2\r", (uint8_t*)buffer, kROBOTEQ_BUFFER_SIZE)) < 0)
217  return res;
218  if (res < 4)
219  return kROBOTEQ_BAD_RESPONSE;
220  // Parse Response
221  if (sscanf((char*)buffer, "V=%i", &voltage) != 1)
222  return kROBOTEQ_BAD_RESPONSE;
223  return voltage;
224  }
225 
232  {
233  // Query: ?V 1 (1 = main motor voltage)
234  // Response: V=<voltage>*10
235  uint8_t buffer[kROBOTEQ_BUFFER_SIZE];
236  int voltage = -1;
237  memset(buffer, NULL, kROBOTEQ_BUFFER_SIZE);
238  int res;
239  if ((res = this->sendQuery("?V 1\r", (uint8_t*)buffer, kROBOTEQ_BUFFER_SIZE)) < 0)
240  return res;
241  if (res < 4)
242  return kROBOTEQ_BAD_RESPONSE;
243  // Parse Response
244  if (sscanf((char*)buffer, "V=%i", &voltage) != 1)
245  return kROBOTEQ_BAD_RESPONSE;
246  return voltage;
247  }
248 
255  int queryMotorPower(uint8_t ch)
256  {
257  // Query: ?M [ch]
258  // Response: M=<motor power>
259  int p;
260  char command[kROBOTEQ_COMMAND_BUFFER_SIZE];
261  char buffer[kROBOTEQ_BUFFER_SIZE];
262  int res;
263 
264  // Build Query Command
265  sprintf(command, "?M %i\r", ch);
266 
267  // Send Query
268  if ((res = this->sendQuery("?BA\r", (uint8_t*)buffer, kROBOTEQ_BUFFER_SIZE)) < 0)
269  return res;
270  if (res < 4)
271  return kROBOTEQ_BAD_RESPONSE;
272 
273  // Parse Response
274  if (sscanf((char*)buffer, "M=%i", &p) < 1) {
275  return kROBOTEQ_BAD_RESPONSE;
276  }
277  return p;
278 
279  }
280 
284  int queryFaultFlag(void)
285  {
286  // Query: ?FF
287  // Response: FF=<status>
288  int fault = -1;
289  uint8_t buffer[kROBOTEQ_BUFFER_SIZE];
290  memset(buffer, NULL, kROBOTEQ_BUFFER_SIZE);
291  int res = 0;
292  if ((res = this->sendQuery("?FF\r", (uint8_t*)buffer, kROBOTEQ_BUFFER_SIZE)) < 0)
293  return res;
294  if (res < 4)
295  return kROBOTEQ_BAD_RESPONSE;
296  // Parse Response
297  if (sscanf((char*)buffer, "FF=%i", &fault) < 1)
298  return kROBOTEQ_BAD_RESPONSE;
299  return fault;
300  }
301 
305  int queryStatusFlag(void)
306  {
307  // Query: ?FS
308  // Response: FS=<status>
309  uint8_t buffer[kROBOTEQ_BUFFER_SIZE];
310  int status = -1;
311  memset(buffer, NULL, kROBOTEQ_BUFFER_SIZE);
312  int res;
313  if ((res = this->sendQuery("?FS\r", (uint8_t*)buffer, kROBOTEQ_BUFFER_SIZE)) < 0)
314  return res;
315  if (res < 4)
316  return kROBOTEQ_BAD_RESPONSE;
317  // Parse Response
318  if (sscanf((char*)buffer, "FS=%i", &status) < 1)
319  return kROBOTEQ_BAD_RESPONSE;
320  return status;
321  }
322 
329  int queryEncoderSpeed(uint8_t ch)
330  {
331  // Query: ?S [ch]
332  // Response: S=[speed]
333  int speed;
334  char command[kROBOTEQ_COMMAND_BUFFER_SIZE];
335  char buffer[kROBOTEQ_BUFFER_SIZE];
336  int res;
337 
338  // Build Query Command
339  sprintf(command, "?S %i\r", ch);
340 
341  // Send Query
342  if ((res = this->sendQuery(command, (uint8_t*)buffer, kROBOTEQ_BUFFER_SIZE)) < 0)
343  return res;
344  if (res < 3)
345  return kROBOTEQ_BAD_RESPONSE;
346 
347  // Parse Response
348  if (sscanf((char*)buffer, "S=%i", &speed) < 1) {
349  return kROBOTEQ_BAD_RESPONSE;
350  }
351  return speed;
352  }
353 
362  int setEncoderPulsePerRotation(uint8_t ch, uint16_t ppr)
363  {
364  char command[kROBOTEQ_COMMAND_BUFFER_SIZE];
365  sprintf(command, "^EPPR %02d %d\r", ch, ppr);
366  return sendCommand(command);
367  }
368 
377  int setMotorAmpLimit(uint8_t ch, uint16_t a)
378  {
379  char command[kROBOTEQ_COMMAND_BUFFER_SIZE];
380  sprintf(command, "^ALIM %i %i", ch, a);
381  return sendCommand(command);
382  }
383 
390  {
391  char command[kROBOTEQ_COMMAND_BUFFER_SIZE];
392  sprintf(command, "%%EELD\r");
393  return sendCommand(command);
394  }
395 
402  {
403  char command[kROBOTEQ_COMMAND_BUFFER_SIZE];
404  sprintf(command, "%%EESAV\r");
405  return this->sendCommand(command);
406  }
407 
411  inline void setTimeout(uint16_t timeout)
412  {
413  fTimeout = timeout;
414  }
415 
416 private:
417  enum
418  {
419  kROBOTEQ_DEFAULT_TIMEOUT = 1000,
420  kROBOTEQ_BUFFER_SIZE = 64,
421  kROBOTEQ_COMMAND_BUFFER_SIZE = 20,
422 
423  kROBOTEQ_QUERY_CHAR = 0x05,
424  kROBOTEQ_ACK_CHAR = 0x06
425  };
426 
427  inline int sendQuery(const char *query, uint8_t *buf, size_t bufSize)
428  {
429  return sendQuery(query, strlen(query), buf, bufSize);
430  }
431 
432  int sendQuery(const char *query, size_t querySize, uint8_t *buf, size_t bufSize)
433  {
434  if (fSerial == NULL)
435  return kROBOTEQ_ERROR;
436 
437  // Write Query to Serial
438  fSerial->write((uint8_t*)query, querySize);
439  fSerial->flush();
440 
441  // Read Serial until timeout or newline
442  int res = readResponse(buf, bufSize);
443  return res;
444  }
445 
446  inline int sendCommand(const char *command)
447  {
448  return sendCommand(command, strlen(command));
449  }
450 
451  int sendCommand(const char *command, size_t commandSize)
452  {
453  if (fSerial == NULL)
454  return kROBOTEQ_ERROR;
455 
456  // Write Command to Serial
457  fSerial->write((uint8_t*)command, commandSize);
458  fSerial->flush();
459 
460  uint8_t buffer[kROBOTEQ_BUFFER_SIZE];
461  int res = 0;
462 
463  // Read Serial until timeout or newline
464  if ((res = this->readResponse((uint8_t*)buffer, kROBOTEQ_BUFFER_SIZE)) < 0)
465  return res;
466 
467  if (res < 1)
468  return kROBOTEQ_BAD_RESPONSE;
469 
470  // Check Command Status
471  if (strncmp((char*)buffer, "+", 1) == 0)
472  return kROBOTEQ_OK;
473  return kROBOTEQ_BAD_COMMAND;
474  }
475 
476  int readResponse(uint8_t *buf, size_t bufSize)
477  {
478  uint8_t inByte;
479  size_t index = 0;
480  uint32_t startTime = millis();
481  while (millis() - startTime < fTimeout)
482  {
483  if (m_Serial->available() > 0)
484  {
485  inByte = m_Serial->read();
486  buf[index++] = inByte;
487  if (index > bufferSize)
488  {
489  // out of buffer space
490  return kROBOTEQ_BUFFER_OVER;
491  }
492  if (inByte == 0x0D)
493  return index;
494  }
495  }
496  return kROBOTEQ_TIMEOUT;
497  }
498 
499  Stream* fSerial;
500  uint16_t fTimeout;
501 
502  static char* chomp(char* s)
503  {
504  int end = strlen(s) - 1;
505  if (end >= 0 && (s[end] == '\n' || s[end] == '\r'))
506  s[end] = '\0';
507  return s;
508  }
509 };
510 
511 #endif
RoboteQController::kROBOTEQ_SCRIPT_RUN
@ kROBOTEQ_SCRIPT_RUN
Definition: RoboteQController.h:61
RoboteQController::RoboteQController
RoboteQController(Stream *serial)
Constructor.
Definition: RoboteQController.h:67
RoboteQController::kROBOTEQ_FAULT_OVERVOLTAGE
@ kROBOTEQ_FAULT_OVERVOLTAGE
Definition: RoboteQController.h:47
RoboteQController::kROBOTEQ_FAULT_MOSFET
@ kROBOTEQ_FAULT_MOSFET
Definition: RoboteQController.h:52
RoboteQController::buffer
char buffer[kROBOTEQ_BUFFER_SIZE]
Definition: RoboteQController.h:183
RoboteQController::queryEncoderSpeed
int queryEncoderSpeed(uint8_t ch)
Query encoder speed in RPM.
Definition: RoboteQController.h:329
RoboteQController::commandEmergencyStop
int commandEmergencyStop(void)
Send emergency stop command (!EX) note: you have to reset the controller after this sending command.
Definition: RoboteQController.h:123
RoboteQController::queryStatusFlag
int queryStatusFlag(void)
Query status flags.
Definition: RoboteQController.h:305
RoboteQController
Communicate with a Roboteq controller.
Definition: RoboteQController.h:34
RoboteQController::saveConfiguration
int saveConfiguration(void)
Save controller configuration.
Definition: RoboteQController.h:401
RoboteQController::queryBatteryAmps
int queryBatteryAmps(void)
Query battery amps.
Definition: RoboteQController.h:148
RoboteQController::kROBOTEQ_FAULT_SHORT
@ kROBOTEQ_FAULT_SHORT
Definition: RoboteQController.h:49
RoboteQController::kROBOTEQ_STATUS_ANALOG
@ kROBOTEQ_STATUS_ANALOG
Definition: RoboteQController.h:57
RoboteQController::queryMotorVoltage
int queryMotorVoltage(void)
Query motor voltage.
Definition: RoboteQController.h:231
RoboteQController::kROBOTEQ_FAULT_OVERHEAT
@ kROBOTEQ_FAULT_OVERHEAT
Definition: RoboteQController.h:46
RoboteQController::kROBOTEQ_BAD_COMMAND
@ kROBOTEQ_BAD_COMMAND
Definition: RoboteQController.h:42
RoboteQController::setTimeout
void setTimeout(uint16_t timeout)
Set timeout.
Definition: RoboteQController.h:411
RoboteQController::kROBOTEQ_OK
@ kROBOTEQ_OK
Definition: RoboteQController.h:39
RoboteQController::res
int res
Definition: RoboteQController.h:184
RoboteQController::queryBatteryVoltage
int queryBatteryVoltage(void)
Query battery voltage.
Definition: RoboteQController.h:208
RoboteQController::loadConfiguration
int loadConfiguration(void)
Load controller configuration.
Definition: RoboteQController.h:389
RoboteQController::kROBOTEQ_ERROR
@ kROBOTEQ_ERROR
Definition: RoboteQController.h:41
RoboteQController::setEncoderPulsePerRotation
int setEncoderPulsePerRotation(uint8_t ch, uint16_t ppr)
Set encoder pulse per rotation.
Definition: RoboteQController.h:362
RoboteQController::kROBOTEQ_FAULT_SCRIPT
@ kROBOTEQ_FAULT_SCRIPT
Definition: RoboteQController.h:51
RoboteQController::amps
return amps
Definition: RoboteQController.h:200
RoboteQController::isConnected
int isConnected(void)
Check if controller is connected.
Definition: RoboteQController.h:78
RoboteQController::kROBOTEQ_TIMEOUT
@ kROBOTEQ_TIMEOUT
Definition: RoboteQController.h:40
RoboteQController::kROBOTEQ_FAULT_CONFIG
@ kROBOTEQ_FAULT_CONFIG
Definition: RoboteQController.h:53
RoboteQController::kROBOTEQ_BAD_RESPONSE
@ kROBOTEQ_BAD_RESPONSE
Definition: RoboteQController.h:43
RoboteQController::kROBOTEQ_STATUS_SERIAL
@ kROBOTEQ_STATUS_SERIAL
Definition: RoboteQController.h:55
RoboteQController::kROBOTEQ_STATUS_STALL
@ kROBOTEQ_STATUS_STALL
Definition: RoboteQController.h:59
RoboteQController::commandMotorPower
int commandMotorPower(uint8_t ch, int16_t p)
Send motor power command (!G)
Definition: RoboteQController.h:110
RoboteQController::kROBOTEQ_FAULT_ESTOP
@ kROBOTEQ_FAULT_ESTOP
Definition: RoboteQController.h:50
RoboteQController::kROBOTEQ_FAULT_UNDERVOLTAGE
@ kROBOTEQ_FAULT_UNDERVOLTAGE
Definition: RoboteQController.h:48
RoboteQController::queryFirmware
int queryFirmware(char *buf, size_t bufSize)
Query controller firmware.
Definition: RoboteQController.h:135
RoboteQController::sprintf
sprintf(command, "?BA %i\r", ch)
RoboteQController::queryFaultFlag
int queryFaultFlag(void)
Query fault flags.
Definition: RoboteQController.h:284
RoboteQController::kROBOTEQ_BUFFER_OVER
@ kROBOTEQ_BUFFER_OVER
Definition: RoboteQController.h:44
RoboteQController::kROBOTEQ_STATUS_PULSE
@ kROBOTEQ_STATUS_PULSE
Definition: RoboteQController.h:56
RoboteQController::command
char command[kROBOTEQ_COMMAND_BUFFER_SIZE]
Definition: RoboteQController.h:178
RoboteQController::queryMotorPower
int queryMotorPower(uint8_t ch)
Query the motor power command.
Definition: RoboteQController.h:255
RoboteQController::kROBOTEQ_STATUS_POWER_OFF
@ kROBOTEQ_STATUS_POWER_OFF
Definition: RoboteQController.h:58
RoboteQController::setMotorAmpLimit
int setMotorAmpLimit(uint8_t ch, uint16_t a)
Set motor amp limit.
Definition: RoboteQController.h:377
RoboteQController::kROBOTEQ_STATUS_LIMIT
@ kROBOTEQ_STATUS_LIMIT
Definition: RoboteQController.h:60