Diff: main.cpp

Revision:

1:c7cfd52db721

Parent:

0:5c809b850f21

Child:

2:48a9c25e5d26

--- a/main.cpp	Mon Jul 23 22:02:33 2018 +0000
+++ b/main.cpp	Wed Aug 01 08:12:41 2018 +0000
@@ -1,7 +1,8 @@
 #include "mbed.h"
+#include "Servo.h"
 #include <ros.h>
 #include <std_msgs/Int16.h>
-//#include <std_msgs/Bool.h>
+#include <std_msgs/Bool.h>
 
 //------------Declare Global Variables------------------//
 
@@ -22,7 +23,6 @@
 PwmOut      rthrust(D14);
 
 int PWMBASELINE = 1500;
-
 // Initialize all pwm values to the baseline value
 int m1pwm = PWMBASELINE;
 int m2pwm = PWMBASELINE;
@@ -40,24 +40,18 @@
 int depthfeedback = 0;
 int depthtot = 0;
 
-int MAXYAW = 50;
-
-                // Tx - 11, Rx - 9
+// Tx - 11, Rx - 9
 RawSerial device(PA_15, PB_7);
-
 DigitalOut testLed(LED1);
-
-//DigitalIn killswitch(D0);
+Servo myservo(D3);
+double pan_servo = 0;
 
 //--------------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)
 {  
@@ -136,25 +130,31 @@
     }   
 }
 
+void pan_callback( const std_msgs::Int16& msg)
+{
+    pan_servo = msg.data/100.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);
+ros::Subscriber<std_msgs::Int16> pan_servo_subscriber("pan_servo", pan_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);
+    nh.subscribe(pan_servo_subscriber);
 
     testLed = 0;
     
     while(1) 
-    {
+    {       
         // If data is recieved, blink led
         if (device.readable())
         {
@@ -185,8 +185,11 @@
         lthrust.pulsewidth_us(lthrust_tot);
         rthrust.pulsewidth_us(rthrust_tot);
         
+        // actuate pan tilt camera
+        myservo = pan_servo;
+        
         nh.spinOnce();
-        wait_ms(2);
+        wait_ms(5);
         testLed = 0;
     }
 }
\ No newline at end of file