Gerardo Hernandez / Mbed 2 deprecated Seadragon_motors_ros

Added the leak sensor and kill switch

Dependencies:   mbed ros_lib_kinetic

Fork of Seadragon_motors_ros by Rogelio Vazquez

main.cpp@1:b07605144252, 2018-07-24 (annotated)

Committer:
ElevatedSounds
Date:
Tue Jul 24 03:46:19 2018 +0000
Revision:
1:b07605144252
Parent:
0:5c809b850f21 
This code implement motors control with PID and a leak sensor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roger_wee 0:5c809b850f21 1 #include "mbed.h"
roger_wee 0:5c809b850f21 2 #include <ros.h>
roger_wee 0:5c809b850f21 3 #include <std_msgs/Int16.h>
roger_wee 0:5c809b850f21 4 //#include <std_msgs/Bool.h>
roger_wee 0:5c809b850f21 5
roger_wee 0:5c809b850f21 6 //------------Declare Global Variables------------------//
roger_wee 0:5c809b850f21 7
roger_wee 0:5c809b850f21 8 // (m1) (m2)
roger_wee 0:5c809b850f21 9 // | |
roger_wee 0:5c809b850f21 10 // | |
roger_wee 0:5c809b850f21 11 // (Ltrhust)| |(Rthrust)
roger_wee 0:5c809b850f21 12 // | |
roger_wee 0:5c809b850f21 13 // | |
roger_wee 0:5c809b850f21 14 // (m3) (m4)
roger_wee 0:5c809b850f21 15
roger_wee 0:5c809b850f21 16 // Initialize PWM objects
roger_wee 0:5c809b850f21 17 PwmOut m1(D8);
roger_wee 0:5c809b850f21 18 PwmOut m2(D9);
roger_wee 0:5c809b850f21 19 PwmOut m3(D10);
roger_wee 0:5c809b850f21 20 PwmOut m4(D11);
roger_wee 0:5c809b850f21 21 PwmOut lthrust(D12);
roger_wee 0:5c809b850f21 22 PwmOut rthrust(D14);
roger_wee 0:5c809b850f21 23
roger_wee 0:5c809b850f21 24 int PWMBASELINE = 1500;
roger_wee 0:5c809b850f21 25
roger_wee 0:5c809b850f21 26 // Initialize all pwm values to the baseline value
roger_wee 0:5c809b850f21 27 int m1pwm = PWMBASELINE;
roger_wee 0:5c809b850f21 28 int m2pwm = PWMBASELINE;
roger_wee 0:5c809b850f21 29 int m3pwm = PWMBASELINE;
roger_wee 0:5c809b850f21 30 int m4pwm = PWMBASELINE;
roger_wee 0:5c809b850f21 31
roger_wee 0:5c809b850f21 32 int lthrustpwm = PWMBASELINE;
roger_wee 0:5c809b850f21 33 int rthrustpwm = PWMBASELINE;
roger_wee 0:5c809b850f21 34
roger_wee 0:5c809b850f21 35 int lfeedback = 0;
roger_wee 0:5c809b850f21 36 int rfeedback = 0;
roger_wee 0:5c809b850f21 37 int lthrust_tot = 0;
roger_wee 0:5c809b850f21 38 int rthrust_tot = 0;
roger_wee 0:5c809b850f21 39
roger_wee 0:5c809b850f21 40 int depthfeedback = 0;
roger_wee 0:5c809b850f21 41 int depthtot = 0;
roger_wee 0:5c809b850f21 42
roger_wee 0:5c809b850f21 43 int MAXYAW = 50;
roger_wee 0:5c809b850f21 44
roger_wee 0:5c809b850f21 45 // Tx - 11, Rx - 9
roger_wee 0:5c809b850f21 46 RawSerial device(PA_15, PB_7);
roger_wee 0:5c809b850f21 47
roger_wee 0:5c809b850f21 48 DigitalOut testLed(LED1);
roger_wee 0:5c809b850f21 49
roger_wee 0:5c809b850f21 50 //DigitalIn killswitch(D0);
roger_wee 0:5c809b850f21 51
roger_wee 0:5c809b850f21 52 //--------------ROS Node Configuration---------------
roger_wee 0:5c809b850f21 53 ros::NodeHandle nh;
roger_wee 0:5c809b850f21 54
roger_wee 0:5c809b850f21 55 std_msgs::Int16 depth_msg;
roger_wee 0:5c809b850f21 56 int depth = 0;
roger_wee 0:5c809b850f21 57 //std_msgs::Bool kill_msg;
ElevatedSounds 1:b07605144252 58 std_msgs::Bool kill_msg;
ElevatedSounds 1:b07605144252 59 ros::Publisher depth_publisher("depth", &depth_msg);
roger_wee 0:5c809b850f21 60
roger_wee 0:5c809b850f21 61 ros::Publisher depth_publisher("depth", &depth_msg);
roger_wee 0:5c809b850f21 62 //ros::Publisher killswitch_publisher("killswitch", &kill_msg);
roger_wee 0:5c809b850f21 63
roger_wee 0:5c809b850f21 64 void depth_motor_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 65 {
roger_wee 0:5c809b850f21 66 if (msg.data == 0)
roger_wee 0:5c809b850f21 67 {
roger_wee 0:5c809b850f21 68 m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE;
roger_wee 0:5c809b850f21 69 }
roger_wee 0:5c809b850f21 70 else if (msg.data < 0)
roger_wee 0:5c809b850f21 71 {
roger_wee 0:5c809b850f21 72 m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE - 25 + msg.data;
roger_wee 0:5c809b850f21 73 }
roger_wee 0:5c809b850f21 74 else{
roger_wee 0:5c809b850f21 75 // Update motor pwm
roger_wee 0:5c809b850f21 76 m1pwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 77 m2pwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 78 m3pwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 79 m4pwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 80 }
roger_wee 0:5c809b850f21 81 }
roger_wee 0:5c809b850f21 82
roger_wee 0:5c809b850f21 83 // Forward thrust callback returns valules [0:150]
roger_wee 0:5c809b850f21 84 void thrust_motor_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 85 {
roger_wee 0:5c809b850f21 86 if (msg.data == 0)
roger_wee 0:5c809b850f21 87 {
roger_wee 0:5c809b850f21 88 lthrustpwm = rthrustpwm = PWMBASELINE;
roger_wee 0:5c809b850f21 89 }
roger_wee 0:5c809b850f21 90 else if (msg.data < 0)
roger_wee 0:5c809b850f21 91 {
roger_wee 0:5c809b850f21 92 lthrustpwm = PWMBASELINE - 25 + msg.data;
roger_wee 0:5c809b850f21 93 rthrustpwm = PWMBASELINE - 25 + msg.data;
roger_wee 0:5c809b850f21 94 }
roger_wee 0:5c809b850f21 95 else{
roger_wee 0:5c809b850f21 96 // Update yaw motor pwm
roger_wee 0:5c809b850f21 97 lthrustpwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 98 rthrustpwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 99 }
roger_wee 0:5c809b850f21 100 }
roger_wee 0:5c809b850f21 101
roger_wee 0:5c809b850f21 102 // Receives values ranging [0:75]
roger_wee 0:5c809b850f21 103 void yaw_feedback_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 104 {
roger_wee 0:5c809b850f21 105 if (lthrustpwm == PWMBASELINE){
roger_wee 0:5c809b850f21 106 if (msg.data > 0.25){
roger_wee 0:5c809b850f21 107 lfeedback = (msg.data + 29) * -1;
roger_wee 0:5c809b850f21 108 rfeedback = (msg.data + 29);
roger_wee 0:5c809b850f21 109 }
roger_wee 0:5c809b850f21 110 else if (msg.data < -0.25){
roger_wee 0:5c809b850f21 111 lfeedback = (msg.data - 29) * -1;
roger_wee 0:5c809b850f21 112 rfeedback = (msg.data - 29);
roger_wee 0:5c809b850f21 113 }
roger_wee 0:5c809b850f21 114 else{
roger_wee 0:5c809b850f21 115 lfeedback = rfeedback = 0;
roger_wee 0:5c809b850f21 116 }
roger_wee 0:5c809b850f21 117 }
roger_wee 0:5c809b850f21 118 else{
roger_wee 0:5c809b850f21 119 lfeedback = msg.data * -1;
roger_wee 0:5c809b850f21 120 rfeedback = msg.data;
roger_wee 0:5c809b850f21 121 }
roger_wee 0:5c809b850f21 122 }
roger_wee 0:5c809b850f21 123
roger_wee 0:5c809b850f21 124 // Receives depth feedback vallues from [0:50]
roger_wee 0:5c809b850f21 125 void depth_feedback_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 126 {
roger_wee 0:5c809b850f21 127 // depth feedback control
roger_wee 0:5c809b850f21 128 if (msg.data > 1)
roger_wee 0:5c809b850f21 129 {
roger_wee 0:5c809b850f21 130 depthfeedback = 25 + msg.data;
roger_wee 0:5c809b850f21 131 }
roger_wee 0:5c809b850f21 132 else if (msg.data < -1)
roger_wee 0:5c809b850f21 133 {
roger_wee 0:5c809b850f21 134 depthfeedback = -25 + msg.data;
roger_wee 0:5c809b850f21 135 }
roger_wee 0:5c809b850f21 136 else{
roger_wee 0:5c809b850f21 137 depthfeedback = 0;
roger_wee 0:5c809b850f21 138 }
roger_wee 0:5c809b850f21 139 }
roger_wee 0:5c809b850f21 140
roger_wee 0:5c809b850f21 141 ros::Subscriber<std_msgs::Int16> depth_pwm_subscriber("depth_pwm", depth_motor_callback);
roger_wee 0:5c809b850f21 142 ros::Subscriber<std_msgs::Int16> yaw_pwm_subscriber("yaw_pwm", thrust_motor_callback);
roger_wee 0:5c809b850f21 143 ros::Subscriber<std_msgs::Int16> fb_yaw_subscriber("yaw_pwm_feedback", yaw_feedback_callback);
roger_wee 0:5c809b850f21 144 ros::Subscriber<std_msgs::Int16> fb_depth_subscriber("depth_pwm_feedback", depth_feedback_callback);
roger_wee 0:5c809b850f21 145
roger_wee 0:5c809b850f21 146 int main()
roger_wee 0:5c809b850f21 147 {
roger_wee 0:5c809b850f21 148 nh.initNode();
roger_wee 0:5c809b850f21 149 nh.advertise(depth_publisher);
roger_wee 0:5c809b850f21 150 // nh.advertise(killswitch_publisher);
roger_wee 0:5c809b850f21 151 nh.subscribe(depth_pwm_subscriber);
roger_wee 0:5c809b850f21 152 nh.subscribe(yaw_pwm_subscriber);
roger_wee 0:5c809b850f21 153 nh.subscribe(fb_yaw_subscriber);
roger_wee 0:5c809b850f21 154 nh.subscribe(fb_depth_subscriber);
ElevatedSounds 1:b07605144252 155 nh.advertise(leak_publisher);
ElevatedSounds 1:b07605144252 156 nh.advertise(killSwitch_publisher);
roger_wee 0:5c809b850f21 157
roger_wee 0:5c809b850f21 158 testLed = 0;
roger_wee 0:5c809b850f21 159
roger_wee 0:5c809b850f21 160 while(1)
roger_wee 0:5c809b850f21 161 {
ElevatedSounds 1:b07605144252 162 if(leakSensor)
ElevatedSounds 1:b07605144252 163 {
ElevatedSounds 1:b07605144252 164 leak_msg.data = 1;
ElevatedSounds 1:b07605144252 165 }
ElevatedSounds 1:b07605144252 166 else
ElevatedSounds 1:b07605144252 167 {
ElevatedSounds 1:b07605144252 168 leak_msg.data = 0;
ElevatedSounds 1:b07605144252 169 }
ElevatedSounds 1:b07605144252 170 leak_publisher.publish(&leak_msg);
ElevatedSounds 1:b07605144252 171
ElevatedSounds 1:b07605144252 172 if(killSwitchSensor)
ElevatedSounds 1:b07605144252 173 {
ElevatedSounds 1:b07605144252 174 killSwitch_msg.data = 1;
ElevatedSounds 1:b07605144252 175 }
ElevatedSounds 1:b07605144252 176 else
ElevatedSounds 1:b07605144252 177 {
ElevatedSounds 1:b07605144252 178 killSwitch_msg.data = 0;
ElevatedSounds 1:b07605144252 179 }
ElevatedSounds 1:b07605144252 180 killSwitch_publisher.publish(&killSwitch_msg);
ElevatedSounds 1:b07605144252 181
roger_wee 0:5c809b850f21 182 // If data is recieved, blink led
roger_wee 0:5c809b850f21 183 if (device.readable())
roger_wee 0:5c809b850f21 184 {
roger_wee 0:5c809b850f21 185 // Obtain depth
roger_wee 0:5c809b850f21 186 depth = device.getc();
roger_wee 0:5c809b850f21 187 // toggle led on
roger_wee 0:5c809b850f21 188 testLed = 1;
roger_wee 0:5c809b850f21 189 }
roger_wee 0:5c809b850f21 190
roger_wee 0:5c809b850f21 191 // Publish depth information
roger_wee 0:5c809b850f21 192 depth_msg.data = depth;
roger_wee 0:5c809b850f21 193 depth_publisher.publish(&depth_msg);
roger_wee 0:5c809b850f21 194
roger_wee 0:5c809b850f21 195 // Depth thrust + feedback
roger_wee 0:5c809b850f21 196 depthtot = m1pwm + depthfeedback;
roger_wee 0:5c809b850f21 197
roger_wee 0:5c809b850f21 198 // Output motor pwm
roger_wee 0:5c809b850f21 199 m1.pulsewidth_us(depthtot);
roger_wee 0:5c809b850f21 200 m2.pulsewidth_us(depthtot);
roger_wee 0:5c809b850f21 201 m3.pulsewidth_us(depthtot);
roger_wee 0:5c809b850f21 202 m4.pulsewidth_us(depthtot);
roger_wee 0:5c809b850f21 203
roger_wee 0:5c809b850f21 204 // Fwd thrust + feedback
roger_wee 0:5c809b850f21 205 lthrust_tot = lthrustpwm + lfeedback;
roger_wee 0:5c809b850f21 206 rthrust_tot = rthrustpwm + rfeedback;
roger_wee 0:5c809b850f21 207
roger_wee 0:5c809b850f21 208 // Output pwm to fwd/rev thrusters
roger_wee 0:5c809b850f21 209 lthrust.pulsewidth_us(lthrust_tot);
roger_wee 0:5c809b850f21 210 rthrust.pulsewidth_us(rthrust_tot);
roger_wee 0:5c809b850f21 211
roger_wee 0:5c809b850f21 212 nh.spinOnce();
roger_wee 0:5c809b850f21 213 wait_ms(2);
roger_wee 0:5c809b850f21 214 testLed = 0;
roger_wee 0:5c809b850f21 215 }
roger_wee 0:5c809b850f21 216 }