You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
93 lines
2.7 KiB
C
93 lines
2.7 KiB
C
#include "robomaster.h"
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <unistd.h>
|
|
|
|
int main(int argc, char* argv[])
|
|
{
|
|
Client client = client_new();
|
|
client_connect(client);
|
|
|
|
union Message msg;
|
|
poll_message(client, &msg);
|
|
if(msg.header.cmdid != SET_SDK_CONNECTION_CMDID || msg.resp.sdkconn.retcode) {
|
|
fprintf(stderr, "Could not set SDK connection\n");
|
|
return 1;
|
|
}
|
|
|
|
set_sdk_mode(client, true);
|
|
poll_message(client, &msg);
|
|
if(msg.header.cmdid != SET_SDK_MODE_CMDID || msg.resp.sdkmode.retcode) {
|
|
fprintf(stderr, "Could not set SDK mode\n");
|
|
return 1;
|
|
}
|
|
|
|
subnode_reset(client);
|
|
poll_message(client, &msg);
|
|
if(msg.header.cmdid != SUBNODE_RESET_CMDID || msg.resp.subnodereset.retcode) {
|
|
fprintf(stderr, "Could not reset subnode subscription\n");
|
|
return 1;
|
|
}
|
|
|
|
subscribe_add_node(client);
|
|
poll_message(client, &msg);
|
|
if(msg.header.cmdid != SUBSCRIBE_ADD_NODE_CMDID || (msg.resp.subnodeadd.retcode && msg.resp.subnodeadd.retcode != 0x50)) {
|
|
fprintf(stderr, "Could not subscribe node\n");
|
|
return 1;
|
|
}
|
|
|
|
set_robot_mode(client, MOVEMENTMODE_FREE);
|
|
poll_message(client, &msg);
|
|
if(msg.header.cmdid != SET_ROBOT_MODE_CMDID || msg.resp.mvmode.retcode) {
|
|
fprintf(stderr, "Could not set move mode\n");
|
|
return 1;
|
|
}
|
|
|
|
sdk_heartbeat(client);
|
|
//poll_message(client, &msg);
|
|
//if(msg.header.cmdid != SDK_HEARTBEAT_CMDID || msg.resp.heartbeat.retcode) {
|
|
// fprintf(stderr, "Did not receive heartbeat\n");
|
|
// return 1;
|
|
//}
|
|
|
|
int c;
|
|
while((c = getopt(argc, argv, "hl:::s:::")) != -1) {
|
|
switch(c) {
|
|
case 'l':
|
|
set_system_led (
|
|
client,
|
|
strtol(argv[optind + 0], NULL, 0),
|
|
strtol(argv[optind + 1], NULL, 0),
|
|
strtol(argv[optind + 2], NULL, 0),
|
|
LEDCOMP_ALL,
|
|
0xFFFF,
|
|
LEDEFFECT_ON,
|
|
100,
|
|
100 );
|
|
poll_message(client, &msg);
|
|
if(msg.header.cmdid != SET_SYSTEM_LED_CMDID || msg.resp.led.retcode) {
|
|
fprintf(stderr, "Could not set LED color\n");
|
|
return 1;
|
|
}
|
|
break;
|
|
case 's':
|
|
chassis_speed_mode (
|
|
client,
|
|
atof(argv[optind + 0]),
|
|
atof(argv[optind + 1]),
|
|
atof(argv[optind + 2]) );
|
|
break;
|
|
case '?':
|
|
default:
|
|
fprintf(stderr, "Unknown argument %c\n", optopt);
|
|
case 'h':
|
|
printf("Usage: [-h] [-l r g b] [-w w1 w2 w3 w4]");
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|