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@1:c7cfd52db721, 2018-08-01 (annotated)

Committer:
roger_wee
Date:
Wed Aug 01 08:12:41 2018 +0000
Revision:
1:c7cfd52db721
Parent:
0:5c809b850f21 
seadragon_code

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 0:5c809b850f21 78 if (msg.data == 0)
roger_wee 0:5c809b850f21 79 {
roger_wee 0:5c809b850f21 80 lthrustpwm = rthrustpwm = PWMBASELINE;
roger_wee 0:5c809b850f21 81 }
roger_wee 0:5c809b850f21 82 else if (msg.data < 0)
roger_wee 0:5c809b850f21 83 {
roger_wee 0:5c809b850f21 84 lthrustpwm = PWMBASELINE - 25 + msg.data;
roger_wee 0:5c809b850f21 85 rthrustpwm = PWMBASELINE - 25 + msg.data;
roger_wee 0:5c809b850f21 86 }
roger_wee 0:5c809b850f21 87 else{
roger_wee 0:5c809b850f21 88 // Update yaw motor pwm
roger_wee 0:5c809b850f21 89 lthrustpwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 90 rthrustpwm = PWMBASELINE + 25 + msg.data;
roger_wee 0:5c809b850f21 91 }
roger_wee 0:5c809b850f21 92 }
roger_wee 0:5c809b850f21 93
roger_wee 0:5c809b850f21 94 // Receives values ranging [0:75]
roger_wee 0:5c809b850f21 95 void yaw_feedback_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 96 {
roger_wee 0:5c809b850f21 97 if (lthrustpwm == PWMBASELINE){
roger_wee 0:5c809b850f21 98 if (msg.data > 0.25){
roger_wee 0:5c809b850f21 99 lfeedback = (msg.data + 29) * -1;
roger_wee 0:5c809b850f21 100 rfeedback = (msg.data + 29);
roger_wee 0:5c809b850f21 101 }
roger_wee 0:5c809b850f21 102 else if (msg.data < -0.25){
roger_wee 0:5c809b850f21 103 lfeedback = (msg.data - 29) * -1;
roger_wee 0:5c809b850f21 104 rfeedback = (msg.data - 29);
roger_wee 0:5c809b850f21 105 }
roger_wee 0:5c809b850f21 106 else{
roger_wee 0:5c809b850f21 107 lfeedback = rfeedback = 0;
roger_wee 0:5c809b850f21 108 }
roger_wee 0:5c809b850f21 109 }
roger_wee 0:5c809b850f21 110 else{
roger_wee 0:5c809b850f21 111 lfeedback = msg.data * -1;
roger_wee 0:5c809b850f21 112 rfeedback = msg.data;
roger_wee 0:5c809b850f21 113 }
roger_wee 0:5c809b850f21 114 }
roger_wee 0:5c809b850f21 115
roger_wee 0:5c809b850f21 116 // Receives depth feedback vallues from [0:50]
roger_wee 0:5c809b850f21 117 void depth_feedback_callback( const std_msgs::Int16& msg)
roger_wee 0:5c809b850f21 118 {
roger_wee 0:5c809b850f21 119 // depth feedback control
roger_wee 0:5c809b850f21 120 if (msg.data > 1)
roger_wee 0:5c809b850f21 121 {
roger_wee 0:5c809b850f21 122 depthfeedback = 25 + msg.data;
roger_wee 0:5c809b850f21 123 }
roger_wee 0:5c809b850f21 124 else if (msg.data < -1)
roger_wee 0:5c809b850f21 125 {
roger_wee 0:5c809b850f21 126 depthfeedback = -25 + msg.data;
roger_wee 0:5c809b850f21 127 }
roger_wee 0:5c809b850f21 128 else{
roger_wee 0:5c809b850f21 129 depthfeedback = 0;
roger_wee 0:5c809b850f21 130 }
roger_wee 0:5c809b850f21 131 }
roger_wee 0:5c809b850f21 132
roger_wee 1:c7cfd52db721 133 void pan_callback( const std_msgs::Int16& msg)
roger_wee 1:c7cfd52db721 134 {
roger_wee 1:c7cfd52db721 135 pan_servo = msg.data/100.0;
roger_wee 1:c7cfd52db721 136 }
roger_wee 1:c7cfd52db721 137
roger_wee 0:5c809b850f21 138 ros::Subscriber<std_msgs::Int16> depth_pwm_subscriber("depth_pwm", depth_motor_callback);
roger_wee 0:5c809b850f21 139 ros::Subscriber<std_msgs::Int16> yaw_pwm_subscriber("yaw_pwm", thrust_motor_callback);
roger_wee 0:5c809b850f21 140 ros::Subscriber<std_msgs::Int16> fb_yaw_subscriber("yaw_pwm_feedback", yaw_feedback_callback);
roger_wee 0:5c809b850f21 141 ros::Subscriber<std_msgs::Int16> fb_depth_subscriber("depth_pwm_feedback", depth_feedback_callback);
roger_wee 1:c7cfd52db721 142 ros::Subscriber<std_msgs::Int16> pan_servo_subscriber("pan_servo", pan_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.subscribe(depth_pwm_subscriber);
roger_wee 0:5c809b850f21 149 nh.subscribe(yaw_pwm_subscriber);
roger_wee 0:5c809b850f21 150 nh.subscribe(fb_yaw_subscriber);
roger_wee 0:5c809b850f21 151 nh.subscribe(fb_depth_subscriber);
roger_wee 1:c7cfd52db721 152 nh.subscribe(pan_servo_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 1:c7cfd52db721 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 1:c7cfd52db721 188 // actuate pan tilt camera
roger_wee 1:c7cfd52db721 189 myservo = pan_servo;
roger_wee 1:c7cfd52db721 190
roger_wee 0:5c809b850f21 191 nh.spinOnce();
roger_wee 1:c7cfd52db721 192 wait_ms(5);
roger_wee 0:5c809b850f21 193 testLed = 0;
roger_wee 0:5c809b850f21 194 }
roger_wee 0:5c809b850f21 195 }