Rogelio Vazquez / Mbed 2 deprecated Seadragon_motors_ros_v2

Hardware interface for seadragon motors

Dependencies:   Servo mbed ros_lib_kinetic

Fork of Seadragon_motors_ros by Rogelio Vazquez

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?

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;
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 }