pgsocks
/
robomaster-sdk
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add command to rotate gimbal to specific coordinates

Browse Source
gimbal
PgSocks 7 months ago
parent c0f2b11db5
commit 98f10c20c9
6 changed files with 216 additions and 0 deletions
Show Stats Download Patch File Download Diff File

18
include/gimbal.h
Unescape Escape View File

@ -18,3 +18,21 @@ gimbal_ctrl_speed (
int16_t p,
int16_t y,
int16_t r );
#define GIMBAL_ROTATE_CMD 0xb03f
void
gimbal_rotate (
union Request* req,
uint16_t seq,
bool ack,
int16_t p,
int16_t y );
void
gimbal_rotate_to (
union Request* req,
uint16_t seq,
bool ack,
int16_t p,
int16_t y );
#define GIMBAL_ACTION_PUSH_CMD 0xb13f

70
include/message.h
Unescape Escape View File

@ -185,6 +185,73 @@ struct PACKED GimbalCtrlSpeedReq
struct Footer footer;
};
struct PACKED GimbalActionPush {
struct Header header;
uint8_t id;
uint8_t progress;
uint8_t state;
int16_t yaw;
int16_t roll;
int16_t pitch;
};
struct PACKED GimbalRotateReq {
struct Header header;
uint8_t id;
//union {
uint8_t control_bitfield;
// struct {
// // always 0 for start
// uint8_t control : 1;
// uint8_t frequency : 2;
// };
//};
//union {
uint8_t coord_bitfield;
// struct {
// uint8_t coordinate : 3;
// // always true
// uint8_t pitch_valid : 1;
// // always false
// uint8_t roll_valid : 1;
// // always true
// uint8_t yaw_valid : 1;
// };
//};
union {
int16_t yaw;
struct {
int8_t l_yaw;
int8_t h_yaw;
};
};
int16_t roll;
union {
int16_t pitch;
struct {
int8_t l_pitch;
int8_t h_pitch;
};
};
uint16_t error;
uint16_t yaw_speed;
uint16_t roll_speed;
uint16_t pitch_speed;
struct Footer footer;
};
struct PACKED GimbalRotateResp {
struct Header header;
uint8_t retcode;
uint8_t accept;
struct Footer footer;
};
struct PACKED GimbalCtrlSpeedResp
{
struct Header header;
@ -339,6 +406,7 @@ union Request {
struct SubNodeResetReq subnodereset;
struct SubscribeAddNodeReq subnodeadd;
struct GimbalCtrlSpeedReq gimbspeed;
struct GimbalRotateReq gimbalrot;
struct BlasterFireReq blaster;
struct StreamCtrlReq stream;
struct AddSubMsgReq sub;
@ -356,10 +424,12 @@ union Response {
struct SubNodeResetResp subnodereset;
struct SubscribeAddNodeResp subnodeadd;
struct GimbalCtrlSpeedResp gimbspeed;
struct GimbalRotateResp gimbalrot;
struct BlasterFireResp blaster;
struct StreamCtrlResp stream;
struct AddSubMsgResp sub;
struct PushPeriodMsg push;
struct GimbalActionPush gimbalaction;
};
union Message {
struct Header header;

3
include/roboeasy.h
Unescape Escape View File

@ -42,6 +42,9 @@ int robot_stop(Robot robot);
*/
int robot_aim(Robot, float p, float y);
int robot_target(Robot robot, float pitch, float yaw);
int robot_target_to(Robot robot, float pitch, float yaw);
/*
* Set the velocity of the robot chassis. A packet will be sent to the robot on
* the next ready tick of the work function.

3
src/connection.c
Unescape Escape View File

@ -36,6 +36,8 @@ message_length(int cmd) {
return sizeof(struct ChassisSpeedModeReq);
case GIMBAL_CTRL_SPEED_CMD:
return sizeof(struct GimbalCtrlSpeedReq);
case GIMBAL_ROTATE_CMD:
return sizeof(struct GimbalRotateReq);
case BLASTER_FIRE_CMD:
return sizeof(struct BlasterFireReq);
case STREAM_CTRL_CMD:
@ -65,6 +67,7 @@ message_module(int cmd) {
case CHASSIS_SPEED_MODE_CMD:
return host2byte(CHASSIS_HOST, CHASSIS_INDEX);
case GIMBAL_CTRL_SPEED_CMD:
case GIMBAL_ROTATE_CMD:
return host2byte(GIMBAL_HOST, GIMBAL_INDEX);
case BLASTER_FIRE_CMD:
return host2byte(BLASTER_HOST, BLASTER_INDEX);

64
src/modules/gimbal.c
Unescape Escape View File

@ -16,3 +16,67 @@ gimbal_ctrl_speed (
req->gimbspeed.ctrl = 0xDC;
req_finalize(seq, GIMBAL_CTRL_SPEED_CMD, ack, req);
}
void
gimbal_rotate (
union Request* req,
uint16_t seq,
bool ack,
int16_t p,
int16_t y ) {
//req->gimbalrot.coordinate = 1;
//req->gimbalrot.yaw_valid = 1;
//req->gimbalrot.pitch_valid = 1;
//req->gimbalrot.roll_valid = 0;
req->gimbalrot.coord_bitfield = 0;
req->gimbalrot.coord_bitfield = 1 | 0 << 1 | 1 << 2 | 1 << 3;
//req->gimbalrot.frequency = 2;
//req->gimbalrot.control = 0;
req->gimbalrot.control_bitfield = 0;
req->gimbalrot.control_bitfield = 0 | 1 << 2;
req->gimbalrot.id = 33;
req->gimbalrot.yaw = 0;
req->gimbalrot.yaw = y;// >> 8;//(y >> 8 | y << 8);
req->gimbalrot.roll = 0;
req->gimbalrot.pitch = 0;
req->gimbalrot.pitch = p;// >> 8;//(p >> 8 | y << 8);
req->gimbalrot.error = 0;
req->gimbalrot.yaw_speed = 30;
req->gimbalrot.roll_speed = 0;
req->gimbalrot.pitch_speed = 30;
req_finalize(seq, GIMBAL_ROTATE_CMD, ack, req);
}
void
gimbal_rotate_to (
union Request* req,
uint16_t seq,
bool ack,
int16_t p,
int16_t y ) {
//req->gimbalrot.coordinate = 1;
//req->gimbalrot.yaw_valid = 1;
//req->gimbalrot.pitch_valid = 1;
//req->gimbalrot.roll_valid = 0;
req->gimbalrot.coord_bitfield = 0;
req->gimbalrot.coord_bitfield = 1 | 0 << 1 | 1 << 2 | 4 << 3;
//req->gimbalrot.frequency = 2;
//req->gimbalrot.control = 0;
req->gimbalrot.control_bitfield = 0;
req->gimbalrot.control_bitfield = 0 | 1 << 2;
req->gimbalrot.id = 33;
req->gimbalrot.yaw = 0;
req->gimbalrot.yaw = y;// >> 8;//(y >> 8 | y << 8);
req->gimbalrot.roll = 0;
req->gimbalrot.pitch = 0;
req->gimbalrot.pitch = p;// >> 8;//(p >> 8 | y << 8);
req->gimbalrot.error = 0;
req->gimbalrot.yaw_speed = 30;
req->gimbalrot.roll_speed = 0;
req->gimbalrot.pitch_speed = 30;
req_finalize(seq, GIMBAL_ROTATE_CMD, ack, req);
}

58
src/robo.c
Unescape Escape View File

@ -5,6 +5,7 @@
#include <string.h>
#include <stdlib.h>
#include <arpa/inet.h>
#include <math.h>
#include <stdio.h>
@ -35,6 +36,11 @@ struct RobotImp {
uint16_t pitch_ground;
uint16_t yaw;
uint16_t pitch;
enum {
TARGET_NONE,
TARGET_REL,
TARGET_ABS
} dirty_target;
enum {
CONNECTING_TO_PROXY_PORT,
@ -87,6 +93,44 @@ robot_aim(Robot robot, float p, float y) {
return 1;
}
int
robot_target(Robot robot, float pitch, float yaw) {
// Get the FOV angle of the point in radians
float FOV = 120 * (M_PI / 180);
yaw = yaw * (FOV / 2);
pitch = pitch * (FOV / 2);
// Convert to degrees
yaw = yaw * (180 / M_PI);
pitch = pitch * (180 / M_PI);
// Convert for Robomaster
robot->yaw = yaw * 10;
robot->pitch = -pitch * 10;
robot->dirty_target = TARGET_REL;
return 1;
};
int
robot_target_to(Robot robot, float pitch, float yaw) {
// Get the FOV angle of the point in radians
float FOV = 120 * (M_PI / 180);
yaw = yaw * (FOV / 2);
pitch = pitch * (FOV / 2);
// Convert to degrees
yaw = yaw * (180 / M_PI);
pitch = pitch * (180 / M_PI);
// Convert for Robomaster
robot->yaw = yaw * 10;
robot->pitch = -pitch * 10;
robot->dirty_target = TARGET_ABS;
return 1;
};
int
robot_led(Robot robot, unsigned char r, unsigned char g, unsigned char b) {
robot->colors[0] = r;
@ -219,6 +263,20 @@ robot_work(Robot robot) {
req_send(robot->client->dev_conn, &req);
robot->dirty_gimbal = false;
}
if(robot->dirty_target) {
if(robot->dirty_target == TARGET_REL)
gimbal_rotate (
&req, robot->seq++, true,
robot->pitch,
robot->yaw );
else if(robot->dirty_target == TARGET_ABS)
gimbal_rotate_to (
&req, robot->seq++, true,
robot->pitch,
robot->yaw );
req_send(robot->client->dev_conn, &req);
robot->dirty_target = TARGET_NONE;
}
if(robot->dirty_colors) {
set_system_led (
&req, robot->seq++, false,

Write Preview
Loading…
Cancel
Save