Rogelio Vazquez / Mbed 2 deprecated Seadragon_motors_ros_2019

Hardware interface for seadragon motors

Dependencies:   mbed Servo ros_lib_kinetic

main.cpp@2:48a9c25e5d26, 2019-06-28 (annotated)

Committer:
roger_wee
Date:
Fri Jun 28 19:58:04 2019 +0000
Revision:
2:48a9c25e5d26
Parent:
1:c7cfd52db721 
Working 2019 thrusters

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roger_wee 0:5c809b850f21 1 #include "mbed.h"
roger_wee 1:c7cfd52db721 2 #include "Servo.h"
roger_wee 0:5c809b850f21 3 #include <ros.h>
roger_wee 0:5c809b850f21 4 #include <std_msgs/Int16.h>
roger_wee 1:c7cfd52db721 5 #include <std_msgs/Bool.h>
roger_wee 0:5c809b850f21 6
roger_wee 0:5c809b850f21 7 //------------Declare Global Variables------------------//
roger_wee 0:5c809b850f21 8
roger_wee 0:5c809b850f21 9 // (m1) (m2)
roger_wee 0:5c809b850f21 10 // | |
roger_wee 0:5c809b850f21 11 // | |
roger_wee 0:5c809b850f21 12 // (Ltrhust)| |(Rthrust)
roger_wee 0:5c809b850f21 13 // | |
roger_wee 0:5c809b850f21 14 // | |
roger_wee 0:5c809b850f21 15 // (m3) (m4)
roger_wee 0:5c809b850f21 16
roger_wee 0:5c809b850f21 17 // Initialize PWM objects
roger_wee 0:5c809b850f21 18 PwmOut m1(D8);
roger_wee 0:5c809b850f21 19 PwmOut m2(D9);
roger_wee 0:5c809b850f21 20 PwmOut m3(D10);
roger_wee 0:5c809b850f21 21 PwmOut m4(D11);
roger_wee 0:5c809b850f21 22 PwmOut lthrust(D12);
roger_wee 0:5c809b850f21 23 PwmOut rthrust(D14);
roger_wee 0:5c809b850f21 24
roger_wee 0:5c809b850f21 25 int PWMBASELINE = 1500;
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 1:c7cfd52db721 43 // Tx - 11, Rx - 9
roger_wee 0:5c809b850f21 44 RawSerial device(PA_15, PB_7);
roger_wee 0:5c809b850f21 45 DigitalOut testLed(LED1);
roger_wee 1:c7cfd52db721 46 Servo myservo(D3);
roger_wee 1:c7cfd52db721 47 double pan_servo = 0;
roger_wee 0:5c809b850f21 48
roger_wee 0:5c809b850f21 49 //--------------ROS Node Configuration---------------
roger_wee 0:5c809b850f21 50 ros::NodeHandle nh;
roger_wee 0:5c809b850f21 51
roger_wee 0:5c809b850f21 52 std_msgs::Int16 depth_msg;
roger_wee 0:5c809b850f21 53 int depth = 0;
roger_wee 0:5c809b850f21 54 ros::Publisher depth_publisher("depth", &depth_msg);
roger_wee 0:5c809b850f21 55
roger_wee 0:5c809b850f21 56 void depth_motor_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 57 {
roger_wee 0:5c809b850f21 58 if (msg.data == 0)
roger_wee 0:5c809b850f21 59 {
roger_wee 0:5c809b850f21 60 m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE;
roger_wee 0:5c809b850f21 61 }
roger_wee 0:5c809b850f21 62 else if (msg.data < 0)
roger_wee 0:5c809b850f21 63 {
roger_wee 0:5c809b850f21 64 m1pwm = m2pwm = m3pwm = m4pwm = PWMBASELINE - 25 + msg.data;
roger_wee 0:5c809b850f21 65 }
roger_wee 0:5c809b850f21 66 else{
roger_wee 0:5c809b850f21 67 // Update motor pwm
roger_wee 0:5c809b850f21 68 m1pwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 69 m2pwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 70 m3pwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 71 m4pwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 72 }
roger_wee 0:5c809b850f21 73 }
roger_wee 0:5c809b850f21 74
roger_wee 0:5c809b850f21 75 // Forward thrust callback returns valules [0:150]
roger_wee 0:5c809b850f21 76 void thrust_motor_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 77 {
roger_wee 2:48a9c25e5d26 78 // Updates pwm values for forward thrust motors
roger_wee 0:5c809b850f21 79 if (msg.data == 0)
roger_wee 0:5c809b850f21 80 {
roger_wee 2:48a9c25e5d26 81 // Stop motors
roger_wee 0:5c809b850f21 82 lthrustpwm = rthrustpwm = PWMBASELINE;
roger_wee 0:5c809b850f21 83 }
roger_wee 0:5c809b850f21 84 else if (msg.data < 0)
roger_wee 0:5c809b850f21 85 {
roger_wee 2:48a9c25e5d26 86 // Go reverse
roger_wee 0:5c809b850f21 87 lthrustpwm = PWMBASELINE - 25 + msg.data;
roger_wee 0:5c809b850f21 88 rthrustpwm = PWMBASELINE - 25 + msg.data;
roger_wee 0:5c809b850f21 89 }
roger_wee 0:5c809b850f21 90 else{
roger_wee 2:48a9c25e5d26 91 // Go forward
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 }
roger_wee 0:5c809b850f21 96
roger_wee 0:5c809b850f21 97 // Receives values ranging [0:75]
roger_wee 0:5c809b850f21 98 void yaw_feedback_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 99 {
roger_wee 0:5c809b850f21 100 if (lthrustpwm == PWMBASELINE){
roger_wee 0:5c809b850f21 101 if (msg.data > 0.25){
roger_wee 0:5c809b850f21 102 lfeedback = (msg.data + 29) * -1;
roger_wee 0:5c809b850f21 103 rfeedback = (msg.data + 29);
roger_wee 0:5c809b850f21 104 }
roger_wee 0:5c809b850f21 105 else if (msg.data < -0.25){
roger_wee 0:5c809b850f21 106 lfeedback = (msg.data - 29) * -1;
roger_wee 0:5c809b850f21 107 rfeedback = (msg.data - 29);
roger_wee 0:5c809b850f21 108 }
roger_wee 0:5c809b850f21 109 else{
roger_wee 0:5c809b850f21 110 lfeedback = rfeedback = 0;
roger_wee 0:5c809b850f21 111 }
roger_wee 0:5c809b850f21 112 }
roger_wee 0:5c809b850f21 113 else{
roger_wee 0:5c809b850f21 114 lfeedback = msg.data * -1;
roger_wee 0:5c809b850f21 115 rfeedback = msg.data;
roger_wee 0:5c809b850f21 116 }
roger_wee 0:5c809b850f21 117 }
roger_wee 0:5c809b850f21 118
roger_wee 0:5c809b850f21 119 // Receives depth feedback vallues from [0:50]
roger_wee 0:5c809b850f21 120 void depth_feedback_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 121 {
roger_wee 0:5c809b850f21 122 // depth feedback control
roger_wee 0:5c809b850f21 123 if (msg.data > 1)
roger_wee 0:5c809b850f21 124 {
roger_wee 0:5c809b850f21 125 depthfeedback = 25 + msg.data;
roger_wee 0:5c809b850f21 126 }
roger_wee 0:5c809b850f21 127 else if (msg.data < -1)
roger_wee 0:5c809b850f21 128 {
roger_wee 0:5c809b850f21 129 depthfeedback = -25 + msg.data;
roger_wee 0:5c809b850f21 130 }
roger_wee 0:5c809b850f21 131 else{
roger_wee 0:5c809b850f21 132 depthfeedback = 0;
roger_wee 0:5c809b850f21 133 }
roger_wee 0:5c809b850f21 134 }
roger_wee 0:5c809b850f21 135
roger_wee 1:c7cfd52db721 136 void pan_callback( const std_msgs::Int16& msg)
roger_wee 1:c7cfd52db721 137 {
roger_wee 1:c7cfd52db721 138 pan_servo = msg.data/100.0;
roger_wee 1:c7cfd52db721 139 }
roger_wee 1:c7cfd52db721 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 1:c7cfd52db721 145 ros::Subscriber<std_msgs::Int16> pan_servo_subscriber("pan_servo", pan_callback);
roger_wee 0:5c809b850f21 146
roger_wee 0:5c809b850f21 147 int main()
roger_wee 0:5c809b850f21 148 {
roger_wee 0:5c809b850f21 149 nh.initNode();
roger_wee 0:5c809b850f21 150 nh.advertise(depth_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);
roger_wee 1:c7cfd52db721 155 nh.subscribe(pan_servo_subscriber);
roger_wee 0:5c809b850f21 156
roger_wee 0:5c809b850f21 157 testLed = 0;
roger_wee 0:5c809b850f21 158
roger_wee 0:5c809b850f21 159 while(1)
roger_wee 1:c7cfd52db721 160 {
roger_wee 0:5c809b850f21 161 // If data is recieved, blink led
roger_wee 0:5c809b850f21 162 if (device.readable())
roger_wee 0:5c809b850f21 163 {
roger_wee 0:5c809b850f21 164 // Obtain depth
roger_wee 0:5c809b850f21 165 depth = device.getc();
roger_wee 0:5c809b850f21 166 // toggle led on
roger_wee 0:5c809b850f21 167 testLed = 1;
roger_wee 0:5c809b850f21 168 }
roger_wee 0:5c809b850f21 169
roger_wee 0:5c809b850f21 170 // Publish depth information
roger_wee 0:5c809b850f21 171 depth_msg.data = depth;
roger_wee 0:5c809b850f21 172 depth_publisher.publish(&depth_msg);
roger_wee 0:5c809b850f21 173
roger_wee 0:5c809b850f21 174 // Depth thrust + feedback
roger_wee 0:5c809b850f21 175 depthtot = m1pwm + depthfeedback;
roger_wee 0:5c809b850f21 176
roger_wee 0:5c809b850f21 177 // Output motor pwm
roger_wee 0:5c809b850f21 178 m1.pulsewidth_us(depthtot);
roger_wee 0:5c809b850f21 179 m2.pulsewidth_us(depthtot);
roger_wee 0:5c809b850f21 180 m3.pulsewidth_us(depthtot);
roger_wee 0:5c809b850f21 181 m4.pulsewidth_us(depthtot);
roger_wee 0:5c809b850f21 182
roger_wee 0:5c809b850f21 183 // Fwd thrust + feedback
roger_wee 0:5c809b850f21 184 lthrust_tot = lthrustpwm + lfeedback;
roger_wee 0:5c809b850f21 185 rthrust_tot = rthrustpwm + rfeedback;
roger_wee 0:5c809b850f21 186
roger_wee 0:5c809b850f21 187 // Output pwm to fwd/rev thrusters
roger_wee 0:5c809b850f21 188 lthrust.pulsewidth_us(lthrust_tot);
roger_wee 0:5c809b850f21 189 rthrust.pulsewidth_us(rthrust_tot);
roger_wee 0:5c809b850f21 190
roger_wee 1:c7cfd52db721 191 // actuate pan tilt camera
roger_wee 1:c7cfd52db721 192 myservo = pan_servo;
roger_wee 1:c7cfd52db721 193
roger_wee 0:5c809b850f21 194 nh.spinOnce();
roger_wee 1:c7cfd52db721 195 wait_ms(5);
roger_wee 0:5c809b850f21 196 testLed = 0;
roger_wee 0:5c809b850f21 197 }
roger_wee 0:5c809b850f21 198 }