Compare commits
16 Commits
Author | SHA1 | Date |
---|---|---|
PgSocks | c0f2b11db5 | 6 months ago |
PgSocks | 11c3ac43fb | 6 months ago |
PgSocks | 23a07f99d9 | 6 months ago |
PgSocks | 19861ca234 | 7 months ago |
PgSocks | 4d11e5b66f | 7 months ago |
PgSocks | 72b9a2ba92 | 7 months ago |
PgSocks | 9de914dfa8 | 1 year ago |
PgSocks | d4123f440e | 1 year ago |
PgSocks | 90552382db | 1 year ago |
PgSocks | f2918b441d | 2 years ago |
PgSocks | 150c450210 | 2 years ago |
PgSocks | 19993ced8e | 2 years ago |
PgSocks | 2c42352c85 | 2 years ago |
PgSocks | 0b238ce048 | 2 years ago |
PgSocks | 2258812fc2 | 2 years ago |
PgSocks | 31d7e57125 | 2 years ago |
@ -0,0 +1,33 @@
|
||||
#pragma once
|
||||
|
||||
#include "message.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
static const uint8_t BLASTER_HOST = 23;
|
||||
static const uint8_t BLASTER_INDEX = 0;
|
||||
|
||||
#define BLASTER_FIRE_CMD 0x513F
|
||||
|
||||
enum FIRETYPE {
|
||||
FIRETYPE_WATER = 0,
|
||||
FIRETYPE_INFRARED = 1
|
||||
};
|
||||
|
||||
enum FIRETIMES {
|
||||
ONE = 1,
|
||||
TWO = 2,
|
||||
THREE = 3,
|
||||
FOUR = 4,
|
||||
FIVE = 5
|
||||
};
|
||||
|
||||
void
|
||||
blaster_fire (
|
||||
union Request* req,
|
||||
uint16_t seq,
|
||||
bool ack,
|
||||
enum FIRETYPE type,
|
||||
enum FIRETIMES times );
|
||||
|
@ -0,0 +1,40 @@
|
||||
#pragma once
|
||||
|
||||
#include "message.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
static const uint8_t STREAM_HOST = 1;
|
||||
static const uint8_t STREAM_INDEX = 0;
|
||||
|
||||
#define STREAM_CTRL_CMD 0xD23F
|
||||
|
||||
#define VIDEO_STREAM_PORT 40921
|
||||
|
||||
enum STREAMSTATE {
|
||||
STREAMSTATE_OFF = 0,
|
||||
STREAMSTATE_ON = 1
|
||||
};
|
||||
|
||||
enum STREAMRESOLUTION {
|
||||
RES_720P = 0,
|
||||
RES_360P = 1,
|
||||
RES_540P = 2
|
||||
};
|
||||
|
||||
enum STREAMCTRL {
|
||||
STREAMCTRL_SDK = 1,
|
||||
STREAMCTRL_VIDEO = 2,
|
||||
STREAMCTRL_AUDIO = 3
|
||||
};
|
||||
|
||||
void
|
||||
stream_ctrl (
|
||||
union Request* req,
|
||||
uint16_t seq,
|
||||
bool ack,
|
||||
enum STREAMSTATE state,
|
||||
enum STREAMRESOLUTION res,
|
||||
enum STREAMCTRL ctrl );
|
||||
|
@ -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,20 @@
|
||||
#pragma once
|
||||
|
||||
#include "message.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
static const uint8_t GIMBAL_HOST = 4;
|
||||
static const uint8_t GIMBAL_INDEX = 0;
|
||||
|
||||
#define GIMBAL_CTRL_SPEED_CMD 0x0C04
|
||||
|
||||
void
|
||||
gimbal_ctrl_speed (
|
||||
union Request* req,
|
||||
uint16_t seq,
|
||||
bool ack,
|
||||
int16_t p,
|
||||
int16_t y,
|
||||
int16_t r );
|
@ -0,0 +1,16 @@
|
||||
#include "message.h"
|
||||
#include "connection.h"
|
||||
#include "robomaster.h"
|
||||
|
||||
void
|
||||
blaster_fire (
|
||||
union Request* req,
|
||||
uint16_t seq,
|
||||
bool ack,
|
||||
enum FIRETYPE type,
|
||||
enum FIRETIMES times ) {
|
||||
req->blaster.type = type;
|
||||
req->blaster.times = times;
|
||||
req_finalize(seq, BLASTER_FIRE_CMD, ack, req);
|
||||
}
|
||||
|
@ -0,0 +1,20 @@
|
||||
#include "message.h"
|
||||
#include "connection.h"
|
||||
#include "robomaster.h"
|
||||
|
||||
void
|
||||
stream_ctrl (
|
||||
union Request* req,
|
||||
uint16_t seq,
|
||||
bool ack,
|
||||
enum STREAMSTATE state,
|
||||
enum STREAMRESOLUTION res,
|
||||
enum STREAMCTRL ctrl ) {
|
||||
req->stream.state = state;
|
||||
req->stream.resolution = res;
|
||||
req->stream.ctrl = ctrl;
|
||||
req->stream.conn_type = 0; // Hardcode to WiFi
|
||||
req_finalize(seq, STREAM_CTRL_CMD, ack, req);
|
||||
}
|
||||
|
||||
|
@ -0,0 +1,18 @@
|
||||
#include "message.h"
|
||||
#include "connection.h"
|
||||
#include "robomaster.h"
|
||||
|
||||
void
|
||||
gimbal_ctrl_speed (
|
||||
union Request* req,
|
||||
uint16_t seq,
|
||||
bool ack,
|
||||
int16_t p,
|
||||
int16_t y,
|
||||
int16_t r ) {
|
||||
req->gimbspeed.yaw = y;
|
||||
req->gimbspeed.roll = r;
|
||||
req->gimbspeed.pitch = p;
|
||||
req->gimbspeed.ctrl = 0xDC;
|
||||
req_finalize(seq, GIMBAL_CTRL_SPEED_CMD, ack, req);
|
||||
}
|
@ -0,0 +1,366 @@
|
||||
#include "roboeasy.h"
|
||||
#include "robomaster.h"
|
||||
#include "connection.h"
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
struct RobotImp {
|
||||
|
||||
struct Client* client;
|
||||
uint16_t seq;
|
||||
|
||||
int16_t wheels[4];
|
||||
bool dirty_wheels;
|
||||
|
||||
uint8_t colors[3];
|
||||
bool dirty_colors;
|
||||
|
||||
int16_t gimbal[2];
|
||||
bool dirty_gimbal;
|
||||
|
||||
bool dirty_blaster;
|
||||
|
||||
bool stream_state;
|
||||
bool dirty_stream;
|
||||
|
||||
bool sdk_mode;
|
||||
|
||||
float x, y, z;
|
||||
|
||||
uint16_t yaw_ground;
|
||||
uint16_t pitch_ground;
|
||||
uint16_t yaw;
|
||||
uint16_t pitch;
|
||||
|
||||
enum {
|
||||
CONNECTING_TO_PROXY_PORT,
|
||||
SETTING_SDK_CONNECTION,
|
||||
CONNECTING_TO_DEVICE_PORT,
|
||||
SETTING_SDK_MODE,
|
||||
RESETTING_SUBNODE,
|
||||
SUSBCRIBING_SUBNODE,
|
||||
SETTING_MOVEMENT_MODE,
|
||||
SUBSCRIBING_POSITION_STATUS,
|
||||
SUBSCRIBING_GIMBAL_STATUS,
|
||||
SENDING_HEARTBEAT,
|
||||
WAITING,
|
||||
READY,
|
||||
STOPPED
|
||||
} state;
|
||||
|
||||
};
|
||||
|
||||
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
|
||||
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_aim(Robot robot, float p, float y) {
|
||||
robot->gimbal[0] = p * 360;
|
||||
robot->gimbal[1] = y * 360;
|
||||
robot->dirty_gimbal = 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, ROBOT_DEFAULT_WIFI_ADDR);
|
||||
connection_connect(robot->client->sdk_conn);
|
||||
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:
|
||||
{
|
||||
if(connection_connect(robot->client->dev_conn))
|
||||
robot->state = STOPPED;
|
||||
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 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:
|
||||
{
|
||||
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_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) {
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
case STOPPED:
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
union Message resp;
|
||||
poll_message(robot->client, &resp);
|
||||
switch(resp.header.cmd) {
|
||||
case 0:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case SET_SDK_CONNECTION_CMD:
|
||||
// TODO: Do more with this
|
||||
if(resp.resp.sdkconn.retcode) {
|
||||
robot->state = STOPPED;
|
||||
break;
|
||||
}
|
||||
robot->client->dev_conn = connection_new(10010, resp.resp.sdkconn.config_ip, 20020, ROBOT_DEFAULT_WIFI_ADDR);
|
||||
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 = SUBSCRIBING_POSITION_STATUS;
|
||||
break;
|
||||
case ADD_SUB_MSG_CMD:
|
||||
if(resp.resp.sub.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;
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
Robot robot_new() {
|
||||
struct RobotImp* robot = malloc(sizeof(struct RobotImp));
|
||||
memset(robot, 0, sizeof(struct RobotImp));
|
||||
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