Rogelio Vazquez
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