John Lam / Mbed 2 deprecated rosserial_mbed_carbase

carbase

Dependencies:   mbed mbed-rtos ros_lib_melodic

main.cpp@7:fe1f52d848fd, 2021-02-18 (annotated)

Committer:
howanglam3
Date:
Thu Feb 18 09:58:58 2021 +0000
Revision:
7:fe1f52d848fd
Parent:
6:66de1f4abff4
Child:
8:e542adce95e9
previous

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