Diff: main.cpp

Revision:

0:5c809b850f21

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jul 23 22:02:33 2018 +0000
@@ -0,0 +1,192 @@
+#include "mbed.h"
+#include <ros.h>
+#include <std_msgs/Int16.h>
+//#include <std_msgs/Bool.h>
+
+//------------Declare Global Variables------------------//
+
+//      (m1)         (m2)
+//          |       |
+//          |       |
+// (Ltrhust)|       |(Rthrust)
+//          |       |
+//          |       |    
+//      (m3)         (m4)   
+
+// Initialize PWM objects
+PwmOut      m1(D8);
+PwmOut      m2(D9);
+PwmOut      m3(D10);
+PwmOut      m4(D11);
+PwmOut      lthrust(D12);
+PwmOut      rthrust(D14);
+
+int PWMBASELINE = 1500;
+
+// Initialize all pwm values to the baseline value
+int m1pwm = PWMBASELINE;
+int m2pwm = PWMBASELINE;
+int m3pwm = PWMBASELINE;
+int m4pwm = PWMBASELINE;
+
+int lthrustpwm = PWMBASELINE;
+int rthrustpwm = PWMBASELINE;
+
+int lfeedback  = 0;
+int rfeedback  = 0;
+int lthrust_tot = 0;
+int rthrust_tot = 0;
+
+int depthfeedback = 0;
+int depthtot = 0;
+
+int MAXYAW = 50;
+
+                // Tx - 11, Rx - 9
+RawSerial device(PA_15, PB_7);
+
+DigitalOut testLed(LED1);
+
+//DigitalIn killswitch(D0);
+
+//--------------ROS Node Configuration---------------
+ros::NodeHandle nh;
+
+std_msgs::Int16 depth_msg;
+int depth = 0;
+//std_msgs::Bool kill_msg;
+
+ros::Publisher depth_publisher("depth", &depth_msg);
+//ros::Publisher killswitch_publisher("killswitch", &kill_msg);
+
+void depth_motor_callback( const std_msgs::Int16& msg)
+{  
+    if (msg.data == 0)
+    {
+        m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE;   
+    }
+    else if (msg.data < 0)   
+    {
+        m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE - 25 + msg.data;
+    }
+    else{
+        // Update motor pwm
+        m1pwm = PWMBASELINE + 25 + msg.data;
+        m2pwm = PWMBASELINE + 25 + msg.data;
+        m3pwm = PWMBASELINE + 25 + msg.data;
+        m4pwm = PWMBASELINE + 25 + msg.data;
+    }
+}
+
+// Forward thrust callback returns valules [0:150]
+void thrust_motor_callback( const std_msgs::Int16& msg)
+{
+    if (msg.data == 0)
+    {
+        lthrustpwm = rthrustpwm = PWMBASELINE;   
+    }
+    else if (msg.data < 0)
+    {
+        lthrustpwm = PWMBASELINE - 25 + msg.data;
+        rthrustpwm = PWMBASELINE - 25 + msg.data;
+    }
+    else{
+        // Update yaw motor pwm
+        lthrustpwm = PWMBASELINE + 25 + msg.data;
+        rthrustpwm = PWMBASELINE + 25 + msg.data;
+    }
+}
+
+// Receives values ranging [0:75]
+void yaw_feedback_callback( const std_msgs::Int16& msg)
+{
+    if (lthrustpwm == PWMBASELINE){
+        if (msg.data > 0.25){
+            lfeedback = (msg.data + 29) * -1;
+            rfeedback = (msg.data + 29);
+        }
+        else if (msg.data < -0.25){
+            lfeedback = (msg.data - 29) * -1;
+            rfeedback = (msg.data - 29);   
+        }
+        else{
+            lfeedback = rfeedback = 0;   
+        }
+    }
+    else{   
+        lfeedback = msg.data * -1;
+        rfeedback = msg.data;       
+    }
+}   
+
+// Receives depth feedback vallues from [0:50]
+void depth_feedback_callback( const std_msgs::Int16& msg)
+{
+    // depth feedback control
+    if (msg.data > 1)
+    {
+        depthfeedback = 25 + msg.data;
+    }
+    else if (msg.data < -1)
+    {
+        depthfeedback = -25 + msg.data;   
+    }
+    else{
+        depthfeedback = 0;
+    }   
+}
+
+ros::Subscriber<std_msgs::Int16> depth_pwm_subscriber("depth_pwm", depth_motor_callback);
+ros::Subscriber<std_msgs::Int16> yaw_pwm_subscriber("yaw_pwm", thrust_motor_callback);
+ros::Subscriber<std_msgs::Int16> fb_yaw_subscriber("yaw_pwm_feedback", yaw_feedback_callback);
+ros::Subscriber<std_msgs::Int16> fb_depth_subscriber("depth_pwm_feedback", depth_feedback_callback);
+
+int main() 
+{    
+    nh.initNode();
+    nh.advertise(depth_publisher);
+//    nh.advertise(killswitch_publisher);
+    nh.subscribe(depth_pwm_subscriber);
+    nh.subscribe(yaw_pwm_subscriber);
+    nh.subscribe(fb_yaw_subscriber);
+    nh.subscribe(fb_depth_subscriber);
+
+    testLed = 0;
+    
+    while(1) 
+    {
+        // If data is recieved, blink led
+        if (device.readable())
+        {
+            // Obtain depth 
+            depth = device.getc();
+            // toggle led on
+            testLed = 1;
+        }
+        
+        // Publish depth information
+        depth_msg.data = depth;
+        depth_publisher.publish(&depth_msg);
+        
+        // Depth thrust + feedback
+        depthtot = m1pwm + depthfeedback;
+        
+        // Output motor pwm
+        m1.pulsewidth_us(depthtot);
+        m2.pulsewidth_us(depthtot);
+        m3.pulsewidth_us(depthtot);
+        m4.pulsewidth_us(depthtot);
+        
+        // Fwd thrust + feedback
+        lthrust_tot = lthrustpwm + lfeedback;
+        rthrust_tot = rthrustpwm + rfeedback;
+        
+        // Output pwm to fwd/rev thrusters    
+        lthrust.pulsewidth_us(lthrust_tot);
+        rthrust.pulsewidth_us(rthrust_tot);
+        
+        nh.spinOnce();
+        wait_ms(2);
+        testLed = 0;
+    }
+}
\ No newline at end of file