|
|
@ -4,11 +4,8 @@
|
|
|
|
|
|
|
|
|
|
|
|
#include <string.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <arpa/inet.h>
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
struct Robot {
|
|
|
|
|
|
|
|
|
|
|
|
struct RobotImp {
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
struct Client* client;
|
|
|
|
struct Client* client;
|
|
|
|
uint16_t seq;
|
|
|
|
uint16_t seq;
|
|
|
@ -19,23 +16,8 @@ struct RobotImp {
|
|
|
|
uint8_t colors[3];
|
|
|
|
uint8_t colors[3];
|
|
|
|
bool dirty_colors;
|
|
|
|
bool dirty_colors;
|
|
|
|
|
|
|
|
|
|
|
|
int16_t gimbal[2];
|
|
|
|
|
|
|
|
bool dirty_gimbal;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
bool dirty_blaster;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
bool stream_state;
|
|
|
|
|
|
|
|
bool dirty_stream;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
bool sdk_mode;
|
|
|
|
bool sdk_mode;
|
|
|
|
|
|
|
|
|
|
|
|
float x, y, z;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
uint16_t yaw_ground;
|
|
|
|
|
|
|
|
uint16_t pitch_ground;
|
|
|
|
|
|
|
|
uint16_t yaw;
|
|
|
|
|
|
|
|
uint16_t pitch;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
enum {
|
|
|
|
enum {
|
|
|
|
CONNECTING_TO_PROXY_PORT,
|
|
|
|
CONNECTING_TO_PROXY_PORT,
|
|
|
|
SETTING_SDK_CONNECTION,
|
|
|
|
SETTING_SDK_CONNECTION,
|
|
|
@ -44,8 +26,6 @@ struct RobotImp {
|
|
|
|
RESETTING_SUBNODE,
|
|
|
|
RESETTING_SUBNODE,
|
|
|
|
SUSBCRIBING_SUBNODE,
|
|
|
|
SUSBCRIBING_SUBNODE,
|
|
|
|
SETTING_MOVEMENT_MODE,
|
|
|
|
SETTING_MOVEMENT_MODE,
|
|
|
|
SUBSCRIBING_POSITION_STATUS,
|
|
|
|
|
|
|
|
SUBSCRIBING_GIMBAL_STATUS,
|
|
|
|
|
|
|
|
SENDING_HEARTBEAT,
|
|
|
|
SENDING_HEARTBEAT,
|
|
|
|
WAITING,
|
|
|
|
WAITING,
|
|
|
|
READY,
|
|
|
|
READY,
|
|
|
@ -54,19 +34,6 @@ struct RobotImp {
|
|
|
|
|
|
|
|
|
|
|
|
};
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
int
|
|
|
|
|
|
|
|
robot_stream(Robot robot, bool state) {
|
|
|
|
|
|
|
|
robot->stream_state = state;
|
|
|
|
|
|
|
|
robot->dirty_stream = true;
|
|
|
|
|
|
|
|
return 1;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int
|
|
|
|
|
|
|
|
robot_blast(Robot robot) {
|
|
|
|
|
|
|
|
robot->dirty_blaster = true;
|
|
|
|
|
|
|
|
return 1;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int
|
|
|
|
int
|
|
|
|
robot_drive(Robot robot, float x, float y, float r) {
|
|
|
|
robot_drive(Robot robot, float x, float y, float r) {
|
|
|
|
// TODO: move individual wheel speed calculation to work function
|
|
|
|
// TODO: move individual wheel speed calculation to work function
|
|
|
@ -79,14 +46,6 @@ robot_drive(Robot robot, float x, float y, float r) {
|
|
|
|
return 1;
|
|
|
|
return 1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int
|
|
|
|
|
|
|
|
robot_aim(Robot robot, float p, float y) {
|
|
|
|
|
|
|
|
robot->gimbal[0] = p * 360;
|
|
|
|
|
|
|
|
robot->gimbal[1] = y * 360;
|
|
|
|
|
|
|
|
robot->dirty_gimbal = true;
|
|
|
|
|
|
|
|
return 1;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int
|
|
|
|
int
|
|
|
|
robot_led(Robot robot, unsigned char r, unsigned char g, unsigned char b) {
|
|
|
|
robot_led(Robot robot, unsigned char r, unsigned char g, unsigned char b) {
|
|
|
|
robot->colors[0] = r;
|
|
|
|
robot->colors[0] = r;
|
|
|
@ -120,8 +79,7 @@ robot_work(Robot robot) {
|
|
|
|
|
|
|
|
|
|
|
|
case CONNECTING_TO_PROXY_PORT:
|
|
|
|
case CONNECTING_TO_PROXY_PORT:
|
|
|
|
{
|
|
|
|
{
|
|
|
|
robot->client->sdk_conn = connection_new(0, 0, 30030, ROBOT_DEFAULT_WIFI_ADDR);
|
|
|
|
robot->client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
|
|
|
|
connection_connect(robot->client->sdk_conn);
|
|
|
|
|
|
|
|
robot->state = SETTING_SDK_CONNECTION;
|
|
|
|
robot->state = SETTING_SDK_CONNECTION;
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
@ -136,8 +94,7 @@ robot_work(Robot robot) {
|
|
|
|
|
|
|
|
|
|
|
|
case CONNECTING_TO_DEVICE_PORT:
|
|
|
|
case CONNECTING_TO_DEVICE_PORT:
|
|
|
|
{
|
|
|
|
{
|
|
|
|
if(connection_connect(robot->client->dev_conn))
|
|
|
|
robot->client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");
|
|
|
|
robot->state = STOPPED;
|
|
|
|
|
|
|
|
robot->state = SETTING_SDK_MODE;
|
|
|
|
robot->state = SETTING_SDK_MODE;
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
@ -182,22 +139,6 @@ robot_work(Robot robot) {
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
case SUBSCRIBING_POSITION_STATUS:
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
add_sub_msg ( &req, robot->seq++, true, 20, 1, DDS_POSITION );
|
|
|
|
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
|
|
|
|
robot->state = SUBSCRIBING_GIMBAL_STATUS;
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case SUBSCRIBING_GIMBAL_STATUS:
|
|
|
|
|
|
|
|
{
|
|
|
|
|
|
|
|
add_sub_msg ( &req, robot->seq++, true, 21, 1, DDS_GIMBAL_POS );
|
|
|
|
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
|
|
|
|
robot->state = WAITING;
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case READY:
|
|
|
|
case READY:
|
|
|
|
{
|
|
|
|
{
|
|
|
|
if(robot->dirty_wheels) {
|
|
|
|
if(robot->dirty_wheels) {
|
|
|
@ -210,15 +151,6 @@ robot_work(Robot robot) {
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
robot->dirty_wheels = false;
|
|
|
|
robot->dirty_wheels = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if(robot->dirty_gimbal) {
|
|
|
|
|
|
|
|
gimbal_ctrl_speed (
|
|
|
|
|
|
|
|
&req, robot->seq++, false,
|
|
|
|
|
|
|
|
robot->gimbal[0],
|
|
|
|
|
|
|
|
robot->gimbal[1],
|
|
|
|
|
|
|
|
0 );
|
|
|
|
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
|
|
|
|
robot->dirty_gimbal = false;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if(robot->dirty_colors) {
|
|
|
|
if(robot->dirty_colors) {
|
|
|
|
set_system_led (
|
|
|
|
set_system_led (
|
|
|
|
&req, robot->seq++, false,
|
|
|
|
&req, robot->seq++, false,
|
|
|
@ -233,32 +165,6 @@ robot_work(Robot robot) {
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
robot->dirty_colors = false;
|
|
|
|
robot->dirty_colors = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if(robot->dirty_blaster) {
|
|
|
|
|
|
|
|
blaster_fire (
|
|
|
|
|
|
|
|
&req, robot->seq++, false,
|
|
|
|
|
|
|
|
FIRETYPE_INFRARED,
|
|
|
|
|
|
|
|
ONE
|
|
|
|
|
|
|
|
);
|
|
|
|
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
|
|
|
|
robot->dirty_blaster = false;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if(robot->dirty_stream) {
|
|
|
|
|
|
|
|
stream_ctrl (
|
|
|
|
|
|
|
|
&req, robot->seq++, true,
|
|
|
|
|
|
|
|
robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
|
|
|
|
|
|
|
|
RES_720P,
|
|
|
|
|
|
|
|
STREAMCTRL_SDK
|
|
|
|
|
|
|
|
);
|
|
|
|
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
|
|
|
|
stream_ctrl (
|
|
|
|
|
|
|
|
&req, robot->seq++, true,
|
|
|
|
|
|
|
|
robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
|
|
|
|
|
|
|
|
RES_720P,
|
|
|
|
|
|
|
|
STREAMCTRL_VIDEO
|
|
|
|
|
|
|
|
);
|
|
|
|
|
|
|
|
req_send(robot->client->dev_conn, &req);
|
|
|
|
|
|
|
|
robot->dirty_stream = false;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
@ -272,15 +178,12 @@ robot_work(Robot robot) {
|
|
|
|
switch(resp.header.cmd) {
|
|
|
|
switch(resp.header.cmd) {
|
|
|
|
case 0:
|
|
|
|
case 0:
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
case SET_SDK_CONNECTION_CMD:
|
|
|
|
case SET_SDK_CONNECTION_CMD:
|
|
|
|
// TODO: Do more with this
|
|
|
|
// TODO: Do more with this
|
|
|
|
if(resp.resp.sdkconn.retcode) {
|
|
|
|
if(resp.resp.sdkconn.retcode) {
|
|
|
|
robot->state = STOPPED;
|
|
|
|
robot->state = STOPPED;
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
robot->client->dev_conn = connection_new(10010, resp.resp.sdkconn.config_ip, 20020, ROBOT_DEFAULT_WIFI_ADDR);
|
|
|
|
|
|
|
|
robot->state = CONNECTING_TO_DEVICE_PORT;
|
|
|
|
robot->state = CONNECTING_TO_DEVICE_PORT;
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
case SET_SDK_MODE_CMD:
|
|
|
|
case SET_SDK_MODE_CMD:
|
|
|
@ -310,13 +213,6 @@ robot_work(Robot robot) {
|
|
|
|
robot->state = STOPPED;
|
|
|
|
robot->state = STOPPED;
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
robot->state = SUBSCRIBING_POSITION_STATUS;
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
case ADD_SUB_MSG_CMD:
|
|
|
|
|
|
|
|
if(resp.resp.sub.retcode) {
|
|
|
|
|
|
|
|
robot->state = STOPPED;
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
robot->state = SENDING_HEARTBEAT;
|
|
|
|
robot->state = SENDING_HEARTBEAT;
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
case SDK_HEARTBEAT_CMD:
|
|
|
|
case SDK_HEARTBEAT_CMD:
|
|
|
@ -326,20 +222,6 @@ robot_work(Robot robot) {
|
|
|
|
}
|
|
|
|
}
|
|
|
|
robot->state = READY;
|
|
|
|
robot->state = READY;
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
case PUSH_PERIOD_MSG_CMD:
|
|
|
|
|
|
|
|
switch(resp.resp.push.msg_id) {
|
|
|
|
|
|
|
|
case 20:
|
|
|
|
|
|
|
|
robot->x = resp.resp.push.position.x;
|
|
|
|
|
|
|
|
robot->y = resp.resp.push.position.y;
|
|
|
|
|
|
|
|
robot->z = resp.resp.push.position.z;
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
case 21:
|
|
|
|
|
|
|
|
robot->yaw_ground = resp.resp.push.gimbal.yaw_ground;
|
|
|
|
|
|
|
|
robot->pitch_ground = resp.resp.push.gimbal.pitch_ground;
|
|
|
|
|
|
|
|
robot->yaw = resp.resp.push.gimbal.yaw;
|
|
|
|
|
|
|
|
robot->pitch = resp.resp.push.gimbal.pitch;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
return 1;
|
|
|
|
return 1;
|
|
|
@ -347,8 +229,8 @@ robot_work(Robot robot) {
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
Robot robot_new() {
|
|
|
|
Robot robot_new() {
|
|
|
|
struct RobotImp* robot = malloc(sizeof(struct RobotImp));
|
|
|
|
struct Robot* robot = malloc(sizeof(struct Robot));
|
|
|
|
memset(robot, 0, sizeof(struct RobotImp));
|
|
|
|
memset(robot, 0, sizeof(struct Robot));
|
|
|
|
return robot;
|
|
|
|
return robot;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|