![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Hardware interface for seadragon motors
Dependencies: Servo mbed ros_lib_kinetic
Fork of Seadragon_motors_ros by
main.cpp@0:5c809b850f21, 2018-07-23 (annotated)
- Committer:
- roger_wee
- Date:
- Mon Jul 23 22:02:33 2018 +0000
- Revision:
- 0:5c809b850f21
- Child:
- 1:c7cfd52db721
seadragon_motors_ros
Who changed what in which revision?
User | Revision | Line number | New 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; |
roger_wee | 0:5c809b850f21 | 58 | |
roger_wee | 0:5c809b850f21 | 59 | ros::Publisher depth_publisher("depth", &depth_msg); |
roger_wee | 0:5c809b850f21 | 60 | //ros::Publisher killswitch_publisher("killswitch", &kill_msg); |
roger_wee | 0:5c809b850f21 | 61 | |
roger_wee | 0:5c809b850f21 | 62 | void depth_motor_callback( const std_msgs::Int16& msg) |
roger_wee | 0:5c809b850f21 | 63 | { |
roger_wee | 0:5c809b850f21 | 64 | if (msg.data == 0) |
roger_wee | 0:5c809b850f21 | 65 | { |
roger_wee | 0:5c809b850f21 | 66 | m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE; |
roger_wee | 0:5c809b850f21 | 67 | } |
roger_wee | 0:5c809b850f21 | 68 | else if (msg.data < 0) |
roger_wee | 0:5c809b850f21 | 69 | { |
roger_wee | 0:5c809b850f21 | 70 | m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE - 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 71 | } |
roger_wee | 0:5c809b850f21 | 72 | else{ |
roger_wee | 0:5c809b850f21 | 73 | // Update motor pwm |
roger_wee | 0:5c809b850f21 | 74 | m1pwm = PWMBASELINE + 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 75 | m2pwm = PWMBASELINE + 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 76 | m3pwm = PWMBASELINE + 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 77 | m4pwm = PWMBASELINE + 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 78 | } |
roger_wee | 0:5c809b850f21 | 79 | } |
roger_wee | 0:5c809b850f21 | 80 | |
roger_wee | 0:5c809b850f21 | 81 | // Forward thrust callback returns valules [0:150] |
roger_wee | 0:5c809b850f21 | 82 | void thrust_motor_callback( const std_msgs::Int16& msg) |
roger_wee | 0:5c809b850f21 | 83 | { |
roger_wee | 0:5c809b850f21 | 84 | if (msg.data == 0) |
roger_wee | 0:5c809b850f21 | 85 | { |
roger_wee | 0:5c809b850f21 | 86 | lthrustpwm = rthrustpwm = PWMBASELINE; |
roger_wee | 0:5c809b850f21 | 87 | } |
roger_wee | 0:5c809b850f21 | 88 | else if (msg.data < 0) |
roger_wee | 0:5c809b850f21 | 89 | { |
roger_wee | 0:5c809b850f21 | 90 | lthrustpwm = PWMBASELINE - 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 91 | rthrustpwm = PWMBASELINE - 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 92 | } |
roger_wee | 0:5c809b850f21 | 93 | else{ |
roger_wee | 0:5c809b850f21 | 94 | // Update yaw motor pwm |
roger_wee | 0:5c809b850f21 | 95 | lthrustpwm = PWMBASELINE + 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 96 | rthrustpwm = PWMBASELINE + 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 97 | } |
roger_wee | 0:5c809b850f21 | 98 | } |
roger_wee | 0:5c809b850f21 | 99 | |
roger_wee | 0:5c809b850f21 | 100 | // Receives values ranging [0:75] |
roger_wee | 0:5c809b850f21 | 101 | void yaw_feedback_callback( const std_msgs::Int16& msg) |
roger_wee | 0:5c809b850f21 | 102 | { |
roger_wee | 0:5c809b850f21 | 103 | if (lthrustpwm == PWMBASELINE){ |
roger_wee | 0:5c809b850f21 | 104 | if (msg.data > 0.25){ |
roger_wee | 0:5c809b850f21 | 105 | lfeedback = (msg.data + 29) * -1; |
roger_wee | 0:5c809b850f21 | 106 | rfeedback = (msg.data + 29); |
roger_wee | 0:5c809b850f21 | 107 | } |
roger_wee | 0:5c809b850f21 | 108 | else if (msg.data < -0.25){ |
roger_wee | 0:5c809b850f21 | 109 | lfeedback = (msg.data - 29) * -1; |
roger_wee | 0:5c809b850f21 | 110 | rfeedback = (msg.data - 29); |
roger_wee | 0:5c809b850f21 | 111 | } |
roger_wee | 0:5c809b850f21 | 112 | else{ |
roger_wee | 0:5c809b850f21 | 113 | lfeedback = rfeedback = 0; |
roger_wee | 0:5c809b850f21 | 114 | } |
roger_wee | 0:5c809b850f21 | 115 | } |
roger_wee | 0:5c809b850f21 | 116 | else{ |
roger_wee | 0:5c809b850f21 | 117 | lfeedback = msg.data * -1; |
roger_wee | 0:5c809b850f21 | 118 | rfeedback = msg.data; |
roger_wee | 0:5c809b850f21 | 119 | } |
roger_wee | 0:5c809b850f21 | 120 | } |
roger_wee | 0:5c809b850f21 | 121 | |
roger_wee | 0:5c809b850f21 | 122 | // Receives depth feedback vallues from [0:50] |
roger_wee | 0:5c809b850f21 | 123 | void depth_feedback_callback( const std_msgs::Int16& msg) |
roger_wee | 0:5c809b850f21 | 124 | { |
roger_wee | 0:5c809b850f21 | 125 | // depth feedback control |
roger_wee | 0:5c809b850f21 | 126 | if (msg.data > 1) |
roger_wee | 0:5c809b850f21 | 127 | { |
roger_wee | 0:5c809b850f21 | 128 | depthfeedback = 25 + msg.data; |
roger_wee | 0:5c809b850f21 | 129 | } |
roger_wee | 0:5c809b850f21 | 130 | else if (msg.data < -1) |
roger_wee | 0:5c809b850f21 | 131 | { |
roger_wee | 0:5c809b850f21 | 132 | depthfeedback = -25 + msg.data; |
roger_wee | 0:5c809b850f21 | 133 | } |
roger_wee | 0:5c809b850f21 | 134 | else{ |
roger_wee | 0:5c809b850f21 | 135 | depthfeedback = 0; |
roger_wee | 0:5c809b850f21 | 136 | } |
roger_wee | 0:5c809b850f21 | 137 | } |
roger_wee | 0:5c809b850f21 | 138 | |
roger_wee | 0:5c809b850f21 | 139 | ros::Subscriber<std_msgs::Int16> depth_pwm_subscriber("depth_pwm", depth_motor_callback); |
roger_wee | 0:5c809b850f21 | 140 | ros::Subscriber<std_msgs::Int16> yaw_pwm_subscriber("yaw_pwm", thrust_motor_callback); |
roger_wee | 0:5c809b850f21 | 141 | ros::Subscriber<std_msgs::Int16> fb_yaw_subscriber("yaw_pwm_feedback", yaw_feedback_callback); |
roger_wee | 0:5c809b850f21 | 142 | ros::Subscriber<std_msgs::Int16> fb_depth_subscriber("depth_pwm_feedback", depth_feedback_callback); |
roger_wee | 0:5c809b850f21 | 143 | |
roger_wee | 0:5c809b850f21 | 144 | int main() |
roger_wee | 0:5c809b850f21 | 145 | { |
roger_wee | 0:5c809b850f21 | 146 | nh.initNode(); |
roger_wee | 0:5c809b850f21 | 147 | nh.advertise(depth_publisher); |
roger_wee | 0:5c809b850f21 | 148 | // nh.advertise(killswitch_publisher); |
roger_wee | 0:5c809b850f21 | 149 | nh.subscribe(depth_pwm_subscriber); |
roger_wee | 0:5c809b850f21 | 150 | nh.subscribe(yaw_pwm_subscriber); |
roger_wee | 0:5c809b850f21 | 151 | nh.subscribe(fb_yaw_subscriber); |
roger_wee | 0:5c809b850f21 | 152 | nh.subscribe(fb_depth_subscriber); |
roger_wee | 0:5c809b850f21 | 153 | |
roger_wee | 0:5c809b850f21 | 154 | testLed = 0; |
roger_wee | 0:5c809b850f21 | 155 | |
roger_wee | 0:5c809b850f21 | 156 | while(1) |
roger_wee | 0:5c809b850f21 | 157 | { |
roger_wee | 0:5c809b850f21 | 158 | // If data is recieved, blink led |
roger_wee | 0:5c809b850f21 | 159 | if (device.readable()) |
roger_wee | 0:5c809b850f21 | 160 | { |
roger_wee | 0:5c809b850f21 | 161 | // Obtain depth |
roger_wee | 0:5c809b850f21 | 162 | depth = device.getc(); |
roger_wee | 0:5c809b850f21 | 163 | // toggle led on |
roger_wee | 0:5c809b850f21 | 164 | testLed = 1; |
roger_wee | 0:5c809b850f21 | 165 | } |
roger_wee | 0:5c809b850f21 | 166 | |
roger_wee | 0:5c809b850f21 | 167 | // Publish depth information |
roger_wee | 0:5c809b850f21 | 168 | depth_msg.data = depth; |
roger_wee | 0:5c809b850f21 | 169 | depth_publisher.publish(&depth_msg); |
roger_wee | 0:5c809b850f21 | 170 | |
roger_wee | 0:5c809b850f21 | 171 | // Depth thrust + feedback |
roger_wee | 0:5c809b850f21 | 172 | depthtot = m1pwm + depthfeedback; |
roger_wee | 0:5c809b850f21 | 173 | |
roger_wee | 0:5c809b850f21 | 174 | // Output motor pwm |
roger_wee | 0:5c809b850f21 | 175 | m1.pulsewidth_us(depthtot); |
roger_wee | 0:5c809b850f21 | 176 | m2.pulsewidth_us(depthtot); |
roger_wee | 0:5c809b850f21 | 177 | m3.pulsewidth_us(depthtot); |
roger_wee | 0:5c809b850f21 | 178 | m4.pulsewidth_us(depthtot); |
roger_wee | 0:5c809b850f21 | 179 | |
roger_wee | 0:5c809b850f21 | 180 | // Fwd thrust + feedback |
roger_wee | 0:5c809b850f21 | 181 | lthrust_tot = lthrustpwm + lfeedback; |
roger_wee | 0:5c809b850f21 | 182 | rthrust_tot = rthrustpwm + rfeedback; |
roger_wee | 0:5c809b850f21 | 183 | |
roger_wee | 0:5c809b850f21 | 184 | // Output pwm to fwd/rev thrusters |
roger_wee | 0:5c809b850f21 | 185 | lthrust.pulsewidth_us(lthrust_tot); |
roger_wee | 0:5c809b850f21 | 186 | rthrust.pulsewidth_us(rthrust_tot); |
roger_wee | 0:5c809b850f21 | 187 | |
roger_wee | 0:5c809b850f21 | 188 | nh.spinOnce(); |
roger_wee | 0:5c809b850f21 | 189 | wait_ms(2); |
roger_wee | 0:5c809b850f21 | 190 | testLed = 0; |
roger_wee | 0:5c809b850f21 | 191 | } |
roger_wee | 0:5c809b850f21 | 192 | } |