BLUESINK / Mbed 2 deprecated s-rov-firmware

S-ROV main board firmware

Dependencies:   mbed pca9685 ros_lib_kinetic ads1015

main.cpp@4:62c92aa48842, 2017-01-26 (annotated)

Committer:
YJ_Kim
Date:
Thu Jan 26 18:02:42 2017 +0000
Revision:
4:62c92aa48842
Parent:
3:2c5258907150 
add ads1015 library

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 4:62c92aa48842 3
YJ_Kim 2:666847c29d89 4 #include <s_rov_msgs/s_rov_control_extra.h>
YJ_Kim 2:666847c29d89 5 #include <s_rov_msgs/s_rov_control_esc.h>
YJ_Kim 2:666847c29d89 6 #include <s_rov_msgs/s_rov_control_led.h>
YJ_Kim 4:62c92aa48842 7 #include <s_rov_msgs/s_rov_state.h>
YJ_Kim 4:62c92aa48842 8
YJ_Kim 0:75ecba0f12ed 9 #include <pca9685/pca9685.h>
YJ_Kim 4:62c92aa48842 10 #include <ads1015/ads1015.h>
YJ_Kim 0:75ecba0f12ed 11
YJ_Kim 3:2c5258907150 12 #define CH_LED1 9
YJ_Kim 3:2c5258907150 13 #define CH_LED2 8
YJ_Kim 3:2c5258907150 14 #define CH_ESC1 10
YJ_Kim 3:2c5258907150 15 #define CH_ESC2 11
YJ_Kim 3:2c5258907150 16 #define CH_ESC3 7
YJ_Kim 3:2c5258907150 17 #define CH_ESC4 6
YJ_Kim 3:2c5258907150 18 #define CH_1 0
YJ_Kim 3:2c5258907150 19 #define CH_2 1
YJ_Kim 3:2c5258907150 20 #define CH_3 2
YJ_Kim 3:2c5258907150 21 #define CH_4 3
YJ_Kim 3:2c5258907150 22 #define CH_5 4
YJ_Kim 3:2c5258907150 23 #define CH_6 5
YJ_Kim 2:666847c29d89 24
YJ_Kim 2:666847c29d89 25 #define ESC_SWITCH PB_1
YJ_Kim 2:666847c29d89 26
YJ_Kim 2:666847c29d89 27
YJ_Kim 2:666847c29d89 28 /**************************** System Configuration ***************************/
YJ_Kim 2:666847c29d89 29
YJ_Kim 4:62c92aa48842 30 Timer t;
YJ_Kim 2:666847c29d89 31 I2C i2c(PB_11, PB_10); // sda, scl
YJ_Kim 4:62c92aa48842 32 PCA9685 pwm(0x40, &i2c, 50); // Address, i2c pointer, frequency
YJ_Kim 4:62c92aa48842 33 ADS1015 adc(0x48, &i2c, ADS1015_GAIN_TWOTHRIDS); // Address, i2c pointer, gain
YJ_Kim 2:666847c29d89 34 DigitalOut ESC_switch(ESC_SWITCH);
YJ_Kim 2:666847c29d89 35
YJ_Kim 2:666847c29d89 36 /**************************** System Configuration ***************************/
YJ_Kim 2:666847c29d89 37
YJ_Kim 2:666847c29d89 38 /******************************ROS Configuration *****************************/
YJ_Kim 2:666847c29d89 39
YJ_Kim 1:2062e7a67afd 40 class NewHardware : public MbedHardware{
YJ_Kim 1:2062e7a67afd 41 public:
YJ_Kim 1:2062e7a67afd 42 NewHardware():MbedHardware(PA_9, PA_10, 57600){};
YJ_Kim 1:2062e7a67afd 43 };
YJ_Kim 1:2062e7a67afd 44 ros::NodeHandle_<NewHardware> nh;
YJ_Kim 1:2062e7a67afd 45
YJ_Kim 2:666847c29d89 46 void messageExtra(const s_rov_msgs::s_rov_control_extra& control_msg){
YJ_Kim 2:666847c29d89 47 pwm.set_servo_angle(CH_1, control_msg.extraPWM[0]);
YJ_Kim 2:666847c29d89 48 pwm.set_servo_angle(CH_2, control_msg.extraPWM[1]);
YJ_Kim 2:666847c29d89 49 pwm.set_servo_angle(CH_3, control_msg.extraPWM[2]);
YJ_Kim 2:666847c29d89 50 pwm.set_servo_angle(CH_4, control_msg.extraPWM[3]);
YJ_Kim 2:666847c29d89 51 pwm.set_servo_angle(CH_5, control_msg.extraPWM[4]);
YJ_Kim 2:666847c29d89 52 pwm.set_servo_angle(CH_6, control_msg.extraPWM[5]);
YJ_Kim 0:75ecba0f12ed 53 }
YJ_Kim 0:75ecba0f12ed 54
YJ_Kim 2:666847c29d89 55 void messageESC(const s_rov_msgs::s_rov_control_esc& control_msg){
YJ_Kim 2:666847c29d89 56 pwm.set_servo_angle(CH_ESC1, control_msg.escPWM[0]);
YJ_Kim 2:666847c29d89 57 pwm.set_servo_angle(CH_ESC2, control_msg.escPWM[1]);
YJ_Kim 2:666847c29d89 58 pwm.set_servo_angle(CH_ESC3, control_msg.escPWM[2]);
YJ_Kim 2:666847c29d89 59 pwm.set_servo_angle(CH_ESC4, control_msg.escPWM[3]);
YJ_Kim 2:666847c29d89 60 ESC_switch = control_msg.escSwitch;
YJ_Kim 2:666847c29d89 61 }
YJ_Kim 2:666847c29d89 62
YJ_Kim 2:666847c29d89 63 void messageLed(const s_rov_msgs::s_rov_control_led& control_msg){
YJ_Kim 2:666847c29d89 64 pwm.set_pwm_duty(CH_LED1, control_msg.ledPWM[0]);
YJ_Kim 2:666847c29d89 65 pwm.set_pwm_duty(CH_LED2, control_msg.ledPWM[1]);
YJ_Kim 2:666847c29d89 66 }
YJ_Kim 2:666847c29d89 67
YJ_Kim 2:666847c29d89 68 ros::Subscriber<s_rov_msgs::s_rov_control_extra> sub_extra_control("/s_rov/control/extra", &messageExtra);
YJ_Kim 2:666847c29d89 69 ros::Subscriber<s_rov_msgs::s_rov_control_esc> sub_esc_control("/s_rov/control/esc", &messageESC);
YJ_Kim 2:666847c29d89 70 ros::Subscriber<s_rov_msgs::s_rov_control_led> sub_led_control("/s_rov/control/led", &messageLed);
YJ_Kim 2:666847c29d89 71
YJ_Kim 4:62c92aa48842 72 s_rov_msgs::s_rov_state state_msg;
YJ_Kim 4:62c92aa48842 73 ros::Publisher pub_state("/s_rov/state", &state_msg);
YJ_Kim 4:62c92aa48842 74
YJ_Kim 2:666847c29d89 75 /******************************ROS Configuration *****************************/
YJ_Kim 0:75ecba0f12ed 76
YJ_Kim 0:75ecba0f12ed 77 int main() {
YJ_Kim 0:75ecba0f12ed 78 nh.initNode();
YJ_Kim 4:62c92aa48842 79 // ros subscriber
YJ_Kim 2:666847c29d89 80 nh.subscribe(sub_extra_control);
YJ_Kim 2:666847c29d89 81 nh.subscribe(sub_esc_control);
YJ_Kim 2:666847c29d89 82 nh.subscribe(sub_led_control);
YJ_Kim 2:666847c29d89 83
YJ_Kim 4:62c92aa48842 84 // ros publisher
YJ_Kim 4:62c92aa48842 85 nh.advertise(pub_state);
YJ_Kim 4:62c92aa48842 86
YJ_Kim 2:666847c29d89 87 ESC_switch = false;
YJ_Kim 0:75ecba0f12ed 88 pwm.init();
YJ_Kim 0:75ecba0f12ed 89
YJ_Kim 4:62c92aa48842 90 t.start();
YJ_Kim 4:62c92aa48842 91 int prev_time, current_time;
YJ_Kim 4:62c92aa48842 92 float voltage, current;
YJ_Kim 4:62c92aa48842 93
YJ_Kim 4:62c92aa48842 94 prev_time = t.read_ms();
YJ_Kim 0:75ecba0f12ed 95 while (1) {
YJ_Kim 4:62c92aa48842 96
YJ_Kim 4:62c92aa48842 97 // 1 Hz : Publish state
YJ_Kim 4:62c92aa48842 98 current_time = t.read_ms();
YJ_Kim 4:62c92aa48842 99 if(current_time - prev_time >= 1000){
YJ_Kim 4:62c92aa48842 100 voltage = (float)adc.read_single_channel(0)*0.003*4.0;
YJ_Kim 4:62c92aa48842 101 current = (float)adc.read_single_channel(1)*0.003*16.7/5.0;
YJ_Kim 4:62c92aa48842 102 state_msg.Voltage = voltage;
YJ_Kim 4:62c92aa48842 103 state_msg.Current = current;
YJ_Kim 4:62c92aa48842 104 pub_state.publish(&state_msg);
YJ_Kim 4:62c92aa48842 105
YJ_Kim 4:62c92aa48842 106 prev_time = prev_time + 1000;
YJ_Kim 4:62c92aa48842 107 }
YJ_Kim 4:62c92aa48842 108
YJ_Kim 0:75ecba0f12ed 109 nh.spinOnce();
YJ_Kim 0:75ecba0f12ed 110 wait_ms(1);
YJ_Kim 0:75ecba0f12ed 111 }
YJ_Kim 0:75ecba0f12ed 112 }