00001
00002
00003
00004
00005 #ifndef RIC_BOARD_PROTOCOL_H
00006 #define RIC_BOARD_PROTOCOL_H
00007
00008 #ifndef PC_SIDE
00009 #include <Arduino.h>
00010
00011 namespace DeviceMessageType {
00012 enum DeviceMessageType {
00013 BuildDevice = 0,
00014 Ack = 1,
00015 MotorSetPointMsg = 2,
00016 MotorFeedback = 3,
00017 SetMotorParam = 4,
00018 ServoFeedback = 5,
00019 ServoSetPoint = 6,
00020 SwitchFeedBack = 7,
00021 UltrasonicFeedback = 8,
00022 RelySetState = 9,
00023 GpsFeedback = 10,
00024 ImuFeedback = 11,
00025 BatteryFeedback = 12,
00026 SetServoParam = 13,
00027 ImuClibFeedback = 14,
00028 ImuSetClibState = 15,
00029 };
00030 }
00031
00032 namespace DeviceType {
00033 enum DevieType {
00034 Battery = 0,
00035 MotorCloseLoop = 1,
00036 MotorOpenLoop = 2,
00037 Imu = 3,
00038 Gps = 4,
00039 Servo = 5,
00040 Ultrasonic = 6,
00041 Switch = 7,
00042 Realy = 8,
00043
00044 };
00045 namespace CloseMotorType {
00046 enum CloseMotorType {
00047 CloseLoopWithEncoder = 0,
00048 };
00049 }
00050
00051 namespace CloseMotorMode {
00052 enum CloseMotorMode {
00053 Speed = 0,
00054 POSITION = 1,
00055 };
00056 }
00057
00058 namespace DataType {
00059 enum DataType {
00060 DeviceMessage = 0,
00061 Debug = 1,
00062 ConnectionState = 2,
00063 KeepAlive = 3,
00064
00065 };
00066 }
00067 namespace DebugLevel {
00068 enum DebugLevel {
00069 Info = 0,
00070 Warn = 1,
00071 Error = 2,
00072 Fatal = 3,
00073 };
00074 }
00075 namespace ConnectEnum {
00076 enum ConnectEnum {
00077 Connected = 1,
00078 NotReady = 2,
00079 AlreadyConnected = 3,
00080 Disconnected = 4,
00081 AlreadyDisconnected = 5,
00082 };
00083 }
00084
00085 namespace KeepAliveState {
00086 enum KeepAliveState {
00087 Ok = 0,
00088 FatalError = 1,
00089 NeedToRestart = 2,
00090 };
00091 }
00092
00093 #endif
00094 #ifdef PC_SIDE
00095 #include <stdint.h>
00096 typedef uint8_t byte;
00097
00098 namespace DeviceMessageType {
00099 enum DeviceMessageType {
00100 BuildDevice = 0,
00101 Ack = 1,
00102 MotorSetPointMsg = 2,
00103 MotorFeedback = 3,
00104 SetMotorParam = 4,
00105 ServoFeedback = 5,
00106 ServoSetPoint = 6,
00107 SwitchFeedback = 7,
00108 UltrasonicFeedback = 8,
00109 RelySetState = 9,
00110 GpsFeedback = 10,
00111 ImuFeedback = 11,
00112 BatteryFeedback = 12,
00113 SetServoParam = 13,
00114 ImuClibFeedback = 14,
00115 ImuSetClibState = 15,
00116 CloseLoopMotorWithPotentiometerSetParam = 16,
00117 };
00118 }
00119
00120 namespace DeviceType {
00121 enum DeviceType {
00122 Battery = 0,
00123 MotorCloseLoop = 1,
00124 MotorOpenLoop = 2,
00125 Imu = 3,
00126 Gps = 4,
00127 Servo = 5,
00128 Ultrasonic = 6,
00129 Switch = 7,
00130 Relay = 8,
00131 };
00132 }
00133 namespace CloseMotorType {
00134 enum CloseMotorType {
00135 CloseLoopWithEncoder = 0,
00136 CloseLoopWithPotentiometer = 1,
00137 };
00138 }
00139
00140 namespace CloseMotorMode {
00141 enum CloseMotorMode {
00142 Speed = 0,
00143 POSITION = 1,
00144 };
00145 }
00146
00147
00148 namespace DataType {
00149 enum DataType {
00150 DeviceMessage = 0,
00151 Debug = 1,
00152 ConnectionState = 2,
00153 KeepAlive = 3,
00154
00155 };
00156 }
00157 namespace DebugLevel {
00158 enum DebugLevel {
00159 Info = 0,
00160 Warn = 1,
00161 Error = 2,
00162 Fatal = 3,
00163 };
00164 }
00165 namespace ConnectEnum {
00166 enum ConnectEnum {
00167 Connected = 1,
00168 NotReady = 2,
00169 AlreadyConnected = 3,
00170 Disconnected = 4,
00171 AlreadyDisconnected = 5,
00172 };
00173 }
00174
00175 namespace KeepAliveState {
00176 enum KeepAliveState {
00177 Ok = 0,
00178 FatalError = 1,
00179 NeedToRestart = 2,
00180 };
00181 }
00182 #endif
00183
00184 #define HEADER_SIGNAL 0xFF
00185
00186
00187 struct Header {
00188 byte length;
00189 byte dataType;
00190 uint16_t checkSum;
00191 }__attribute__((__packed__));
00192
00193 struct DebugMsg : Header {
00194 DebugMsg() {
00195 dataType = DataType::Debug;
00196 }
00197 byte level;
00198 char message[110];
00199 }__attribute__((__packed__));
00200
00201 struct ConnectState : Header{
00202 ConnectState() {
00203 dataType = DataType::ConnectionState;
00204 }
00205 byte state;
00206 byte version;
00207 }__attribute__((__packed__));
00208
00209 struct KeepAliveMsg : Header {
00210 KeepAliveMsg() {
00211 dataType = DataType::KeepAlive;
00212 }
00213
00214 byte state;
00215 }__attribute__((__packed__));
00216
00217 struct DeviceMessage : Header {
00218 DeviceMessage() {
00219 dataType = DataType::DeviceMessage;
00220 }
00221
00222 byte id;
00223 byte deviceMessageType;
00224 }__attribute__((__packed__));
00225
00226
00227 struct DeviceAck : DeviceMessage {
00228 DeviceAck() : DeviceMessage() {
00229 deviceMessageType = DeviceMessageType::Ack;
00230 }
00231
00232 byte ackId;
00233 }__attribute__((__packed__));
00234
00235 struct BuildDevice : DeviceMessage {
00236 BuildDevice() : DeviceMessage() {
00237 deviceMessageType = DeviceMessageType::BuildDevice;
00238 }
00239 byte deviceType;
00240 }__attribute__((__packed__));
00241
00242 struct BuildBattery : BuildDevice {
00243 BuildBattery() {
00244 deviceType = DeviceType::Battery;
00245 }
00246 byte pin;
00247 }__attribute__((__packed__));
00248
00249 struct BuildUltrasonic : BuildDevice {
00250 BuildUltrasonic() {
00251 deviceType = DeviceType::Ultrasonic;
00252 }
00253 byte pin;
00254 }__attribute__((__packed__));
00255
00256 struct BuildGps : BuildDevice {
00257 BuildGps() {
00258 deviceType = DeviceType::Gps;
00259 }
00260 unsigned int baudrate;
00261 }__attribute__((__packed__));
00262
00263 struct BuildImu : BuildDevice {
00264 BuildImu() {
00265 deviceType = DeviceType::Imu;
00266 }
00267 uint16_t fusionHz;
00268 bool enableGyro;
00269 bool fuseCompass;
00270 }__attribute__((__packed__));
00271
00272 struct BuildSwitch : BuildDevice {
00273 BuildSwitch() {
00274 deviceType = DeviceType::Switch;
00275 }
00276 byte pin;
00277 };
00278
00279 struct BuildRelay : BuildDevice {
00280 BuildRelay() {
00281 deviceType = DeviceType::Relay;
00282 }
00283 byte pin;
00284 };
00285
00286 struct BuildMotorCloseLoop : BuildDevice {
00287 BuildMotorCloseLoop() {
00288 deviceType = DeviceType::MotorCloseLoop;
00289 }
00290 byte motorAddress;
00291 byte motorMode;
00292 byte eSwitchPin;
00293 byte eSwitchType;
00294 byte motorType;
00295 uint16_t LPFHzSpeed;
00296 uint16_t LPFHzInput;
00297 uint16_t PIDHz;
00298 uint16_t PPR;
00299 uint16_t timeout;
00300 int8_t motorDirection;
00301 int8_t encoderDirection;
00302 float LPFAlphaSpeed;
00303 float LPFAlphaInput;
00304 float KP;
00305 float KI;
00306 float KD;
00307 float KFF;
00308 float maxSetPointSpeed;
00309 float minSetPointSpeed;
00310 float maxSetPointPos;
00311 float minSetPointPos;
00312 float limit;
00313 float stopLimitMax;
00314 float stopLimitMin;
00315 int16_t baisMin;
00316 int16_t baisMax;
00317
00318 }__attribute__((__packed__));
00319
00320 struct BuildMotorCloseLoopWithEncoder : BuildMotorCloseLoop {
00321 BuildMotorCloseLoopWithEncoder() {
00322 motorType = CloseMotorType::CloseLoopWithEncoder;
00323 }
00324 byte encoderPinA;
00325 byte encoderPinB;
00326 }__attribute__((__packed__));
00327
00328 struct BuildCloseLoopWithPotentiometer : BuildMotorCloseLoop {
00329 BuildCloseLoopWithPotentiometer() {
00330 motorType = CloseMotorType::CloseLoopWithPotentiometer;
00331 }
00332 byte pin;
00333 uint16_t toleranceTime;
00334 float k;
00335 float a;
00336 float b;
00337 float tolerance;
00338 }__attribute__((__packed__));
00339
00340 struct BuildMotorOpenLoop : BuildDevice {
00341 BuildMotorOpenLoop() {
00342 deviceType = DeviceType::MotorOpenLoop;
00343 }
00344 byte motorAddress;
00345 byte eSwitchPin;
00346 byte eSwitchType;
00347 byte encoderPinA;
00348 byte encoderPinB;
00349 uint16_t LPFHzSpeed;
00350 uint16_t LPFHzInput;
00351 uint16_t PPR;
00352 int8_t motorDirection;
00353 int8_t encoderDirection;
00354 float maxSetPointSpeed;
00355 float minSetPointSpeed;
00356 float LPFAlphaSpeed;
00357 float LPFAlphaInput;
00358
00359 }__attribute__((__packed__));
00360
00361 struct BuildServo : BuildDevice {
00362 BuildServo() {
00363 deviceType = DeviceType::Servo;
00364 }
00365 byte pin;
00366 float a;
00367 float b;
00368 float max;
00369 float min;
00370 }__attribute__((__packed__));
00371
00372 struct ServoFeedback : DeviceMessage {
00373 ServoFeedback() {
00374 deviceMessageType = DeviceMessageType::ServoFeedback;
00375 }
00376 float pos;
00377 }__attribute__((__packed__));
00378
00379 struct SwitchFeedback : DeviceMessage {
00380 SwitchFeedback() {
00381 deviceMessageType = DeviceMessageType::SwitchFeedback;
00382 }
00383 bool state;
00384 }__attribute__((__packed__));
00385
00386 struct GpsFeedback : DeviceMessage {
00387 GpsFeedback() {
00388 deviceMessageType = DeviceMessageType::GpsFeedback;
00389 }
00390 float lat;
00391 float lng;
00392 float meters;
00393 int16_t HDOP;
00394 int16_t satellites;
00395 int8_t fix;
00396 }__attribute__((__packed__));
00397
00398 struct BatteryFeedback : DeviceMessage {
00399 BatteryFeedback() {
00400 deviceMessageType = DeviceMessageType::BatteryFeedback;
00401 }
00402 uint16_t currentRead;
00403 }__attribute__((__packed__));
00404
00405 struct ImuFeedback : DeviceMessage {
00406 ImuFeedback() {
00407 deviceMessageType = DeviceMessageType::ImuFeedback;
00408 }
00409 float velocityX;
00410 float velocityY;
00411 float velocityZ;
00412
00413 float accelerationX;
00414 float accelerationY;
00415 float accelerationZ;
00416
00417 float magnetometerX;
00418 float magnetometerY;
00419 float magnetometerZ;
00420
00421 float orientationX;
00422 float orientationY;
00423 float orientationZ;
00424 float orientationW;
00425
00426 }__attribute__((__packed__));
00427
00428 struct ImuClibFeedback : DeviceMessage {
00429 ImuClibFeedback() {
00430 deviceMessageType = DeviceMessageType::ImuClibFeedback;
00431 }
00432
00433 float max[3];
00434 float min[3];
00435
00436 }__attribute__((__packed__));
00437
00438 struct UltrasonicFeedback : DeviceMessage {
00439 UltrasonicFeedback() {
00440 deviceMessageType = DeviceMessageType::UltrasonicFeedback;
00441 }
00442 uint16_t currentRead;
00443 }__attribute__((__packed__));
00444
00445 struct MotorFeedback : DeviceMessage {
00446 MotorFeedback() {
00447 deviceMessageType = DeviceMessageType::MotorFeedback;
00448 }
00449 float rad;
00450 float rad_s;
00451 }__attribute__((__packed__));
00452
00453 struct ServoSetPoint : DeviceMessage {
00454 ServoSetPoint() {
00455 deviceMessageType = DeviceMessageType::ServoSetPoint;
00456 }
00457 float pos;
00458 }__attribute__((__packed__));
00459
00460 struct MotorSetPoint : DeviceMessage {
00461 MotorSetPoint() {
00462 deviceMessageType = DeviceMessageType::MotorSetPointMsg;
00463 }
00464 float point;
00465 }__attribute__((__packed__));
00466
00467 struct RelaySetState : DeviceMessage {
00468 RelaySetState() {
00469 deviceMessageType = DeviceMessageType::RelySetState;
00470 }
00471 bool state;
00472 }__attribute__((__packed__));
00473
00474 struct SetMotorParam : DeviceMessage{
00475 SetMotorParam() {
00476 deviceMessageType = DeviceMessageType::SetMotorParam;
00477 }
00478
00479 uint16_t speedLpfHz;
00480 uint16_t inputLpfHz;
00481 uint16_t pidHz;
00482 float speedLfpAlpha;
00483 float inputLfpAlpha;
00484 float limit;
00485 float KP;
00486 float KI;
00487 float KD;
00488 float KFF;
00489
00490 }__attribute__((__packed__));
00491
00492 struct SetCloseMotorWithPotentiometer : DeviceMessage {
00493 SetCloseMotorWithPotentiometer() {
00494 deviceMessageType = DeviceMessageType::CloseLoopMotorWithPotentiometerSetParam;
00495 }
00496
00497 uint16_t speedLpfHz;
00498 uint16_t inputLpfHz;
00499 uint16_t pidHz;
00500 uint16_t toleranceTime;
00501 float speedLfpAlpha;
00502 float inputLfpAlpha;
00503 float limit;
00504 float KP;
00505 float KI;
00506 float KD;
00507 float KFF;
00508 float K;
00509 float a;
00510 float b;
00511 float tolerance;
00512
00513
00514 }__attribute__((__packed__));
00515
00516 struct SetServoParam : DeviceMessage {
00517 SetServoParam() {
00518 deviceMessageType = DeviceMessageType::SetServoParam;
00519 }
00520 float a;
00521 float b;
00522 float max;
00523 float min;
00524 }__attribute__((__packed__));
00525
00526 struct ImuSetClibState : DeviceMessage {
00527 ImuSetClibState(){
00528 deviceMessageType = DeviceMessageType::ImuSetClibState;
00529 }
00530
00531 byte state;
00532
00533 }__attribute__((__packed__));
00534
00535
00536
00537 #endif //RIC_BOARD_PROTOCOL_H