Compare commits
2 Commits
| Author | SHA1 | Date |
|---|---|---|
 PgSocks
|
463fe60a1b | 3 years ago |
|
PgSocks
|
d883385d8c | 3 years ago |
@ -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