BLUESINK / Mbed 2 deprecated s-rov-firmware

S-ROV main board firmware

Dependencies:   mbed pca9685 ros_lib_kinetic ads1015

main.cpp@2:666847c29d89, 2017-01-16 (annotated)

Committer:
YJ_Kim
Date:
Mon Jan 16 13:01:15 2017 +0000
Revision:
2:666847c29d89
Parent:
1:2062e7a67afd
Child:
3:2c5258907150
Import rosserial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YJ_Kim 0:75ecba0f12ed 1 #include "mbed.h"
YJ_Kim 0:75ecba0f12ed 2 #include <ros.h>
YJ_Kim 2:666847c29d89 3 #include <s_rov_msgs/s_rov_control_extra.h>
YJ_Kim 2:666847c29d89 4 #include <s_rov_msgs/s_rov_control_esc.h>
YJ_Kim 2:666847c29d89 5 #include <s_rov_msgs/s_rov_control_led.h>
YJ_Kim 0:75ecba0f12ed 6 #include <pca9685/pca9685.h>
YJ_Kim 0:75ecba0f12ed 7
YJ_Kim 2:666847c29d89 8 #define CH_LED1 2
YJ_Kim 2:666847c29d89 9 #define CH_LED2 3
YJ_Kim 2:666847c29d89 10 #define CH_ESC1 4
YJ_Kim 2:666847c29d89 11 #define CH_ESC2 5
YJ_Kim 2:666847c29d89 12 #define CH_ESC3 6
YJ_Kim 2:666847c29d89 13 #define CH_ESC4 7
YJ_Kim 2:666847c29d89 14 #define CH_1 11
YJ_Kim 2:666847c29d89 15 #define CH_2 10
YJ_Kim 2:666847c29d89 16 #define CH_3 9
YJ_Kim 2:666847c29d89 17 #define CH_4 8
YJ_Kim 2:666847c29d89 18 #define CH_5 0
YJ_Kim 2:666847c29d89 19 #define CH_6 1
YJ_Kim 2:666847c29d89 20
YJ_Kim 2:666847c29d89 21 #define ESC_SWITCH PB_1
YJ_Kim 2:666847c29d89 22
YJ_Kim 2:666847c29d89 23
YJ_Kim 2:666847c29d89 24 /**************************** System Configuration ***************************/
YJ_Kim 2:666847c29d89 25
YJ_Kim 2:666847c29d89 26 I2C i2c(PB_11, PB_10); // sda, scl
YJ_Kim 2:666847c29d89 27 PCA9685 pwm(0x40, i2c, 50); // Address, i2c object, frequency
YJ_Kim 2:666847c29d89 28 DigitalOut ESC_switch(ESC_SWITCH);
YJ_Kim 2:666847c29d89 29
YJ_Kim 2:666847c29d89 30 /**************************** System Configuration ***************************/
YJ_Kim 2:666847c29d89 31
YJ_Kim 2:666847c29d89 32 /******************************ROS Configuration *****************************/
YJ_Kim 2:666847c29d89 33
YJ_Kim 1:2062e7a67afd 34 class NewHardware : public MbedHardware{
YJ_Kim 1:2062e7a67afd 35 public:
YJ_Kim 1:2062e7a67afd 36 NewHardware():MbedHardware(PA_9, PA_10, 57600){};
YJ_Kim 1:2062e7a67afd 37 };
YJ_Kim 1:2062e7a67afd 38 ros::NodeHandle_<NewHardware> nh;
YJ_Kim 1:2062e7a67afd 39
YJ_Kim 2:666847c29d89 40 void messageExtra(const s_rov_msgs::s_rov_control_extra& control_msg){
YJ_Kim 2:666847c29d89 41 pwm.set_servo_angle(CH_1, control_msg.extraPWM[0]);
YJ_Kim 2:666847c29d89 42 pwm.set_servo_angle(CH_2, control_msg.extraPWM[1]);
YJ_Kim 2:666847c29d89 43 pwm.set_servo_angle(CH_3, control_msg.extraPWM[2]);
YJ_Kim 2:666847c29d89 44 pwm.set_servo_angle(CH_4, control_msg.extraPWM[3]);
YJ_Kim 2:666847c29d89 45 pwm.set_servo_angle(CH_5, control_msg.extraPWM[4]);
YJ_Kim 2:666847c29d89 46 pwm.set_servo_angle(CH_6, control_msg.extraPWM[5]);
YJ_Kim 0:75ecba0f12ed 47 }
YJ_Kim 0:75ecba0f12ed 48
YJ_Kim 2:666847c29d89 49 void messageESC(const s_rov_msgs::s_rov_control_esc& control_msg){
YJ_Kim 2:666847c29d89 50 pwm.set_servo_angle(CH_ESC1, control_msg.escPWM[0]);
YJ_Kim 2:666847c29d89 51 pwm.set_servo_angle(CH_ESC2, control_msg.escPWM[1]);
YJ_Kim 2:666847c29d89 52 pwm.set_servo_angle(CH_ESC3, control_msg.escPWM[2]);
YJ_Kim 2:666847c29d89 53 pwm.set_servo_angle(CH_ESC4, control_msg.escPWM[3]);
YJ_Kim 2:666847c29d89 54 ESC_switch = control_msg.escSwitch;
YJ_Kim 2:666847c29d89 55 }
YJ_Kim 2:666847c29d89 56
YJ_Kim 2:666847c29d89 57 void messageLed(const s_rov_msgs::s_rov_control_led& control_msg){
YJ_Kim 2:666847c29d89 58 pwm.set_pwm_duty(CH_LED1, control_msg.ledPWM[0]);
YJ_Kim 2:666847c29d89 59 pwm.set_pwm_duty(CH_LED2, control_msg.ledPWM[1]);
YJ_Kim 2:666847c29d89 60 }
YJ_Kim 2:666847c29d89 61
YJ_Kim 2:666847c29d89 62 ros::Subscriber<s_rov_msgs::s_rov_control_extra> sub_extra_control("/s_rov/control/extra", &messageExtra);
YJ_Kim 2:666847c29d89 63 ros::Subscriber<s_rov_msgs::s_rov_control_esc> sub_esc_control("/s_rov/control/esc", &messageESC);
YJ_Kim 2:666847c29d89 64 ros::Subscriber<s_rov_msgs::s_rov_control_led> sub_led_control("/s_rov/control/led", &messageLed);
YJ_Kim 2:666847c29d89 65
YJ_Kim 2:666847c29d89 66 /******************************ROS Configuration *****************************/
YJ_Kim 0:75ecba0f12ed 67
YJ_Kim 0:75ecba0f12ed 68 int main() {
YJ_Kim 0:75ecba0f12ed 69 nh.initNode();
YJ_Kim 2:666847c29d89 70 nh.subscribe(sub_extra_control);
YJ_Kim 2:666847c29d89 71 nh.subscribe(sub_esc_control);
YJ_Kim 2:666847c29d89 72 nh.subscribe(sub_led_control);
YJ_Kim 2:666847c29d89 73
YJ_Kim 2:666847c29d89 74 ESC_switch = false;
YJ_Kim 0:75ecba0f12ed 75 pwm.init();
YJ_Kim 0:75ecba0f12ed 76
YJ_Kim 0:75ecba0f12ed 77 while (1) {
YJ_Kim 0:75ecba0f12ed 78 nh.spinOnce();
YJ_Kim 0:75ecba0f12ed 79 wait_ms(1);
YJ_Kim 0:75ecba0f12ed 80 }
YJ_Kim 0:75ecba0f12ed 81 }