All mbed code for control over dive planes, pump motor, valve motor, BCUs, UART interface, etc.
Dependencies: mbed ESC mbed MODDMA
robotic_fish_6/ROSControl/ROSController.h@0:c3a329a5b05d, 2020-01-14 (annotated)
- Committer:
- juansal12
- Date:
- Tue Jan 14 19:17:05 2020 +0000
- Revision:
- 0:c3a329a5b05d
Sofi7 mbed code;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
juansal12 | 0:c3a329a5b05d | 1 | /* |
juansal12 | 0:c3a329a5b05d | 2 | * ROSController.h |
juansal12 | 0:c3a329a5b05d | 3 | * |
juansal12 | 0:c3a329a5b05d | 4 | * Author: Joseph DelPreto |
juansal12 | 0:c3a329a5b05d | 5 | */ |
juansal12 | 0:c3a329a5b05d | 6 | |
juansal12 | 0:c3a329a5b05d | 7 | //#define rosControl |
juansal12 | 0:c3a329a5b05d | 8 | |
juansal12 | 0:c3a329a5b05d | 9 | #ifdef rosControl |
juansal12 | 0:c3a329a5b05d | 10 | |
juansal12 | 0:c3a329a5b05d | 11 | #ifndef ROSCONTROL_ROSCONTROLLER_H_ |
juansal12 | 0:c3a329a5b05d | 12 | #define ROSCONTROL_ROSCONTROLLER_H_ |
juansal12 | 0:c3a329a5b05d | 13 | |
juansal12 | 0:c3a329a5b05d | 14 | #include "FishController.h" |
juansal12 | 0:c3a329a5b05d | 15 | #include "mbed.h" |
juansal12 | 0:c3a329a5b05d | 16 | #include <ros.h> |
juansal12 | 0:c3a329a5b05d | 17 | #include <std_msgs/Empty.h> |
juansal12 | 0:c3a329a5b05d | 18 | #include <fish_msgs/JoystickState.h> |
juansal12 | 0:c3a329a5b05d | 19 | |
juansal12 | 0:c3a329a5b05d | 20 | // ROS setup |
juansal12 | 0:c3a329a5b05d | 21 | // TODO replace this with your topic name |
juansal12 | 0:c3a329a5b05d | 22 | #define rosTopicName "to_serial" |
juansal12 | 0:c3a329a5b05d | 23 | |
juansal12 | 0:c3a329a5b05d | 24 | // Pins and comm |
juansal12 | 0:c3a329a5b05d | 25 | #define rosDefaultBaudUSB 115200 |
juansal12 | 0:c3a329a5b05d | 26 | #define rosDefaultBaud 115200 |
juansal12 | 0:c3a329a5b05d | 27 | #define rosDefaultTX p13 |
juansal12 | 0:c3a329a5b05d | 28 | #define rosDefaultRX p14 |
juansal12 | 0:c3a329a5b05d | 29 | // note lowBatteryVoltagePin is defined in FishController |
juansal12 | 0:c3a329a5b05d | 30 | |
juansal12 | 0:c3a329a5b05d | 31 | //#define printStatusROSController // whether to print what's going on (i.e. when it gets commands, etc.) |
juansal12 | 0:c3a329a5b05d | 32 | #define debugLEDsROS // LED1: initialized LED2: running LED3: receiving a message LED4: done (others turn off) |
juansal12 | 0:c3a329a5b05d | 33 | #define runTimeROS 10000 // how long to run for (in milliseconds) if inifiniteLoopROS is undefined |
juansal12 | 0:c3a329a5b05d | 34 | #define infiniteLoopROS // if defined, will run forever (or until stop() is called from another thread) |
juansal12 | 0:c3a329a5b05d | 35 | |
juansal12 | 0:c3a329a5b05d | 36 | #define rosControllerControlFish // whether to start fishController to control the servos and motor |
juansal12 | 0:c3a329a5b05d | 37 | |
juansal12 | 0:c3a329a5b05d | 38 | // Map bytes sent over ROS serial to ranges for each fish property |
juansal12 | 0:c3a329a5b05d | 39 | #define rosMinPitch fishMinPitch |
juansal12 | 0:c3a329a5b05d | 40 | #define rosMaxPitch fishMaxPitch |
juansal12 | 0:c3a329a5b05d | 41 | #define rosMinYaw fishMinYaw |
juansal12 | 0:c3a329a5b05d | 42 | #define rosMaxYaw fishMaxYaw |
juansal12 | 0:c3a329a5b05d | 43 | #define rosMinThrust fishMinThrust |
juansal12 | 0:c3a329a5b05d | 44 | #define rosMaxThrust fishMaxThrust |
juansal12 | 0:c3a329a5b05d | 45 | #define rosMinFrequency fishMinFrequency |
juansal12 | 0:c3a329a5b05d | 46 | #define rosMaxFrequency fishMaxFrequency |
juansal12 | 0:c3a329a5b05d | 47 | |
juansal12 | 0:c3a329a5b05d | 48 | class ROSController |
juansal12 | 0:c3a329a5b05d | 49 | { |
juansal12 | 0:c3a329a5b05d | 50 | public: |
juansal12 | 0:c3a329a5b05d | 51 | // Initialization |
juansal12 | 0:c3a329a5b05d | 52 | ROSController(Serial* serialObject = NULL, Serial* usbSerialObject = NULL); // if objects are null, ones will be created |
juansal12 | 0:c3a329a5b05d | 53 | void init(Serial* serialObject = NULL, Serial* usbSerialObject = NULL); // if serial objects are null, ones will be created |
juansal12 | 0:c3a329a5b05d | 54 | // Execution control |
juansal12 | 0:c3a329a5b05d | 55 | void run(); |
juansal12 | 0:c3a329a5b05d | 56 | void stop(); |
juansal12 | 0:c3a329a5b05d | 57 | void lowBatteryCallback(); |
juansal12 | 0:c3a329a5b05d | 58 | // ROS |
juansal12 | 0:c3a329a5b05d | 59 | ros::NodeHandle nodeHandle; |
juansal12 | 0:c3a329a5b05d | 60 | ros::Subscriber<fish_msgs::JoystickState> subscriber; |
juansal12 | 0:c3a329a5b05d | 61 | void processROSMessage(const fish_msgs::JoystickState& msg); |
juansal12 | 0:c3a329a5b05d | 62 | private: |
juansal12 | 0:c3a329a5b05d | 63 | Timer programTimer; |
juansal12 | 0:c3a329a5b05d | 64 | bool terminated; |
juansal12 | 0:c3a329a5b05d | 65 | Serial* usbSerial; |
juansal12 | 0:c3a329a5b05d | 66 | Serial* serial; |
juansal12 | 0:c3a329a5b05d | 67 | |
juansal12 | 0:c3a329a5b05d | 68 | // Low battery monitor |
juansal12 | 0:c3a329a5b05d | 69 | DigitalIn* lowBatteryVoltageInput; |
juansal12 | 0:c3a329a5b05d | 70 | Ticker lowBatteryTicker; |
juansal12 | 0:c3a329a5b05d | 71 | bool detectedLowBattery; |
juansal12 | 0:c3a329a5b05d | 72 | |
juansal12 | 0:c3a329a5b05d | 73 | // Debug LEDs |
juansal12 | 0:c3a329a5b05d | 74 | #ifdef debugLEDsROS |
juansal12 | 0:c3a329a5b05d | 75 | DigitalOut* rosLEDs[4]; |
juansal12 | 0:c3a329a5b05d | 76 | #endif |
juansal12 | 0:c3a329a5b05d | 77 | }; |
juansal12 | 0:c3a329a5b05d | 78 | |
juansal12 | 0:c3a329a5b05d | 79 | // Create a static instance of ROSController to be used by anyone doing ROS control |
juansal12 | 0:c3a329a5b05d | 80 | extern ROSController rosController; |
juansal12 | 0:c3a329a5b05d | 81 | |
juansal12 | 0:c3a329a5b05d | 82 | #endif /* ROSCONTROL_ROSCONTROLLER_H_ */ |
juansal12 | 0:c3a329a5b05d | 83 | |
juansal12 | 0:c3a329a5b05d | 84 | #endif // #ifdef rosControl |