int svrapp::client_ack(param_list * pl){
log_out(log_debug, "client_ack::json (%s)\n", pl->content);
uint cnt_id = 0;
char cnt_serial[128] = {0};
if(parse_serial(pl->serial, cnt_id, cnt_serial, sizeof(cnt_serial)) < 0){
log_out(log_error, "client_ack::json invalid serial %s\n", pl->serial);
return -1;
}
std::map<uint, app_connection*>::iterator it = m_cntmap.find(cnt_id);
if (it == m_cntmap.end()){
log_out(log_error, "client_ack::client(%u) is off\n", cnt_id);
return -1;
}
cJSON * jserror = cJSON_GetObjectItem(pl->json, "error");
cJSON * jsreason = cJSON_GetObjectItem(pl->json, "reason");
if(!jserror || !jsreason){
log_out(log_error, "client_ack::no error item\n");
return -1;
}
cJSON_DeleteItemFromObject(pl->jsdata, "device_id");
cJSON_DeleteItemFromObject(pl->jsdata, "company_id");
char * out = cJSON_PrintUnformatted(pl->jsdata);
post_ack(it->second, pl->cmd, cnt_serial,
jserror->valueint, jsreason->valuestring, out);
return 0;
}
int svrapp::client_ack(param_list * pl){
log_out(log_debug, "client_ack::json (%s)\n", pl->content);
uint realplay_id = 0;
char cnt_serial[128] = {0};
if(parse_serial(pl->serial, realplay_id, cnt_serial, sizeof(cnt_serial)) < 0){
log_out(log_error, "client_ack::json invalid serial %s\n", pl->serial);
return -1;
}
std::map<uint, app_connection*>::iterator it = m_cntmap.find(realplay_id);
if (it == m_cntmap.end()){
log_out(log_error, "client_ack::client(%u) is off\n", realplay_id);
return -1;
}
cJSON * jserror = cJSON_GetObjectItem(pl->json, "error");
cJSON * jsreason = cJSON_GetObjectItem(pl->json, "reason");
if(!jserror || !jsreason){
log_out(log_error, "client_ack::no error item\n");
return -1;
}
if(strcmp(pl->cmd, start_realplay_ack) == 0){
start_realplay_ackx(realplay_id, jserror->valueint, it->second);
}
else if(strcmp(pl->cmd, stop_realplay_ack) == 0){
stop_realplay_ackx(realplay_id, jserror->valueint, it->second);
}
cJSON_DeleteItemFromObject(pl->jsdata, "device_id");
cJSON_DeleteItemFromObject(pl->jsdata, "company_id");
char * out = cJSON_PrintUnformatted(pl->jsdata);
realplay_info * rpif = (realplay_info*)it->second->get_context();
if(rpif){
rpif->post_ack(pl->cmd, cnt_serial,
jserror->valueint, jsreason->valuestring, out);
}
free(out);
return 0;
}
///@brief Argp Parser Function for example-specific command line options
///@return Zero or error code specifying how to continue
static error_t
dummy_parse_opt(
const int key, ///< [in] Key field from the options vector
char *arg, ///< [in,out] Value given as option argument
struct argp_state *state) ///< [in,out] Parsing state for use by parser
{
// Retreive the input argument from argp_parse
struct tool_config *tool = state->input;
switch (key) {
case OPT_SET_SERIAL:
if (0 != parse_serial(&tool->overrides, arg)) {
argp_error(state, _("Invalid serial number `%s' specified."), arg);
}
break;
default:
return ARGP_ERR_UNKNOWN;
}
return 0;
}
int main(int argc, char **argv)
{
int expected_size = 0;
unsigned int transfer_size = 0;
enum mode mode = MODE_NONE;
struct dfu_status status;
libusb_context *ctx;
struct dfu_file file;
char *end;
int final_reset = 0;
int detach_unsupported = 0;
int ret;
int dfuse_device = 0;
int fd;
const char *dfuse_options = NULL;
int detach_delay = 5;
uint16_t runtime_vendor;
uint16_t runtime_product;
memset(&file, 0, sizeof(file));
/* make sure all prints are flushed */
setvbuf(stdout, NULL, _IONBF, 0);
while (1) {
int c, option_index = 0;
c = getopt_long(argc, argv, "hVvlenE:d:p:c:i:a:S:t:U:D:Rs:Z:", opts,
&option_index);
if (c == -1)
break;
switch (c) {
case 'h':
help();
break;
case 'V':
mode = MODE_VERSION;
break;
case 'v':
verbose++;
break;
case 'l':
mode = MODE_LIST;
break;
case 'e':
mode = MODE_DETACH;
match_iface_alt_index = 0;
match_iface_index = 0;
break;
case 'E':
detach_delay = atoi(optarg);
break;
case 'n':
detach_unsupported = 1;
break;
case 'd':
parse_vendprod(optarg);
break;
case 'p':
/* Parse device path */
ret = resolve_device_path(optarg);
if (ret < 0)
errx(EX_SOFTWARE, "Unable to parse '%s'", optarg);
if (!ret)
errx(EX_SOFTWARE, "Cannot find '%s'", optarg);
break;
case 'c':
/* Configuration */
match_config_index = atoi(optarg);
break;
case 'i':
/* Interface */
match_iface_index = atoi(optarg);
break;
case 'a':
/* Interface Alternate Setting */
match_iface_alt_index = strtoul(optarg, &end, 0);
if (*end) {
match_iface_alt_name = optarg;
match_iface_alt_index = -1;
}
break;
case 'S':
parse_serial(optarg);
break;
case 't':
transfer_size = atoi(optarg);
break;
case 'U':
mode = MODE_UPLOAD;
file.name = optarg;
break;
case 'Z':
expected_size = atoi(optarg);
break;
case 'D':
mode = MODE_DOWNLOAD;
file.name = optarg;
break;
case 'R':
final_reset = 1;
break;
case 's':
dfuse_options = optarg;
break;
default:
help();
break;
}
}
print_version();
if (mode == MODE_VERSION) {
exit(0);
}
if (mode == MODE_NONE) {
fprintf(stderr, "You need to specify one of -D or -U\n");
help();
}
if (match_config_index == 0) {
/* Handle "-c 0" (unconfigured device) as don't care */
match_config_index = -1;
}
if (mode == MODE_DOWNLOAD) {
dfu_load_file(&file, MAYBE_SUFFIX, MAYBE_PREFIX);
/* If the user didn't specify product and/or vendor IDs to match,
* use any IDs from the file suffix for device matching */
if (match_vendor < 0 && file.idVendor != 0xffff) {
match_vendor = file.idVendor;
printf("Match vendor ID from file: %04x\n", match_vendor);
}
if (match_product < 0 && file.idProduct != 0xffff) {
match_product = file.idProduct;
printf("Match product ID from file: %04x\n", match_product);
}
}
ret = libusb_init(&ctx);
if (ret)
errx(EX_IOERR, "unable to initialize libusb: %i", ret);
if (verbose > 2) {
libusb_set_debug(ctx, 255);
}
probe_devices(ctx);
if (mode == MODE_LIST) {
list_dfu_interfaces();
exit(0);
}
if (dfu_root == NULL) {
errx(EX_IOERR, "No DFU capable USB device available");
} else if (dfu_root->next != NULL) {
/* We cannot safely support more than one DFU capable device
* with same vendor/product ID, since during DFU we need to do
* a USB bus reset, after which the target device will get a
* new address */
errx(EX_IOERR, "More than one DFU capable USB device found! "
"Try `--list' and specify the serial number "
"or disconnect all but one device\n");
}
/* We have exactly one device. Its libusb_device is now in dfu_root->dev */
printf("Opening DFU capable USB device...\n");
ret = libusb_open(dfu_root->dev, &dfu_root->dev_handle);
if (ret || !dfu_root->dev_handle)
errx(EX_IOERR, "Cannot open device");
printf("ID %04x:%04x\n", dfu_root->vendor, dfu_root->product);
printf("Run-time device DFU version %04x\n",
libusb_le16_to_cpu(dfu_root->func_dfu.bcdDFUVersion));
/* Transition from run-Time mode to DFU mode */
if (!(dfu_root->flags & DFU_IFF_DFU)) {
int err;
/* In the 'first round' during runtime mode, there can only be one
* DFU Interface descriptor according to the DFU Spec. */
/* FIXME: check if the selected device really has only one */
runtime_vendor = dfu_root->vendor;
runtime_product = dfu_root->product;
printf("Claiming USB DFU Runtime Interface...\n");
if (libusb_claim_interface(dfu_root->dev_handle, dfu_root->interface) < 0) {
errx(EX_IOERR, "Cannot claim interface %d",
dfu_root->interface);
}
if (libusb_set_interface_alt_setting(dfu_root->dev_handle, dfu_root->interface, 0) < 0) {
errx(EX_IOERR, "Cannot set alt interface zero");
}
printf("Determining device status: ");
err = dfu_get_status(dfu_root, &status);
if (err == LIBUSB_ERROR_PIPE) {
printf("Device does not implement get_status, assuming appIDLE\n");
status.bStatus = DFU_STATUS_OK;
status.bwPollTimeout = 0;
status.bState = DFU_STATE_appIDLE;
status.iString = 0;
} else if (err < 0) {
errx(EX_IOERR, "error get_status");
} else {
printf("state = %s, status = %d\n",
dfu_state_to_string(status.bState), status.bStatus);
}
milli_sleep(status.bwPollTimeout);
switch (status.bState) {
case DFU_STATE_appIDLE:
case DFU_STATE_appDETACH:
if (!detach_unsupported) {
printf("Device really in Runtime Mode, send DFU "
"detach request...\n");
if (dfu_detach(dfu_root->dev_handle,
dfu_root->interface, 1000) < 0) {
warnx("error detaching");
}
}
if (dfu_root->func_dfu.bmAttributes & USB_DFU_WILL_DETACH) {
printf("Device will detach and reattach...\n");
} else {
printf("Resetting USB...\n");
ret = libusb_reset_device(dfu_root->dev_handle);
if (ret < 0 && ret != LIBUSB_ERROR_NOT_FOUND)
errx(EX_IOERR, "error resetting "
"after detach");
}
break;
case DFU_STATE_dfuERROR:
printf("dfuERROR, clearing status\n");
if (dfu_clear_status(dfu_root->dev_handle,
dfu_root->interface) < 0) {
errx(EX_IOERR, "error clear_status");
}
/* fall through */
default:
warnx("WARNING: Runtime device already in DFU state ?!?");
libusb_release_interface(dfu_root->dev_handle,
dfu_root->interface);
goto dfustate;
}
libusb_release_interface(dfu_root->dev_handle,
dfu_root->interface);
libusb_close(dfu_root->dev_handle);
dfu_root->dev_handle = NULL;
if (mode == MODE_DETACH) {
libusb_exit(ctx);
exit(0);
}
/* keeping handles open might prevent re-enumeration */
disconnect_devices();
milli_sleep(detach_delay * 1000);
/* Change match vendor and product to impossible values to force
* only DFU mode matches in the following probe */
match_vendor = match_product = 0x10000;
probe_devices(ctx);
if (dfu_root == NULL) {
errx(EX_IOERR, "Lost device after RESET?");
} else if (dfu_root->next != NULL) {
errx(EX_IOERR, "More than one DFU capable USB device found! "
"Try `--list' and specify the serial number "
"or disconnect all but one device");
}
/* Check for DFU mode device */
if (!(dfu_root->flags | DFU_IFF_DFU))
errx(EX_SOFTWARE, "Device is not in DFU mode");
printf("Opening DFU USB Device...\n");
ret = libusb_open(dfu_root->dev, &dfu_root->dev_handle);
if (ret || !dfu_root->dev_handle) {
errx(EX_IOERR, "Cannot open device");
}
} else {
/* we're already in DFU mode, so we can skip the detach/reset
* procedure */
/* If a match vendor/product was specified, use that as the runtime
* vendor/product, otherwise use the DFU mode vendor/product */
runtime_vendor = match_vendor < 0 ? dfu_root->vendor : match_vendor;
runtime_product = match_product < 0 ? dfu_root->product : match_product;
}
dfustate:
#if 0
printf("Setting Configuration %u...\n", dfu_root->configuration);
if (libusb_set_configuration(dfu_root->dev_handle, dfu_root->configuration) < 0) {
errx(EX_IOERR, "Cannot set configuration");
}
#endif
printf("Claiming USB DFU Interface...\n");
if (libusb_claim_interface(dfu_root->dev_handle, dfu_root->interface) < 0) {
errx(EX_IOERR, "Cannot claim interface");
}
printf("Setting Alternate Setting #%d ...\n", dfu_root->altsetting);
if (libusb_set_interface_alt_setting(dfu_root->dev_handle, dfu_root->interface, dfu_root->altsetting) < 0) {
errx(EX_IOERR, "Cannot set alternate interface");
}
status_again:
printf("Determining device status: ");
if (dfu_get_status(dfu_root, &status ) < 0) {
errx(EX_IOERR, "error get_status");
}
printf("state = %s, status = %d\n",
dfu_state_to_string(status.bState), status.bStatus);
milli_sleep(status.bwPollTimeout);
switch (status.bState) {
case DFU_STATE_appIDLE:
case DFU_STATE_appDETACH:
errx(EX_IOERR, "Device still in Runtime Mode!");
break;
case DFU_STATE_dfuERROR:
printf("dfuERROR, clearing status\n");
if (dfu_clear_status(dfu_root->dev_handle, dfu_root->interface) < 0) {
errx(EX_IOERR, "error clear_status");
}
goto status_again;
break;
case DFU_STATE_dfuDNLOAD_IDLE:
case DFU_STATE_dfuUPLOAD_IDLE:
printf("aborting previous incomplete transfer\n");
if (dfu_abort(dfu_root->dev_handle, dfu_root->interface) < 0) {
errx(EX_IOERR, "can't send DFU_ABORT");
}
goto status_again;
break;
case DFU_STATE_dfuIDLE:
printf("dfuIDLE, continuing\n");
break;
default:
break;
}
if (DFU_STATUS_OK != status.bStatus ) {
printf("WARNING: DFU Status: '%s'\n",
dfu_status_to_string(status.bStatus));
/* Clear our status & try again. */
if (dfu_clear_status(dfu_root->dev_handle, dfu_root->interface) < 0)
errx(EX_IOERR, "USB communication error");
if (dfu_get_status(dfu_root, &status) < 0)
errx(EX_IOERR, "USB communication error");
if (DFU_STATUS_OK != status.bStatus)
errx(EX_SOFTWARE, "Status is not OK: %d", status.bStatus);
milli_sleep(status.bwPollTimeout);
}
printf("DFU mode device DFU version %04x\n",
libusb_le16_to_cpu(dfu_root->func_dfu.bcdDFUVersion));
if (dfu_root->func_dfu.bcdDFUVersion == libusb_cpu_to_le16(0x11a))
dfuse_device = 1;
/* If not overridden by the user */
if (!transfer_size) {
transfer_size = libusb_le16_to_cpu(
dfu_root->func_dfu.wTransferSize);
if (transfer_size) {
printf("Device returned transfer size %i\n",
transfer_size);
} else {
errx(EX_IOERR, "Transfer size must be specified");
}
}
#ifdef HAVE_GETPAGESIZE
/* autotools lie when cross-compiling for Windows using mingw32/64 */
#ifndef __MINGW32__
/* limitation of Linux usbdevio */
if ((int)transfer_size > getpagesize()) {
transfer_size = getpagesize();
printf("Limited transfer size to %i\n", transfer_size);
}
#endif /* __MINGW32__ */
#endif /* HAVE_GETPAGESIZE */
if (transfer_size < dfu_root->bMaxPacketSize0) {
transfer_size = dfu_root->bMaxPacketSize0;
printf("Adjusted transfer size to %i\n", transfer_size);
}
switch (mode) {
case MODE_UPLOAD:
/* open for "exclusive" writing */
fd = open(file.name, O_WRONLY | O_BINARY | O_CREAT | O_EXCL | O_TRUNC, 0666);
if (fd < 0)
err(EX_IOERR, "Cannot open file %s for writing", file.name);
if (dfuse_device || dfuse_options) {
if (dfuse_do_upload(dfu_root, transfer_size, fd,
dfuse_options) < 0)
exit(1);
} else {
if (dfuload_do_upload(dfu_root, transfer_size,
expected_size, fd) < 0) {
exit(1);
}
}
close(fd);
break;
case MODE_DOWNLOAD:
if (((file.idVendor != 0xffff && file.idVendor != runtime_vendor) ||
(file.idProduct != 0xffff && file.idProduct != runtime_product)) &&
((file.idVendor != 0xffff && file.idVendor != dfu_root->vendor) ||
(file.idProduct != 0xffff && file.idProduct != dfu_root->product))) {
errx(EX_IOERR, "Error: File ID %04x:%04x does "
"not match device (%04x:%04x or %04x:%04x)",
file.idVendor, file.idProduct,
runtime_vendor, runtime_product,
dfu_root->vendor, dfu_root->product);
}
if (dfuse_device || dfuse_options || file.bcdDFU == 0x11a) {
if (dfuse_do_dnload(dfu_root, transfer_size, &file,
dfuse_options) < 0)
exit(1);
} else {
if (dfuload_do_dnload(dfu_root, transfer_size, &file) < 0)
exit(1);
}
break;
case MODE_DETACH:
if (detach_unsupported)
break;
ret = dfu_detach(dfu_root->dev_handle, dfu_root->interface, 1000);
if (ret < 0) {
warnx("can't detach");
}
break;
default:
errx(EX_IOERR, "Unsupported mode: %u", mode);
break;
}
if (final_reset) {
if (detach_unsupported) {
// STM32 DFU devices dont support DFU_DETACH
// Instead, force the device into STATE_DFU_MANIFEST_WAIT_RESET
// by sending a download request of size 0 and checking the state
if (dfu_download(dfu_root->dev_handle, dfu_root->interface, 0x0, 0x0, NULL)) {
warnx("Failure forcing a manifest");
} else {
if (dfu_get_status(dfu_root, &status) < 0) {
warnx("Unable to check status after manifest");
} else {
printf("state = %s, status = %d\n",
dfu_state_to_string(status.bState), status.bStatus);
if (status.bState != STATE_DFU_MANIFEST) {
warnx("Device should be in manifest state");
}
}
}
} else {
if (dfu_detach(dfu_root->dev_handle, dfu_root->interface, 1000) < 0) {
/* Even if detach failed, just carry on to leave the
device in a known state */
warnx("can't detach");
}
}
printf("Resetting USB to switch back to runtime mode\n");
ret = libusb_reset_device(dfu_root->dev_handle);
if (ret < 0 && ret != LIBUSB_ERROR_NOT_FOUND) {
errx(EX_IOERR, "error resetting after download");
}
}
libusb_close(dfu_root->dev_handle);
dfu_root->dev_handle = NULL;
libusb_exit(ctx);
return (0);
}
static int parse_header(void *addr, int len)
{
struct cb_header *header;
unsigned char *ptr = (unsigned char *)addr;
int i;
for (i = 0; i < len; i += 16, ptr += 16) {
header = (struct cb_header *)ptr;
if (!strncmp((const char *)header->signature, "LBIO", 4))
break;
}
/* We walked the entire space and didn't find anything. */
if (i >= len)
return -1;
if (!header->table_bytes)
return 0;
/* FIXME: Check the checksum. */
if (ipchksum((uint16_t *) header, sizeof(*header)))
return -1;
if (ipchksum((uint16_t *) (ptr + sizeof(*header)), header->table_bytes)
!= header->table_checksum)
return -1;
/* Now, walk the tables. */
ptr += header->header_bytes;
for (i = 0; i < header->table_entries; i++) {
struct cb_record *rec = (struct cb_record *)ptr;
switch (rec->tag) {
case CB_TAG_FORWARD:
return parse_header((void *)(unsigned long)((struct cb_forward *)rec)->forward, 1);
break;
case CB_TAG_MEMORY:
parse_memory(ptr);
break;
case CB_TAG_MAINBOARD:
parse_mainboard(ptr);
break;
case CB_TAG_VERSION:
case CB_TAG_EXTRA_VERSION:
case CB_TAG_BUILD:
case CB_TAG_COMPILE_TIME:
case CB_TAG_COMPILE_BY:
case CB_TAG_COMPILE_HOST:
case CB_TAG_COMPILE_DOMAIN:
case CB_TAG_COMPILER:
case CB_TAG_LINKER:
case CB_TAG_ASSEMBLER:
parse_strings(ptr);
break;
case CB_TAG_SERIAL:
parse_serial(ptr);
break;
case CB_TAG_CONSOLE:
parse_console(ptr);
break;
default:
break;
}
ptr += rec->size;
}
return 1;
}
int main(void)
{
DDRC = 0x3f; // data direction
PORTC = 0x3f; // LEDs and buzzer off
// timer 1 ctrl: 7654=0000 (no pins), 32=00 (no irq), 10=00 (ocr1a cmp)
TCCR1A = 0;
// timer 1 ctrl: 76=00 (no pin), 43=01 (ocr1a cmp), 210=001 (no prescaler)
TCCR1B = (1<<WGM12) | (1<<CS10);
// timer comparison, less than 150 is too fast for this CPU. 156 = 10/64 ms
OCR1A = 156;
// enable timer 1 output compare A match
TIMSK = 1<<OCIE1A;
// uart: U2X=1 and UBRR=0,12 means 9600 baud. With a 1-MHz clock, this is
// the highest reliable speed supported by Atmega chips (see spec p. 156).
UBRRH = 0; // 9600 baud (if U2X=1)
UBRRL = 12;
UCSRA = (1<<U2X); // double-speed baudrate set
UCSRB = (1<<RXEN) | (1<<TXEN); // enable receiver&transmitter
UCSRC = (1<<URSEL) | (3<<UCSZ0); // frame format: 8 data, 1 stop
char *p = (char *)action; // clear all actions, dark
int i;
for (i=0; i < sizeof(action); i++)
p[i] = 0;
set_default_actions(); // some interesting defaults
serial_len = 0; // prime serial command buffer
sei(); // enable timer interrupts
for (;;) { // event loop
if (need_next) { // bit is done, do next
recalculate();
need_next = false;
}
if (UCSRA & (1<<RXC)) { // serial byte received
uchar data = UDR; // fetch byte from host
if (serial_len == 0) { // first char:
if (data != '=' && // = define action
data != '+' && // + start action
data != '-') // - stop action
data = '?';
else
serial_buf[serial_len++] = data;
} else { // following char:
if (data == '\t' || data == ' ') // skip whitespace
;
else if (data == '\n' || // newline: parse cmd
data == '\r') {
serial_buf[serial_len] = 0;
parse_serial();
serial_len = 0;
} else // other char: store
if (serial_len > sizeof(serial_buf)-2)
data = '?';
else
serial_buf[serial_len++] = data;
}
while (!(UCSRA & (1<<UDRE))); // wait for tx ready
UDR = data; // echo byte
}
}
return 0;
}
/** Main program entry point. This routine contains the overall program flow
*/
int main(void)
{
initialize(); //Configures Ports, sets default values, etc.
//Loop Forever
for (;;)
{
//we add data starting with an '.' to the buffer until we get the newline character.
fgets(input, sizeof(input), stdin); //Get the actual input (reads up to and including newline character)
cmd = input[1]; //command char is always the first char of the input data after the '.'
//Command = 'q' - Query for the current status
if (cmd == 'q')
{
memset(input, 0, sizeof(input)); //Clear previous input
fprintf(stdout, ".q%d,%d\n", FIRMWARE_VERSION, NUM_MOTORS); //ACK w/ Firmware Version, # of Motors
}
//Command = 'l' - Command to Control Debug LED
else if (cmd == 'l')
{
if (input[2] == '1') DEBUG_LED_ON(); //Turn LED On
else if (input[2] == '0') DEBUG_LED_OFF(); //Turn LED Off
memset(input, 0, sizeof(input)); //Clear previous input
fprintf(stdout, ".l%d\n", DEBUG_LED_STATE()); //ACK
}
//Command = 'v' - Sets all motor speeds
//Comma separated entries telling all motors to set certain speeds
//Assumes motor IDs are 0 <-> NUM_MOTORS-1
else if (cmd == 'v')
{
parse_serial(); //Read the input string to an array of values
memset(input, 0, sizeof(input)); //Clear previous input
//send those speed commands
for (int i = 0; i<NUM_MOTORS; i++)
{
ax12SetRegister2(i, AX_GOAL_SPEED_L, vals[i]);
}
_delay_ms(25);
//Only after we have commanded all the speeds, can we check the status
fprintf(stdout, ".v"); //ACK Character
//Send ACK Info
for (int i = 0; i<NUM_MOTORS; i++)
{
fprintf(stdout, "%d", ax12GetRegister(i, AX_GOAL_SPEED_L, 2)); //Return velocity setting
if (i<NUM_MOTORS-1) fprintf(stdout, ","); //Print delimiter
}
fprintf(stdout, "\n"); //ACK Newline
}
//Command = 'c' - Command all the motors to a new position
//Comma separated entries telling all motors to move to positions from 0-1023
//Assumes motor IDs are 0 <-> NUM_MOTORS-1
else if (cmd == 'c')
{
parse_serial(); //Read the input string to an array of positions
memset(input, 0, sizeof(input)); //Clear previous input
//send those position commands
for (int i = 0; i<NUM_MOTORS; i++)
{
ax12SetRegister2(i, AX_GOAL_POSITION_L, vals[i]);
}
//Only after we have commanded all the positions, can we check the status
fprintf(stdout, ".c"); //ACK Character
//Send ACK Info
for (int i = 0; i<NUM_MOTORS; i++)
{
while(ax12GetRegister(i,AX_MOVING,1)); //Wait for this motor to finish moving
fprintf(stdout, "%d", ax12GetRegister(i, AX_PRESENT_POSITION_L, 2)); //Return the present position
if (i<NUM_MOTORS-1) fprintf(stdout, ","); //Print delimiter
}
fprintf(stdout, "\n"); //ACK Newline
}
}
}
