main.cpp

00001 #include "mbed.h"
00002 #include <ros.h>
00003 
00004 #include <s_rov_msgs/s_rov_control_extra.h>
00005 #include <s_rov_msgs/s_rov_control_esc.h>
00006 #include <s_rov_msgs/s_rov_control_led.h>
00007 #include <s_rov_msgs/s_rov_state.h>
00008 
00009 #include <pca9685/pca9685.h>
00010 #include <ads1015/ads1015.h>
00011 
00012 #define CH_LED1        9
00013 #define CH_LED2        8
00014 #define CH_ESC1       10
00015 #define CH_ESC2       11
00016 #define CH_ESC3        7
00017 #define CH_ESC4        6
00018 #define CH_1           0
00019 #define CH_2           1
00020 #define CH_3           2
00021 #define CH_4           3
00022 #define CH_5           4
00023 #define CH_6           5
00024 
00025 #define ESC_SWITCH  PB_1
00026 
00027 
00028 /**************************** System Configuration ***************************/
00029 
00030 Timer t;
00031 I2C i2c(PB_11, PB_10);        // sda, scl
00032 PCA9685 pwm(0x40, &i2c, 50);  // Address, i2c pointer, frequency
00033 ADS1015 adc(0x48, &i2c, ADS1015_GAIN_TWOTHRIDS); // Address, i2c pointer, gain
00034 DigitalOut ESC_switch(ESC_SWITCH);
00035 
00036 /**************************** System Configuration ***************************/
00037 
00038 /******************************ROS Configuration *****************************/
00039 
00040 class NewHardware : public MbedHardware{
00041     public:
00042     NewHardware():MbedHardware(PA_9, PA_10, 57600){};    
00043 };
00044 ros::NodeHandle_<NewHardware> nh;
00045 
00046 void messageExtra(const s_rov_msgs::s_rov_control_extra& control_msg){
00047     pwm.set_servo_angle(CH_1, control_msg.extraPWM[0]);
00048     pwm.set_servo_angle(CH_2, control_msg.extraPWM[1]);
00049     pwm.set_servo_angle(CH_3, control_msg.extraPWM[2]);
00050     pwm.set_servo_angle(CH_4, control_msg.extraPWM[3]);
00051     pwm.set_servo_angle(CH_5, control_msg.extraPWM[4]);
00052     pwm.set_servo_angle(CH_6, control_msg.extraPWM[5]);
00053 }
00054 
00055 void messageESC(const s_rov_msgs::s_rov_control_esc& control_msg){
00056     pwm.set_servo_angle(CH_ESC1, control_msg.escPWM[0]);
00057     pwm.set_servo_angle(CH_ESC2, control_msg.escPWM[1]);
00058     pwm.set_servo_angle(CH_ESC3, control_msg.escPWM[2]);
00059     pwm.set_servo_angle(CH_ESC4, control_msg.escPWM[3]);
00060     ESC_switch = control_msg.escSwitch;
00061 }
00062 
00063 void messageLed(const s_rov_msgs::s_rov_control_led& control_msg){
00064     pwm.set_pwm_duty(CH_LED1, control_msg.ledPWM[0]);
00065     pwm.set_pwm_duty(CH_LED2, control_msg.ledPWM[1]);
00066 }
00067 
00068 ros::Subscriber<s_rov_msgs::s_rov_control_extra> sub_extra_control("/s_rov/control/extra", &messageExtra);
00069 ros::Subscriber<s_rov_msgs::s_rov_control_esc> sub_esc_control("/s_rov/control/esc", &messageESC);
00070 ros::Subscriber<s_rov_msgs::s_rov_control_led> sub_led_control("/s_rov/control/led", &messageLed);
00071 
00072 s_rov_msgs::s_rov_state state_msg;
00073 ros::Publisher pub_state("/s_rov/state", &state_msg);
00074 
00075 /******************************ROS Configuration *****************************/
00076 
00077 int main() {
00078     nh.initNode();
00079     // ros subscriber
00080     nh.subscribe(sub_extra_control);
00081     nh.subscribe(sub_esc_control);
00082     nh.subscribe(sub_led_control);
00083     
00084     // ros publisher
00085     nh.advertise(pub_state);
00086     
00087     ESC_switch = false;
00088     pwm.init();
00089     
00090     t.start();
00091     int prev_time, current_time;
00092     float voltage, current;
00093     
00094     prev_time = t.read_ms();
00095     while (1) {
00096         
00097         // 1 Hz : Publish state
00098         current_time = t.read_ms();
00099         if(current_time - prev_time >= 1000){
00100             voltage = (float)adc.read_single_channel(0)*0.003*4.0;
00101             current = (float)adc.read_single_channel(1)*0.003*16.7/5.0;
00102             state_msg.Voltage = voltage;
00103             state_msg.Current = current;
00104             pub_state.publish(&state_msg);
00105             
00106             prev_time = prev_time + 1000;
00107         }
00108         
00109         nh.spinOnce();
00110         wait_ms(1);     
00111     }
00112 }