John Lam / Mbed 2 deprecated rosserial_mbed_carbase

carbase

Dependencies:   mbed mbed-rtos ros_lib_melodic

main.cpp@9:bfd88f814eec, 2021-02-25 (annotated)

Committer:
howanglam3
Date:
Thu Feb 25 05:54:25 2021 +0000
Revision:
9:bfd88f814eec
Parent:
8:e542adce95e9 
finish thread

Who changed what in which revision?

UserRevisionLine numberNew contents of line
howanglam3 3:a1a69ae50c18 1 #define BAUD 115200
Gary Servin 0:f48f597ef690 2 /*
Gary Servin 0:f48f597ef690 3 * rosserial Subscriber Example
Gary Servin 0:f48f597ef690 4 * Blinks an LED on callback
Gary Servin 0:f48f597ef690 5 */
Gary Servin 0:f48f597ef690 6 #include "mbed.h"
howanglam3 8:e542adce95e9 7 #include "rtos.h"
Gary Servin 0:f48f597ef690 8 #include <ros.h>
howanglam3 3:a1a69ae50c18 9 #include <std_msgs/Float32MultiArray.h>
howanglam3 3:a1a69ae50c18 10 #include <std_msgs/Int32MultiArray.h>
howanglam3 6:66de1f4abff4 11 #include "geometry_msgs/Twist.h"
howanglam3 4:aa8ef06b9469 12 //motor header
howanglam3 4:aa8ef06b9469 13 #include "MotorDrive.h"
howanglam3 4:aa8ef06b9469 14 #include "OmniWheel.h"
Gary Servin 0:f48f597ef690 15
howanglam3 9:bfd88f814eec 16 using namespace ros;
howanglam3 9:bfd88f814eec 17 using namespace std_msgs;
howanglam3 9:bfd88f814eec 18 using namespace geometry_msgs;
howanglam3 9:bfd88f814eec 19
howanglam3 9:bfd88f814eec 20
howanglam3 9:bfd88f814eec 21 NodeHandle nh;
howanglam3 4:aa8ef06b9469 22
Gary Servin 0:f48f597ef690 23 DigitalOut myled(LED1);
howanglam3 4:aa8ef06b9469 24 DigitalOut Refer_Volt_3_3 = PB_1;
howanglam3 4:aa8ef06b9469 25 DigitalOut motor_enable = PC_7;
howanglam3 4:aa8ef06b9469 26 DigitalOut motor_stop[] = {PB_6, PC_5, PC_0};
howanglam3 7:fe1f52d848fd 27
howanglam3 9:bfd88f814eec 28
howanglam3 8:e542adce95e9 29 // thread handling wheel
howanglam3 9:bfd88f814eec 30 Twist wheel_data; //global
howanglam3 8:e542adce95e9 31 Thread wheel_calc;
howanglam3 9:bfd88f814eec 32 Twist save;
howanglam3 7:fe1f52d848fd 33
howanglam3 4:aa8ef06b9469 34 // -> Motor PWN Pin
howanglam3 4:aa8ef06b9469 35 PwmOut motor_pwm[] = {PA_9, PC_8, PA_11};
howanglam3 4:aa8ef06b9469 36 //motor number
howanglam3 4:aa8ef06b9469 37 const int motor_num = sizeof(motor_pwm) / sizeof(PwmOut);
howanglam3 4:aa8ef06b9469 38 //motor pwm period (ms)
howanglam3 4:aa8ef06b9469 39 const int pwm_period = 20;
howanglam3 4:aa8ef06b9469 40 //motor stop pwm
howanglam3 4:aa8ef06b9469 41 const double stop_pwm = 0.5;
howanglam3 4:aa8ef06b9469 42
howanglam3 4:aa8ef06b9469 43 MotorDrive motor_drive;
howanglam3 4:aa8ef06b9469 44
howanglam3 4:aa8ef06b9469 45 void motor_init() {
howanglam3 4:aa8ef06b9469 46 //init motor
howanglam3 4:aa8ef06b9469 47 motor_enable = 0;
howanglam3 4:aa8ef06b9469 48
howanglam3 4:aa8ef06b9469 49 for(int i = 0; i < motor_num; i++){
howanglam3 4:aa8ef06b9469 50 motor_pwm[i].period_ms(pwm_period);
howanglam3 4:aa8ef06b9469 51 motor_pwm[i] = stop_pwm;
howanglam3 4:aa8ef06b9469 52 }
howanglam3 4:aa8ef06b9469 53 //ref voltage
howanglam3 4:aa8ef06b9469 54 Refer_Volt_3_3 = 1;
howanglam3 4:aa8ef06b9469 55 wait(1);
howanglam3 4:aa8ef06b9469 56 motor_enable = 1;
howanglam3 4:aa8ef06b9469 57 }
howanglam3 4:aa8ef06b9469 58
howanglam3 4:aa8ef06b9469 59 void motorMove(int motor_index, double pwm){
howanglam3 5:e9a3ee77cb97 60 motor_stop[motor_index] = 0;
howanglam3 4:aa8ef06b9469 61 motor_pwm[motor_index] = pwm;
howanglam3 4:aa8ef06b9469 62 }
howanglam3 5:e9a3ee77cb97 63 void motorStop(int motor_index){
howanglam3 5:e9a3ee77cb97 64 motor_stop[motor_index] = 1;
howanglam3 5:e9a3ee77cb97 65 motor_pwm[motor_index] = 0.5;
howanglam3 5:e9a3ee77cb97 66 }
howanglam3 6:66de1f4abff4 67 float round(float x){
howanglam3 6:66de1f4abff4 68 float value = (int)(x*100+0.5);
howanglam3 6:66de1f4abff4 69 return (float)value/100;
howanglam3 6:66de1f4abff4 70 }
howanglam3 9:bfd88f814eec 71 void baseCallback(const Float32MultiArray& base_msg){
howanglam3 6:66de1f4abff4 72 double x = (double)round(base_msg.data[1]);
howanglam3 6:66de1f4abff4 73 double y = (double)round(base_msg.data[0]);
howanglam3 6:66de1f4abff4 74 double z = (double)round(base_msg.data[2]);
howanglam3 6:66de1f4abff4 75 OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(x, y, z);
howanglam3 4:aa8ef06b9469 76 vector<double> omni_wheel_pwm = omni_wheel_data.getPwm();
howanglam3 6:66de1f4abff4 77 //for(int i = 0; i < motor_num; i++){
howanglam3 6:66de1f4abff4 78 // if(omni_wheel_pwm[i]==0.5)
howanglam3 6:66de1f4abff4 79 // motorStop(i);
howanglam3 6:66de1f4abff4 80 // else
howanglam3 6:66de1f4abff4 81 // motorMove(i, omni_wheel_pwm[i]);
howanglam3 6:66de1f4abff4 82 // }
howanglam3 6:66de1f4abff4 83
howanglam3 6:66de1f4abff4 84 if(omni_wheel_pwm[0] == 0.5)
howanglam3 6:66de1f4abff4 85 motorStop(0);
howanglam3 6:66de1f4abff4 86 else
howanglam3 6:66de1f4abff4 87 motorMove(0, omni_wheel_pwm[0]);
howanglam3 6:66de1f4abff4 88
howanglam3 6:66de1f4abff4 89 if(omni_wheel_pwm[1] == 0.5)
howanglam3 6:66de1f4abff4 90 motorStop(1);
howanglam3 6:66de1f4abff4 91 else
howanglam3 6:66de1f4abff4 92 motorMove(1, omni_wheel_pwm[1]);
howanglam3 6:66de1f4abff4 93
howanglam3 6:66de1f4abff4 94 if(omni_wheel_pwm[2] == 0.5)
howanglam3 6:66de1f4abff4 95 motorStop(2);
howanglam3 6:66de1f4abff4 96 else
howanglam3 6:66de1f4abff4 97 motorMove(2, omni_wheel_pwm[2]);
howanglam3 4:aa8ef06b9469 98 }
Gary Servin 0:f48f597ef690 99
howanglam3 9:bfd88f814eec 100 void messageCb(const Float32MultiArray& toggle_msg){
Gary Servin 0:f48f597ef690 101 myled = !myled; // blink the led
Gary Servin 0:f48f597ef690 102 }
Gary Servin 0:f48f597ef690 103
howanglam3 9:bfd88f814eec 104 void twCallback(const Twist& _msg) {
howanglam3 6:66de1f4abff4 105 OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(_msg.linear.x, _msg.linear.y, _msg.angular.z);
howanglam3 6:66de1f4abff4 106
howanglam3 6:66de1f4abff4 107 vector<double> omni_wheel_pwm = omni_wheel_data.getPwm(); // Get PWM data
howanglam3 6:66de1f4abff4 108
howanglam3 6:66de1f4abff4 109 for(int i = 0; i < motor_num; i++) // Send PWM to each motor
howanglam3 6:66de1f4abff4 110 if(omni_wheel_pwm[i] != stop_pwm)
howanglam3 6:66de1f4abff4 111 motorMove(i, omni_wheel_pwm[i]);
howanglam3 6:66de1f4abff4 112 else
howanglam3 6:66de1f4abff4 113 motorStop(i);
howanglam3 6:66de1f4abff4 114 }
howanglam3 6:66de1f4abff4 115
howanglam3 8:e542adce95e9 116 void wheel_calc_thread() {
howanglam3 8:e542adce95e9 117 while (true) {
howanglam3 9:bfd88f814eec 118
howanglam3 9:bfd88f814eec 119 bool x = wheel_data.linear.x==save.linear.x;
howanglam3 9:bfd88f814eec 120 bool y = wheel_data.linear.y==save.linear.y;
howanglam3 9:bfd88f814eec 121 bool z = wheel_data.angular.z==save.angular.z;
howanglam3 9:bfd88f814eec 122
howanglam3 9:bfd88f814eec 123 // calc
howanglam3 9:bfd88f814eec 124 if(!(x && y && z))
howanglam3 9:bfd88f814eec 125 {
howanglam3 9:bfd88f814eec 126 twCallback(wheel_data);
howanglam3 9:bfd88f814eec 127 wait(5);
howanglam3 9:bfd88f814eec 128 }
howanglam3 9:bfd88f814eec 129 save = wheel_data;
howanglam3 8:e542adce95e9 130 }
howanglam3 7:fe1f52d848fd 131
howanglam3 8:e542adce95e9 132 }
howanglam3 7:fe1f52d848fd 133
howanglam3 9:bfd88f814eec 134 void saveCallback(const Twist& _msg) {
howanglam3 8:e542adce95e9 135 wheel_data = _msg;
howanglam3 7:fe1f52d848fd 136 }
howanglam3 7:fe1f52d848fd 137
howanglam3 9:bfd88f814eec 138 Subscriber<Float32MultiArray> subax("moving_base", &baseCallback);
howanglam3 9:bfd88f814eec 139 Subscriber<Float32MultiArray> subbt("fcn_button", &messageCb);
howanglam3 9:bfd88f814eec 140 Subscriber<Twist> subtw("base_twist", &saveCallback);
Gary Servin 0:f48f597ef690 141 int main() {
howanglam3 9:bfd88f814eec 142 //Rate rate(10);
howanglam3 3:a1a69ae50c18 143 nh.getHardware()->setBaud(BAUD);
Gary Servin 0:f48f597ef690 144 nh.initNode();
howanglam3 8:e542adce95e9 145 wheel_calc.start(wheel_calc_thread);
howanglam3 6:66de1f4abff4 146 // nh.subscribe(subax);
howanglam3 3:a1a69ae50c18 147 nh.subscribe(subbt);
howanglam3 7:fe1f52d848fd 148 nh.subscribe(subtw);
howanglam3 7:fe1f52d848fd 149
howanglam3 4:aa8ef06b9469 150 motor_init();
Gary Servin 0:f48f597ef690 151 while (1) {
howanglam3 3:a1a69ae50c18 152 nh.spinOnce();
howanglam3 4:aa8ef06b9469 153
howanglam3 4:aa8ef06b9469 154
howanglam3 4:aa8ef06b9469 155 if(nh.connected())
howanglam3 4:aa8ef06b9469 156 myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
howanglam3 4:aa8ef06b9469 157 else
howanglam3 4:aa8ef06b9469 158 myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
Gary Servin 0:f48f597ef690 159 }
howanglam3 4:aa8ef06b9469 160
Gary Servin 0:f48f597ef690 161 }