diff --git a/include/gimbal.h b/include/gimbal.h index 73e9220..27f177e 100644 --- a/include/gimbal.h +++ b/include/gimbal.h @@ -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 diff --git a/include/message.h b/include/message.h index 8e42ee3..a08d3cb 100644 --- a/include/message.h +++ b/include/message.h @@ -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; diff --git a/include/roboeasy.h b/include/roboeasy.h index 0c80dc6..36dc131 100644 --- a/include/roboeasy.h +++ b/include/roboeasy.h @@ -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. diff --git a/src/connection.c b/src/connection.c index 10f81a7..9ba4d58 100644 --- a/src/connection.c +++ b/src/connection.c @@ -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); diff --git a/src/modules/gimbal.c b/src/modules/gimbal.c index e949080..d605526 100644 --- a/src/modules/gimbal.c +++ b/src/modules/gimbal.c @@ -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); +} diff --git a/src/robo.c b/src/robo.c index 488d2f3..f12b678 100644 --- a/src/robo.c +++ b/src/robo.c @@ -5,6 +5,7 @@ #include #include #include +#include #include @@ -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,