![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Hardware interface for seadragon motors
Dependencies: mbed Servo ros_lib_kinetic
main.cpp
- Committer:
- roger_wee
- Date:
- 2018-08-01
- Revision:
- 1:c7cfd52db721
- Parent:
- 0:5c809b850f21
- Child:
- 2:48a9c25e5d26
File content as of revision 1:c7cfd52db721:
#include "mbed.h" #include "Servo.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; // Tx - 11, Rx - 9 RawSerial device(PA_15, PB_7); DigitalOut testLed(LED1); Servo myservo(D3); double pan_servo = 0; //--------------ROS Node Configuration--------------- ros::NodeHandle nh; std_msgs::Int16 depth_msg; int depth = 0; ros::Publisher depth_publisher("depth", &depth_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; } } 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.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()) { // 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); // actuate pan tilt camera myservo = pan_servo; nh.spinOnce(); wait_ms(5); testLed = 0; } }