From 90552382dbb1721d4ed55df970e8ee2ab67ac348 Mon Sep 17 00:00:00 2001 From: PgSocks Date: Tue, 23 May 2023 10:15:34 -0500 Subject: [PATCH] Add blaster fire message --- CMakeLists.txt | 1 + include/blaster.h | 33 +++++++++++++++++++++++++++++++++ include/message.h | 17 +++++++++++++++++ include/roboeasy.h | 2 ++ include/robomaster.h | 1 + src/connection.c | 4 ++++ src/modules/blaster.c | 16 ++++++++++++++++ src/robo.c | 19 +++++++++++++++++++ src/sdl.c | 3 +++ 9 files changed, 96 insertions(+) create mode 100644 include/blaster.h create mode 100644 src/modules/blaster.c diff --git a/CMakeLists.txt b/CMakeLists.txt index f33f2db..58079b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ add_library(robomaster src/modules/sdk.c src/modules/chassis.c src/modules/gimbal.c + src/modules/blaster.c src/connection.c src/robomaster.c src/robo.c diff --git a/include/blaster.h b/include/blaster.h new file mode 100644 index 0000000..89bf7cf --- /dev/null +++ b/include/blaster.h @@ -0,0 +1,33 @@ +#pragma once + +#include "message.h" + +#include +#include + +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 ); + diff --git a/include/message.h b/include/message.h index bcd0b76..945e249 100644 --- a/include/message.h +++ b/include/message.h @@ -248,6 +248,21 @@ struct PACKED SetSdkConnectionResp { 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; +}; + union Request { struct Header header; struct SetSdkConnectionReq sdkconn; @@ -261,6 +276,7 @@ union Request { struct SubNodeResetReq subnodereset; struct SubscribeAddNodeReq subnodeadd; struct GimbalCtrlSpeedReq gimbspeed; + struct BlasterFireReq blaster; }; union Response { struct Header header; @@ -275,6 +291,7 @@ union Response { struct SubNodeResetResp subnodereset; struct SubscribeAddNodeResp subnodeadd; struct GimbalCtrlSpeedResp gimbspeed; + struct BlasterFireResp blaster; }; union Message { struct Header header; diff --git a/include/roboeasy.h b/include/roboeasy.h index 4377133..71ee181 100644 --- a/include/roboeasy.h +++ b/include/roboeasy.h @@ -65,6 +65,8 @@ 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_blast(Robot); + /* * 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 diff --git a/include/robomaster.h b/include/robomaster.h index 6bf7bf9..408fc15 100644 --- a/include/robomaster.h +++ b/include/robomaster.h @@ -8,6 +8,7 @@ typedef struct Client* Client; #include "chassis.h" #include "sdk.h" #include "gimbal.h" +#include "blaster.h" Client client_new(); void client_connect(Client client); diff --git a/src/connection.c b/src/connection.c index 6d9d2f2..a3177a9 100644 --- a/src/connection.c +++ b/src/connection.c @@ -37,6 +37,8 @@ message_length(int cmd) { return sizeof(struct ChassisSpeedModeReq); case GIMBAL_CTRL_SPEED_CMD: return sizeof(struct GimbalCtrlSpeedReq); + case BLASTER_FIRE_CMD: + return sizeof(struct BlasterFireReq); default: return 0; } @@ -60,6 +62,8 @@ message_module(int cmd) { 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); default: return 0; } diff --git a/src/modules/blaster.c b/src/modules/blaster.c new file mode 100644 index 0000000..d05d91e --- /dev/null +++ b/src/modules/blaster.c @@ -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); +} + diff --git a/src/robo.c b/src/robo.c index ef2d115..43a92ca 100644 --- a/src/robo.c +++ b/src/robo.c @@ -5,6 +5,8 @@ #include #include +#include + struct RobotImp { struct Client* client; @@ -19,6 +21,8 @@ struct RobotImp { int16_t gimbal[2]; bool dirty_gimbal; + bool dirty_blaster; + bool sdk_mode; enum { @@ -37,6 +41,12 @@ struct RobotImp { }; +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 @@ -185,6 +195,15 @@ robot_work(Robot robot) { 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; + } break; } diff --git a/src/sdl.c b/src/sdl.c index 2c70a84..58e77b9 100644 --- a/src/sdl.c +++ b/src/sdl.c @@ -90,6 +90,9 @@ int main(int argc, char* argv[]) { robot_led(robot, colors[color].r, colors[color].g, colors[color].b); color = (color + 1) % 3; break; + case SDL_SCANCODE_RETURN: + robot_blast(robot); + break; default: break; } break;