John Lam / Mbed 2 deprecated rosserial_mbed_carbase

carbase

Dependencies:   mbed mbed-rtos ros_lib_melodic

main.cpp@8:e542adce95e9, 2021-02-18 (annotated)

Committer:
howanglam3
Date:
Thu Feb 18 10:15:52 2021 +0000
Revision:
8:e542adce95e9
Parent:
7:fe1f52d848fd
Child:
9:bfd88f814eec
error in include

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