Implement simple SDK API
parent
af5a6efdb3
commit
31d7e57125
@ -0,0 +1,13 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
struct Connection;
|
||||||
|
typedef Connection* Connection;
|
||||||
|
|
||||||
|
struct Connection*
|
||||||
|
conn_new (
|
||||||
|
struct Connection* connection,
|
||||||
|
unsigned int source_port,
|
||||||
|
const char* source_ip,
|
||||||
|
unsigned int dest_port,
|
||||||
|
const char* dest_ip );
|
||||||
|
extern const size_t SIZEOF_CONN;
|
||||||
@ -0,0 +1,248 @@
|
|||||||
|
#include "roboeasy.h"
|
||||||
|
#include "robomaster.h"
|
||||||
|
#include "connection.h"
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
struct Robot {
|
||||||
|
|
||||||
|
struct Client* client;
|
||||||
|
uint16_t seq;
|
||||||
|
|
||||||
|
int16_t wheels[4];
|
||||||
|
bool dirty_wheels;
|
||||||
|
|
||||||
|
uint8_t colors[3];
|
||||||
|
bool dirty_colors;
|
||||||
|
|
||||||
|
bool sdk_mode;
|
||||||
|
|
||||||
|
enum {
|
||||||
|
CONNECTING_TO_PROXY_PORT,
|
||||||
|
SETTING_SDK_CONNECTION,
|
||||||
|
CONNECTING_TO_DEVICE_PORT,
|
||||||
|
SETTING_SDK_MODE,
|
||||||
|
RESETTING_SUBNODE,
|
||||||
|
SUSBCRIBING_SUBNODE,
|
||||||
|
SETTING_MOVEMENT_MODE,
|
||||||
|
SENDING_HEARTBEAT,
|
||||||
|
WAITING,
|
||||||
|
READY,
|
||||||
|
STOPPED
|
||||||
|
} state;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
int
|
||||||
|
robot_drive(Robot robot, float x, float y, float r) {
|
||||||
|
// TODO: move individual wheel speed calculation to work function
|
||||||
|
// This function may be used in a timer handler, so fast as possible
|
||||||
|
robot->wheels[0] = (y + x + r) * 1000;
|
||||||
|
robot->wheels[1] = -(y - x - r) * 1000;
|
||||||
|
robot->wheels[2] = -(y + x - r) * 1000;
|
||||||
|
robot->wheels[3] = (y - x + r) * 1000;
|
||||||
|
robot->dirty_wheels = true;
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
robot_led(Robot robot, unsigned char r, unsigned char g, unsigned char b) {
|
||||||
|
robot->colors[0] = r;
|
||||||
|
robot->colors[1] = g;
|
||||||
|
robot->colors[2] = b;
|
||||||
|
robot->dirty_colors = true;
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
robot_heartbeat(Robot robot) {
|
||||||
|
|
||||||
|
if(robot->state != READY)
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
robot->state = SENDING_HEARTBEAT;
|
||||||
|
|
||||||
|
return 1;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
robot_work(Robot robot) {
|
||||||
|
|
||||||
|
union Request req = {0};
|
||||||
|
|
||||||
|
switch(robot->state) {
|
||||||
|
|
||||||
|
case WAITING:
|
||||||
|
break;
|
||||||
|
|
||||||
|
case CONNECTING_TO_PROXY_PORT:
|
||||||
|
{
|
||||||
|
robot->client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
|
||||||
|
robot->state = SETTING_SDK_CONNECTION;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case SETTING_SDK_CONNECTION:
|
||||||
|
{
|
||||||
|
set_sdk_connection(&req, robot->seq++, true, CONNECTION_WIFI_AP, 0, 10010);
|
||||||
|
req_send(robot->client->sdk_conn, &req);
|
||||||
|
robot->state = WAITING;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case CONNECTING_TO_DEVICE_PORT:
|
||||||
|
{
|
||||||
|
robot->client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");
|
||||||
|
robot->state = SETTING_SDK_MODE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case SETTING_SDK_MODE:
|
||||||
|
{
|
||||||
|
set_sdk_mode(&req, robot->seq++, true, robot->sdk_mode);
|
||||||
|
req_send(robot->client->dev_conn, &req);
|
||||||
|
robot->state = WAITING;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case RESETTING_SUBNODE:
|
||||||
|
{
|
||||||
|
subnode_reset(&req, robot->seq++, true);
|
||||||
|
req_send(robot->client->dev_conn, &req);
|
||||||
|
robot->state = WAITING;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case SUSBCRIBING_SUBNODE:
|
||||||
|
{
|
||||||
|
subscribe_add_node(&req, robot->seq++, true);
|
||||||
|
req_send(robot->client->dev_conn, &req);
|
||||||
|
robot->state = WAITING;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case SETTING_MOVEMENT_MODE:
|
||||||
|
{
|
||||||
|
set_robot_mode(&req, robot->seq++, true, MOVEMENTMODE_FREE);
|
||||||
|
req_send(robot->client->dev_conn, &req);
|
||||||
|
robot->state = WAITING;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case SENDING_HEARTBEAT:
|
||||||
|
{
|
||||||
|
sdk_heartbeat(&req, robot->seq++, true);
|
||||||
|
req_send(robot->client->sdk_conn, &req);
|
||||||
|
robot->state = WAITING;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case READY:
|
||||||
|
{
|
||||||
|
if(robot->dirty_wheels) {
|
||||||
|
set_wheel_speed (
|
||||||
|
&req, robot->seq++, false,
|
||||||
|
robot->wheels[0],
|
||||||
|
robot->wheels[1],
|
||||||
|
robot->wheels[2],
|
||||||
|
robot->wheels[3] );
|
||||||
|
req_send(robot->client->dev_conn, &req);
|
||||||
|
robot->dirty_wheels = false;
|
||||||
|
}
|
||||||
|
if(robot->dirty_colors) {
|
||||||
|
set_system_led (
|
||||||
|
&req, robot->seq++, false,
|
||||||
|
robot->colors[0],
|
||||||
|
robot->colors[1],
|
||||||
|
robot->colors[2],
|
||||||
|
LEDCOMP_ALL,
|
||||||
|
0xFFFF,
|
||||||
|
LEDEFFECT_ON,
|
||||||
|
100,
|
||||||
|
100 );
|
||||||
|
req_send(robot->client->dev_conn, &req);
|
||||||
|
robot->dirty_colors = false;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case STOPPED:
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
union Message resp;
|
||||||
|
poll_message(robot->client, &resp);
|
||||||
|
switch(resp.header.cmd) {
|
||||||
|
case 0:
|
||||||
|
break;
|
||||||
|
case SET_SDK_CONNECTION_CMD:
|
||||||
|
// TODO: Do more with this
|
||||||
|
if(resp.resp.sdkconn.retcode) {
|
||||||
|
robot->state = STOPPED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
robot->state = CONNECTING_TO_DEVICE_PORT;
|
||||||
|
break;
|
||||||
|
case SET_SDK_MODE_CMD:
|
||||||
|
// TODO: Do more with this
|
||||||
|
if(resp.resp.sdkmode.retcode) {
|
||||||
|
robot->state = STOPPED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
robot->state = robot->sdk_mode ? RESETTING_SUBNODE : STOPPED;
|
||||||
|
break;
|
||||||
|
case SUBNODE_RESET_CMD:
|
||||||
|
if(resp.resp.subnodereset.retcode) {
|
||||||
|
robot->state = STOPPED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
robot->state = SUSBCRIBING_SUBNODE;
|
||||||
|
break;
|
||||||
|
case SUBSCRIBE_ADD_NODE_CMD:
|
||||||
|
if(resp.resp.subnodeadd.retcode && resp.resp.subnodeadd.retcode != 0x50) {
|
||||||
|
robot->state = STOPPED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
robot->state = SETTING_MOVEMENT_MODE;
|
||||||
|
break;
|
||||||
|
case SET_ROBOT_MODE_CMD:
|
||||||
|
if(resp.resp.mvmode.retcode) {
|
||||||
|
robot->state = STOPPED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
robot->state = SENDING_HEARTBEAT;
|
||||||
|
break;
|
||||||
|
case SDK_HEARTBEAT_CMD:
|
||||||
|
if(resp.resp.heartbeat.retcode) {
|
||||||
|
robot->state = STOPPED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
robot->state = READY;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 1;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
Robot robot_new() {
|
||||||
|
struct Robot* robot = malloc(sizeof(struct Robot));
|
||||||
|
memset(robot, 0, sizeof(struct Robot));
|
||||||
|
return robot;
|
||||||
|
}
|
||||||
|
|
||||||
|
int robot_init(Robot robot) {
|
||||||
|
robot->client = client_new();
|
||||||
|
robot->sdk_mode = true;
|
||||||
|
robot->state = CONNECTING_TO_PROXY_PORT;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int robot_stop(Robot robot) {
|
||||||
|
robot->sdk_mode = false;
|
||||||
|
robot->state = SETTING_SDK_MODE;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue