![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Hardware interface for seadragon motors
Dependencies: mbed Servo ros_lib_kinetic
Diff: main.cpp
- Revision:
- 1:c7cfd52db721
- Parent:
- 0:5c809b850f21
- Child:
- 2:48a9c25e5d26
diff -r 5c809b850f21 -r c7cfd52db721 main.cpp --- 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