Revision 4:62c92aa48842, committed 2017-01-26

Comitter:

YJ_Kim

Date:

Thu Jan 26 18:02:42 2017 +0000

Parent:

3:2c5258907150

Commit message:

add ads1015 library

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ads1015.lib	Thu Jan 26 18:02:42 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/BLUESINK/code/ads1015/#2407708483f4

--- a/main.cpp	Thu Jan 26 12:42:22 2017 +0000
+++ b/main.cpp	Thu Jan 26 18:02:42 2017 +0000
@@ -1,9 +1,13 @@
 #include "mbed.h"
 #include <ros.h>
+
 #include <s_rov_msgs/s_rov_control_extra.h>
 #include <s_rov_msgs/s_rov_control_esc.h>
 #include <s_rov_msgs/s_rov_control_led.h>
+#include <s_rov_msgs/s_rov_state.h>
+
 #include <pca9685/pca9685.h>
+#include <ads1015/ads1015.h>
 
 #define CH_LED1        9
 #define CH_LED2        8
@@ -23,8 +27,10 @@
 
 /**************************** System Configuration ***************************/
 
+Timer t;
 I2C i2c(PB_11, PB_10);        // sda, scl
-PCA9685 pwm(0x40, i2c, 50);  // Address, i2c object, frequency
+PCA9685 pwm(0x40, &i2c, 50);  // Address, i2c pointer, frequency
+ADS1015 adc(0x48, &i2c, ADS1015_GAIN_TWOTHRIDS); // Address, i2c pointer, gain
 DigitalOut ESC_switch(ESC_SWITCH);
 
 /**************************** System Configuration ***************************/
@@ -63,18 +69,43 @@
 ros::Subscriber<s_rov_msgs::s_rov_control_esc> sub_esc_control("/s_rov/control/esc", &messageESC);
 ros::Subscriber<s_rov_msgs::s_rov_control_led> sub_led_control("/s_rov/control/led", &messageLed);
 
+s_rov_msgs::s_rov_state state_msg;
+ros::Publisher pub_state("/s_rov/state", &state_msg);
+
 /******************************ROS Configuration *****************************/
 
 int main() {
     nh.initNode();
+    // ros subscriber
     nh.subscribe(sub_extra_control);
     nh.subscribe(sub_esc_control);
     nh.subscribe(sub_led_control);
     
+    // ros publisher
+    nh.advertise(pub_state);
+    
     ESC_switch = false;
     pwm.init();
     
+    t.start();
+    int prev_time, current_time;
+    float voltage, current;
+    
+    prev_time = t.read_ms();
     while (1) {
+        
+        // 1 Hz : Publish state
+        current_time = t.read_ms();
+        if(current_time - prev_time >= 1000){
+            voltage = (float)adc.read_single_channel(0)*0.003*4.0;
+            current = (float)adc.read_single_channel(1)*0.003*16.7/5.0;
+            state_msg.Voltage = voltage;
+            state_msg.Current = current;
+            pub_state.publish(&state_msg);
+            
+            prev_time = prev_time + 1000;
+        }
+        
         nh.spinOnce();
         wait_ms(1);     
     }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/msg/s_rov_msgs/s_rov_state.h	Thu Jan 26 18:02:42 2017 +0000
@@ -0,0 +1,84 @@
+#ifndef _ROS_s_rov_msgs_s_rov_state_h
+#define _ROS_s_rov_msgs_s_rov_state_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace s_rov_msgs
+{
+
+  class s_rov_state : public ros::Msg
+  {
+    public:
+      float Voltage;
+      float Current;
+
+    s_rov_state():
+      Voltage(0),
+      Current(0)
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        float real;
+        uint32_t base;
+      } u_Voltage;
+      u_Voltage.real = this->Voltage;
+      *(outbuffer + offset + 0) = (u_Voltage.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_Voltage.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_Voltage.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_Voltage.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->Voltage);
+      union {
+        float real;
+        uint32_t base;
+      } u_Current;
+      u_Current.real = this->Current;
+      *(outbuffer + offset + 0) = (u_Current.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_Current.base >> (8 * 1)) & 0xFF;
+      *(outbuffer + offset + 2) = (u_Current.base >> (8 * 2)) & 0xFF;
+      *(outbuffer + offset + 3) = (u_Current.base >> (8 * 3)) & 0xFF;
+      offset += sizeof(this->Current);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        float real;
+        uint32_t base;
+      } u_Voltage;
+      u_Voltage.base = 0;
+      u_Voltage.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_Voltage.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_Voltage.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_Voltage.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->Voltage = u_Voltage.real;
+      offset += sizeof(this->Voltage);
+      union {
+        float real;
+        uint32_t base;
+      } u_Current;
+      u_Current.base = 0;
+      u_Current.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_Current.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      u_Current.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
+      u_Current.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
+      this->Current = u_Current.real;
+      offset += sizeof(this->Current);
+     return offset;
+    }
+
+    const char * getType(){ return "s_rov_msgs/s_rov_state"; };
+    const char * getMD5(){ return "e2848ca8a25ac096219c461e07bcd312"; };
+
+  };
+
+}
+#endif
\ No newline at end of file

--- a/pca9685.lib	Thu Jan 26 12:42:22 2017 +0000
+++ b/pca9685.lib	Thu Jan 26 18:02:42 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/BLUESINK/code/pca9685/#7e071acc57b1
+https://developer.mbed.org/teams/BLUESINK/code/pca9685/#5c8802f876f8