Compare commits

..

2 Commits

Author SHA1 Message Date
PgSocks 463fe60a1b Increment major version
The low level API needed to change to accomodate the hight level API,
which is a major revision.
2 years ago
PgSocks d883385d8c Implement simple SDK API 2 years ago

@ -15,12 +15,8 @@ add_library(robomaster
src/message.c src/message.c
src/modules/sdk.c src/modules/sdk.c
src/modules/chassis.c src/modules/chassis.c
src/modules/gimbal.c
src/modules/blaster.c
src/modules/camera.c
src/connection.c src/connection.c
src/robomaster.c src/robomaster.c
src/robo.c
) )
target_include_directories(robomaster target_include_directories(robomaster
@ -30,19 +26,14 @@ target_include_directories(robomaster
include include
) )
include(FetchContent) find_package(SDL2)
FetchContent_Declare (
SDL2
GIT_REPOSITORY https://github.com/libsdl-org/SDL
GIT_TAG release-2.0.20
GIT_SHALLOW TRUE
)
FetchContent_MakeAvailable(SDL2)
if(SDL2_FOUND)
add_executable(robomasterapp add_executable(robomasterapp
src/sdl.c src/sdl.c
) )
target_link_libraries(robomasterapp target_link_libraries(robomasterapp
robomaster robomaster
SDL2-static SDL2::SDL2-static
) )
endif()

@ -1,33 +0,0 @@
#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 );

@ -1,40 +0,0 @@
#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 );

@ -1,20 +0,0 @@
#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 );

@ -168,30 +168,6 @@ struct PACKED SetChassisWheelSpeedResp
struct Footer footer; struct Footer footer;
}; };
struct PACKED GimbalCtrlSpeedReq
{
struct Header header;
// Values between -360 and 360
union {
int16_t yrp[3];
struct {
int16_t yaw;
int16_t roll;
int16_t pitch;
};
};
// Always 0xDC
uint8_t ctrl;
struct Footer footer;
};
struct PACKED GimbalCtrlSpeedResp
{
struct Header header;
int8_t retcode;
struct Footer footer;
};
struct PACKED SetWheelSpeedReq struct PACKED SetWheelSpeedReq
{ {
struct Header header; struct Header header;
@ -248,84 +224,6 @@ struct PACKED SetSdkConnectionResp {
struct Footer footer; struct Footer footer;
}; };
struct PACKED BlasterFireReq {
struct Header header;
struct {
uint8_t type : 4;
uint8_t times : 4;
};
struct Footer footer;
};
struct PACKED BlasterFireResp {
struct Header header;
uint8_t retcode;
struct Footer footer;
};
struct PACKED StreamCtrlReq {
struct Header header;
uint8_t ctrl;
struct {
uint8_t state : 4;
uint8_t conn_type : 4;
};
uint8_t resolution;
struct Footer footer;
};
struct PACKED StreamCtrlResp {
struct Header header;
uint8_t retcode;
struct Footer footer;
};
struct PACKED AddSubMsgReq {
struct Header header;
uint8_t node_id;
uint8_t msg_id;
uint8_t reserved;
uint8_t sub_mode;
uint8_t subject_count;
uint64_t subject_uuid;
uint16_t freq;
struct Footer footer;
};
struct PACKED AddSubMsgResp {
struct Header header;
uint8_t retcode;
uint8_t pub_node_id;
uint8_t ack_sub_mode;
uint8_t ack_msg_id;
uint16_t ack_err_uid_data;
struct Footer footer;
};
struct PACKED PushPeriodMsg {
struct Header header;
uint8_t sub_mode;
uint8_t msg_id;
union {
struct {
float x;
float y;
float z;
} position;
struct {
uint16_t yaw_ground;
uint16_t pitch_ground;
uint16_t yaw;
uint16_t pitch;
struct {
uint8_t option_mode : 2;
uint8_t return_center : 1;
};
} gimbal;
};
struct Footer footer;
};
union Request { union Request {
struct Header header; struct Header header;
struct SetSdkConnectionReq sdkconn; struct SetSdkConnectionReq sdkconn;
@ -338,10 +236,6 @@ union Request {
struct SetRobotModeReq mvmode; struct SetRobotModeReq mvmode;
struct SubNodeResetReq subnodereset; struct SubNodeResetReq subnodereset;
struct SubscribeAddNodeReq subnodeadd; struct SubscribeAddNodeReq subnodeadd;
struct GimbalCtrlSpeedReq gimbspeed;
struct BlasterFireReq blaster;
struct StreamCtrlReq stream;
struct AddSubMsgReq sub;
}; };
union Response { union Response {
struct Header header; struct Header header;
@ -355,11 +249,6 @@ union Response {
struct SetRobotModeResp mvmode; struct SetRobotModeResp mvmode;
struct SubNodeResetResp subnodereset; struct SubNodeResetResp subnodereset;
struct SubscribeAddNodeResp subnodeadd; struct SubscribeAddNodeResp subnodeadd;
struct GimbalCtrlSpeedResp gimbspeed;
struct BlasterFireResp blaster;
struct StreamCtrlResp stream;
struct AddSubMsgResp sub;
struct PushPeriodMsg push;
}; };
union Message { union Message {
struct Header header; struct Header header;

@ -1,10 +1,8 @@
#pragma once #pragma once
#include <stdbool.h>
// Handle for the high level robot interface // Handle for the high level robot interface
struct RobotImp; struct Robot;
typedef struct RobotImp* Robot; typedef struct Robot* Robot;
/* /*
* Return a handle to the high level robot interface, or NULL on error. * Return a handle to the high level robot interface, or NULL on error.
@ -31,17 +29,6 @@ int robot_init(Robot robot);
*/ */
int robot_stop(Robot robot); int robot_stop(Robot robot);
/*
* Set the gimble speed. A packet will be sent to the robot on
* the next ready tick of the work function.
*
* robot: The robot to set the velocity of
* p: pitch speed
* y: yaw speed
* returns: 0 on success, non-zero on failure
*/
int robot_aim(Robot, float p, float y);
/* /*
* Set the velocity of the robot chassis. A packet will be sent to the robot on * Set the velocity of the robot chassis. A packet will be sent to the robot on
* the next ready tick of the work function. * the next ready tick of the work function.
@ -67,10 +54,6 @@ int robot_drive(Robot, float x, float y, float r);
*/ */
int robot_led(Robot, unsigned char r, unsigned char g, unsigned char b); int robot_led(Robot, unsigned char r, unsigned char g, unsigned char b);
int robot_blast(Robot);
int robot_stream(Robot, bool);
/* /*
* If the robot is in ready mode, this makes the robot send a heartbeat in * If the robot is in ready mode, this makes the robot send a heartbeat in
* the next tick of the work function. The work function will not do anything * the next tick of the work function. The work function will not do anything

@ -7,9 +7,6 @@ typedef struct Client* Client;
#include "message.h" #include "message.h"
#include "chassis.h" #include "chassis.h"
#include "sdk.h" #include "sdk.h"
#include "gimbal.h"
#include "blaster.h"
#include "camera.h"
Client client_new(); Client client_new();
void client_connect(Client client); void client_connect(Client client);

@ -108,34 +108,3 @@ subscribe_add_node (
uint16_t seq, uint16_t seq,
bool ack ); bool ack );
#define ADD_SUB_MSG_CMD 0x0348
#define PUSH_PERIOD_MSG_CMD 0x0848
#define DDS_BATTERY 0x000200096862229f
#define DDS_GIMBAL_BASE 0x00020009f5882874
#define DDS_VELOCITY 0x0002000949a4009c
#define DDS_ESC 0x00020009c14cb7c5
#define DDS_ATTITUDE 0x000200096b986306
#define DDS_IMU 0x00020009a7985b8d
#define DDS_POSITION 0x00020009eeb7cece
#define DDS_SA_STATUS 0x000200094a2c6d55
#define DDS_CHASSIS_MODE 0x000200094fcb1146
#define DDS_SBUS 0x0002000988223568
#define DDS_SERVO 0x000200095f0059e7
#define DDS_ARM 0x0002000926abd64d
#define DDS_GRIPPER 0x00020009124d156a
#define DDS_GIMBAL_POS 0x00020009f79b3c97
#define DDS_STICK 0x0002000955e9a0fa
#define DDS_MOVE_MODE 0x00020009784c7bfd
#define DDS_TOF 0x0002000986e4c05a
#define DDS_PINBOARD 0x00020009eebb9ffc
void
add_sub_msg (
union Request* req,
uint16_t seq,
bool ack,
uint8_t msg_id,
uint8_t freq,
uint64_t subject_uuid );

@ -7,6 +7,7 @@
#include <sys/socket.h> #include <sys/socket.h>
#include <sys/select.h> #include <sys/select.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h> #include <string.h>
#include <fcntl.h> #include <fcntl.h>
#include <stdio.h> #include <stdio.h>
@ -34,14 +35,6 @@ message_length(int cmd) {
return sizeof(struct SetWheelSpeedReq); return sizeof(struct SetWheelSpeedReq);
case CHASSIS_SPEED_MODE_CMD: case CHASSIS_SPEED_MODE_CMD:
return sizeof(struct ChassisSpeedModeReq); return sizeof(struct ChassisSpeedModeReq);
case GIMBAL_CTRL_SPEED_CMD:
return sizeof(struct GimbalCtrlSpeedReq);
case BLASTER_FIRE_CMD:
return sizeof(struct BlasterFireReq);
case STREAM_CTRL_CMD:
return sizeof(struct StreamCtrlReq);
case ADD_SUB_MSG_CMD:
return sizeof(struct AddSubMsgReq);
default: default:
return 0; return 0;
} }
@ -59,17 +52,10 @@ message_module(int cmd) {
case SET_ROBOT_MODE_CMD: case SET_ROBOT_MODE_CMD:
case SUBNODE_RESET_CMD: case SUBNODE_RESET_CMD:
case SUBSCRIBE_ADD_NODE_CMD: case SUBSCRIBE_ADD_NODE_CMD:
case ADD_SUB_MSG_CMD:
return host2byte(SDK_HOST, SDK_INDEX); return host2byte(SDK_HOST, SDK_INDEX);
case SET_WHEEL_SPEED_CMD: case SET_WHEEL_SPEED_CMD:
case CHASSIS_SPEED_MODE_CMD: case CHASSIS_SPEED_MODE_CMD:
return host2byte(CHASSIS_HOST, CHASSIS_INDEX); return host2byte(CHASSIS_HOST, CHASSIS_INDEX);
case GIMBAL_CTRL_SPEED_CMD:
return host2byte(GIMBAL_HOST, GIMBAL_INDEX);
case BLASTER_FIRE_CMD:
return host2byte(BLASTER_HOST, BLASTER_INDEX);
case STREAM_CTRL_CMD:
return host2byte(STREAM_HOST, STREAM_INDEX);
default: default:
return 0; return 0;
} }
@ -81,49 +67,44 @@ int max_fd = -1;
// TODO: Close the socket and return NULL on error // TODO: Close the socket and return NULL on error
struct Connection* struct Connection*
connection_new(uint16_t source_port, uint32_t source_ip, uint16_t dest_port, uint32_t dest_ip) connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip)
{ {
struct Connection* conn = malloc(sizeof(struct Connection)); struct Connection* conn = malloc(sizeof(struct Connection));
memset(conn, 0, sizeof(struct Connection)); memset(conn, 0, sizeof(struct Connection));
// Request a UDP socket
conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);
// Set the source address and port if they are provided // Set the source address and port if they are provided
if(source_port && source_ip) { if(source_port && source_ip) {
conn->local_addr.sin_family = AF_INET; struct sockaddr_in loc_addr;
conn->local_addr.sin_port = htons(source_port); loc_addr.sin_family = AF_INET;
conn->local_addr.sin_addr.s_addr = source_ip; loc_addr.sin_port = htons(source_port);
loc_addr.sin_addr.s_addr = inet_addr(source_ip);
if(bind(conn->sockfd, (struct sockaddr*)&loc_addr, sizeof(loc_addr)) < 0)
{
perror("unable to bind local port");
exit(EXIT_FAILURE);
}
} }
// Make the socket non-blocking
int flags = fcntl(conn->sockfd, F_GETFL);
fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK);
// Set the address of the drone // Set the address of the drone
memset(&conn->remote_addr, 0, sizeof(conn->remote_addr)); memset(&conn->remote_addr, 0, sizeof(conn->remote_addr));
conn->addrlen = sizeof(conn->remote_addr); conn->addrlen = sizeof(conn->remote_addr);
conn->remote_addr.sin_family = AF_INET; conn->remote_addr.sin_family = AF_INET;
conn->remote_addr.sin_port = htons(dest_port); conn->remote_addr.sin_port = htons(dest_port);
conn->remote_addr.sin_addr.s_addr = dest_ip; conn->remote_addr.sin_addr.s_addr = inet_addr(dest_ip);
return conn;
}
enum connection_error
connection_connect(struct Connection* conn) {
// Request a UDP socket
conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if(conn->local_addr.sin_addr.s_addr
&& bind(conn->sockfd, (struct sockaddr*)&conn->local_addr, sizeof(conn->local_addr)) < 0)
return CONNECTION_LOCAL_BIND;
// Make the socket non-blocking
int flags = fcntl(conn->sockfd, F_GETFL);
fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK);
// File descriptors are numbers that count up sequentially, // File descriptors are numbers that count up sequentially,
// so save the last one as the greatest file descriptor. // so save the last one as the greatest file descriptor.
// This is needed for polling the sockets later. // This is needed for polling the sockets later.
max_fd = conn->sockfd; max_fd = conn->sockfd;
return CONNECTION_NO_ERROR; return conn;
} }
struct Connection* struct Connection*

@ -6,25 +6,14 @@
#include <stddef.h> #include <stddef.h>
#include <netinet/in.h> #include <netinet/in.h>
#define ROBOT_DEFAULT_WIFI_ADDR 0X102A8C0
struct Connection { struct Connection {
int sockfd; int sockfd;
socklen_t addrlen; socklen_t addrlen;
struct sockaddr_in remote_addr; struct sockaddr_in remote_addr;
struct sockaddr_in local_addr;
}; };
struct Connection* struct Connection*
connection_new(uint16_t source_port, uint32_t source_ip, uint16_t dest_port, uint32_t dest_ip); connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip);
enum connection_error {
CONNECTION_NO_ERROR,
CONNECTION_LOCAL_BIND
};
enum connection_error
connection_connect();
struct Connection* struct Connection*
connection_poll_ready(struct Client* client); connection_poll_ready(struct Client* client);

@ -1,16 +0,0 @@
#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);
}

@ -1,20 +0,0 @@
#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);
}

@ -15,7 +15,7 @@ set_wheel_speed (
req->wheel.wheel_speed[1] = w2; req->wheel.wheel_speed[1] = w2;
req->wheel.wheel_speed[2] = w3; req->wheel.wheel_speed[2] = w3;
req->wheel.wheel_speed[3] = w4; req->wheel.wheel_speed[3] = w4;
req_finalize(seq, SET_WHEEL_SPEED_CMD, ack, req); req_finalize(seq, SET_WHEEL_SPEED_CMD, false, req);
} }
void void
@ -29,5 +29,5 @@ chassis_speed_mode (
req->chsspeed.speed[0] = x; req->chsspeed.speed[0] = x;
req->chsspeed.speed[1] = y; req->chsspeed.speed[1] = y;
req->chsspeed.speed[2] = z; req->chsspeed.speed[2] = z;
req_finalize(seq, CHASSIS_SPEED_MODE_CMD, ack, req); req_finalize(seq, CHASSIS_SPEED_MODE_CMD, false, req);
} }

@ -1,18 +0,0 @@
#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);
}

@ -17,7 +17,7 @@ set_sdk_connection(
req->sdkconn.protocol = 0; req->sdkconn.protocol = 0;
req->sdkconn.ip_address = ip_address; req->sdkconn.ip_address = ip_address;
req->sdkconn.port = port; req->sdkconn.port = port;
req_finalize(seq, SET_SDK_CONNECTION_CMD, ack, req); req_finalize(seq, SET_SDK_CONNECTION_CMD, true, req);
} }
void void
@ -27,7 +27,7 @@ set_sdk_mode(
bool ack, bool ack,
bool enable ) { bool enable ) {
req->sdkmode.enable = enable; req->sdkmode.enable = enable;
req_finalize(seq, SET_SDK_MODE_CMD, ack, req); req_finalize(seq, SET_SDK_MODE_CMD, true, req);
} }
void void
@ -35,7 +35,7 @@ sdk_heartbeat(
union Request* req, union Request* req,
uint16_t seq, uint16_t seq,
bool ack ) { bool ack ) {
req_finalize(seq, SDK_HEARTBEAT_CMD, ack, req); req_finalize(seq, SDK_HEARTBEAT_CMD, false, req);
} }
void void
@ -45,7 +45,7 @@ set_robot_mode (
bool ack, bool ack,
enum MOVEMENTMODE mode ) { enum MOVEMENTMODE mode ) {
req->mvmode.mode = mode; req->mvmode.mode = mode;
req_finalize(seq, SET_ROBOT_MODE_CMD, ack, req); req_finalize(seq, SET_ROBOT_MODE_CMD, true, req);
} }
void void
@ -54,7 +54,7 @@ subnode_reset (
uint16_t seq, uint16_t seq,
bool ack ) { bool ack ) {
req->subnodereset.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX); req->subnodereset.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
req_finalize(seq, SUBNODE_RESET_CMD, ack, req); req_finalize(seq, SUBNODE_RESET_CMD, true, req);
} }
void void
@ -64,7 +64,7 @@ subscribe_add_node (
bool ack ) { bool ack ) {
req->subnodeadd.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX); req->subnodeadd.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
req->subnodeadd.sub_vision = 0x03000000; req->subnodeadd.sub_vision = 0x03000000;
req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, ack, req); req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, true, req);
} }
void void
@ -93,27 +93,6 @@ set_system_led (
req->led.t1 = t1; req->led.t1 = t1;
req->led.t2 = t2; req->led.t2 = t2;
req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req); req_finalize(seq, SET_SYSTEM_LED_CMD, true, req);
}
void
add_sub_msg (
union Request* req,
uint16_t seq,
bool ack,
uint8_t msg_id,
uint8_t freq,
uint64_t subject_uuid ) {
req->sub.node_id = host2byte(CLIENT_HOST, CLIENT_INDEX);
req->sub.msg_id = msg_id;
req->sub.reserved = 0; // NOTE: Always 0
req->sub.sub_mode = 0; // NOTE: Always 0
req->sub.subject_count = 1; // TODO: Add support for multiple
req->sub.subject_uuid = subject_uuid; // TODO: Add support for multiple
req->sub.freq = freq;
req_finalize(seq, ADD_SUB_MSG_CMD, ack, req);
} }

@ -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;
} }

@ -16,11 +16,9 @@ struct {
}; };
int color = 0; int color = 0;
float pitch = 0, yaw = 0;
float x = 0, y = 0, z = 0; float x = 0, y = 0, z = 0;
static Uint32 drive_timer_handler(Uint32 interval, void* param) { static Uint32 drive_timer_handler(Uint32 interval, void* param) {
robot_drive((Robot)param, x, y, z); robot_drive((Robot)param, x, y, z);
robot_aim((Robot)param, pitch, yaw);
return 75; return 75;
} }
static Uint32 heartbeat_timer_handler(Uint32 interval, void* param) { static Uint32 heartbeat_timer_handler(Uint32 interval, void* param) {
@ -29,15 +27,10 @@ static Uint32 heartbeat_timer_handler(Uint32 interval, void* param) {
} }
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_JOYSTICK) < 0) { if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_GAMECONTROLLER) < 0) {
fprintf(stderr, "%s", SDL_GetError()); fprintf(stderr, "%s", SDL_GetError());
return 1; return 1;
} }
printf("Detected %d joysticks\n", SDL_NumJoysticks());
SDL_Joystick* joystick = NULL;
if(SDL_NumJoysticks() > 0) {
joystick = SDL_JoystickOpen(0);
}
SDL_Window* win = SDL_CreateWindow( SDL_Window* win = SDL_CreateWindow(
"Robomaster", "Robomaster",
SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED,
@ -51,14 +44,9 @@ int main(int argc, char* argv[]) {
Robot robot = robot_new(); Robot robot = robot_new();
robot_init(robot); robot_init(robot);
robot_stream(robot, true);
SDL_AddTimer(75, drive_timer_handler, robot); SDL_AddTimer(75, drive_timer_handler, robot);
SDL_AddTimer(1000, heartbeat_timer_handler, robot); SDL_AddTimer(1000, heartbeat_timer_handler, robot);
int h, w;
SDL_GetWindowSize(win, &w, &h);
while(robot_work(robot)) { while(robot_work(robot)) {
SDL_Event event; SDL_Event event;
while(SDL_PollEvent(&event)) { while(SDL_PollEvent(&event)) {
@ -91,39 +79,8 @@ int main(int argc, char* argv[]) {
case SDL_SCANCODE_SPACE: case SDL_SCANCODE_SPACE:
robot_led(robot, colors[color].r, colors[color].g, colors[color].b); robot_led(robot, colors[color].r, colors[color].g, colors[color].b);
color = (color + 1) % 3; color = (color + 1) % 3;
break;
case SDL_SCANCODE_RETURN:
robot_blast(robot);
break;
default: break; default: break;
} }
break;
case SDL_MOUSEMOTION:
yaw = (float)event.motion.xrel;
pitch = (float)event.motion.yrel;
break;
case SDL_JOYAXISMOTION:
switch(event.jaxis.axis) {
case 0:
x = (float)event.jaxis.value / 32767;
break;
case 1:
y = (float)event.jaxis.value / 32767;
break;
case 4:
z = (float)event.jaxis.value / 32767 / 2;
break;
case 2:
yaw = (float)event.jaxis.value / 32767;
break;
case 3:
pitch = (float)event.jaxis.value / 32767;
break;
default:
printf("axis: %d\n", event.jaxis.axis);
break;
}
break;
case SDL_WINDOWEVENT: case SDL_WINDOWEVENT:
if(event.window.event != SDL_WINDOWEVENT_CLOSE) break; if(event.window.event != SDL_WINDOWEVENT_CLOSE) break;
case SDL_QUIT: case SDL_QUIT:
@ -133,7 +90,6 @@ int main(int argc, char* argv[]) {
} }
} }
SDL_JoystickClose(joystick);
SDL_Quit(); SDL_Quit();
return 0; return 0;
} }

Loading…
Cancel
Save