![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Hardware interface for seadragon motors / pressure sensor / leak sensor
Dependencies: mbed ros_lib_kinetic
Revision 0:5c809b850f21, committed 2018-07-23
- Comitter:
- roger_wee
- Date:
- Mon Jul 23 22:02:33 2018 +0000
- Commit message:
- seadragon_motors_ros
Changed in this revision
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jul 23 22:02:33 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/994bdf8177cb \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Mon Jul 23 22:02:33 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f