Improve LED controller and add Sonar topic #4 #6

panther_body/CMakeLists.txt

@@ -14,7 +14,12 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
 )
 
-catkin_package()
+catkin_package(
+  CATKIN_DEPENDS 
+    geometry_msgs
+    std_msgs
+    sensor_msgs
+)
 
 ###########
 ## Build ##
@@ -27,9 +32,10 @@ rosserial_generate_ros_lib(
 
 rosserial_configure_client(
   DIRECTORY firmware
-  TOOLCHAIN_FILE ${ROSSERIAL_ARDUINO_TOOLCHAIN}
+  TOOLCHAIN_FILE ${ROS_TEENSY_TOOLCHAIN}
 )
 
-rosserial_add_client_target(firmware body ALL)
-rosserial_add_client_target(firmware body-upload)
+# TODO: Removed waiting NUCLEO to change
+# rosserial_add_client_target(firmware body_Firmware ALL)
+# rosserial_add_client_target(firmware body-upload)
 

panther_body/firmware/SFR10.cpp → panther_body/firmware/body/SFR10.ino

@@ -28,58 +28,57 @@
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-#if defined(ARDUINO) && ARDUINO >= 100
-    #include "Arduino.h"
-#else
-    #include "WProgram.h"
-#endif
-
-#include <Wire.h>
-
-#include "SFR10.hpp"
-
+/**
+ * @brief Connect I2C
+ * SFR10: https://www.robot-electronics.co.uk/htm/srf10tech.htm
+ */
 void SFR10_connect(sensor_msgs::Range* range_msg)
 {
   // Waits to make sure everything is powered up before sending or receiving data
   Wire.begin();
-  // Initialze range message
-  // SFR10 https://www.robot-electronics.co.uk/htm/srf10tech.htm
+  // Initialize range sensor
   range_msg->radiation_type = sensor_msgs::Range::ULTRASOUND;
-  // range_msg.header.frame_id =  frameid;
-  range_msg->field_of_view = 0.01;
-  range_msg->min_range = 0.03;
-  range_msg->max_range = 0.4;
+  range_msg->field_of_view = 1;
+  range_msg->min_range = 0.0;
+  range_msg->max_range = 5.0;
 }
-
-void SFR10_init(SFR10_t* SFR10, ros::Publisher* pub, sensor_msgs::Range* range_msg, int address, char* name)
+/**
+ * @brief Initialize ROS
+ */
+void SFR10_init(ros::NodeHandle* nh, SFR10_t &SFR10)
 {
-  SFR10->pub = pub;
-  SFR10->msg = range_msg;
-  SFR10->address = address;
-  SFR10->name = name;
+  // Initialize topic
+  nh->advertise(*SFR10.pub); 
 }
-
-int SFR10_publish(ros::NodeHandle* nh, SFR10_t* SFR10)
+/**
+ * @brief Publish range sensor
+ */
+void SFR10_publish(ros::NodeHandle* nh, SFR10_t &SFR10)
 {
-  // read range sensor
-  int range = SFR10_getRange(SFR10->address);
+  float range = ((float) SFR10.distance) / 100.0;
   // Publish message
-  SFR10->msg->header.stamp = nh->now();
-  SFR10->msg->range = range;
-  SFR10->pub->publish(SFR10->msg);
-  // return range
-  return range;
+  SFR10.msg->header.stamp = nh->now();
+  SFR10.msg->header.frame_id = SFR10.frame_id;
+  SFR10.msg->range = range;
+  SFR10.pub->publish(SFR10.msg);
+}
+/**
+ * @brief Update SFR10 status
+ */
+void SFR10_update(SFR10_t &SFR10)
+{
+  // read range sensor
+  SFR10.distance = SFR10_getRange(SFR10);
 }
-
 /**
  * @brief SFR10 sonar
  * reference: https://www.robot-electronics.co.uk/htm/srf10tech.htm
  */
-int SFR10_getRange(int srfAddress)
+int SFR10_getRange(SFR10_t &SFR10)
 {
-  int range = 0;
+  int distance = 0;
   // step 1: instruct sensor to read echoes
-  Wire.beginTransmission(srfAddress); // transmit to device #112 (0x70)
+  Wire.beginTransmission(SFR10.address); // transmit to device #112 (0x70)
   // the address specified in the datasheet is 224 (0xE0)
   // but i2c adressing uses the high 7 bits so it's 112
   Wire.write(byte(0x00));             // sets register pointer to the command register (0x00)
@@ -91,16 +90,16 @@ int SFR10_getRange(int srfAddress)
   // step 2: wait for readings to happen
   delay(70);                          // datasheet suggests at least 65 milliseconds
   // step 3: instruct sensor to return a particular echo reading
-  Wire.beginTransmission(srfAddress); // transmit to device #112
+  Wire.beginTransmission(SFR10.address); // transmit to device #112
   Wire.write(byte(0x02));             // sets register pointer to echo #1 register (0x02)
   Wire.endTransmission();             // stop transmitting
   // step 4: request reading from sensor
-  Wire.requestFrom(srfAddress, 2);    // request 2 bytes from slave device #112
+  Wire.requestFrom(SFR10.address, 2);    // request 2 bytes from slave device #112
   // step 5: receive reading from sensor
   if (2 <= Wire.available()) {        // if two bytes were received
-    range = Wire.read();              // receive high byte (overwrites previous reading)
-    range = range << 8;               // shift high byte to be high 8 bits
-    range |= Wire.read();             // receive low byte as lower 8 bits
+    distance = Wire.read();           // receive high byte (overwrites previous reading)
+    distance = distance << 8;         // shift high byte to be high 8 bits
+    distance |= Wire.read();          // receive low byte as lower 8 bits
   }
-  return range;
-}
+  return distance;
+}