John Lam / Mbed 2 deprecated rosserial_mbed_carbase

carbase

Dependencies:   mbed mbed-rtos ros_lib_melodic

main.cpp@5:e9a3ee77cb97, 2021-02-04 (annotated)

Committer:
howanglam3
Date:
Thu Feb 04 08:40:48 2021 +0000
Revision:
5:e9a3ee77cb97
Parent:
4:aa8ef06b9469
Child:
6:66de1f4abff4
yeah

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 4:aa8ef06b9469 10 //motor header
howanglam3 4:aa8ef06b9469 11 #include "MotorDrive.h"
howanglam3 4:aa8ef06b9469 12 #include "OmniWheel.h"
Gary Servin 0:f48f597ef690 13
Gary Servin 0:f48f597ef690 14 ros::NodeHandle nh;
howanglam3 4:aa8ef06b9469 15
Gary Servin 0:f48f597ef690 16 DigitalOut myled(LED1);
howanglam3 4:aa8ef06b9469 17 DigitalOut Refer_Volt_3_3 = PB_1;
howanglam3 4:aa8ef06b9469 18 DigitalOut motor_enable = PC_7;
howanglam3 4:aa8ef06b9469 19 DigitalOut motor_stop[] = {PB_6, PC_5, PC_0};
howanglam3 4:aa8ef06b9469 20 // -> Motor PWN Pin
howanglam3 4:aa8ef06b9469 21 PwmOut motor_pwm[] = {PA_9, PC_8, PA_11};
howanglam3 4:aa8ef06b9469 22 //motor number
howanglam3 4:aa8ef06b9469 23 const int motor_num = sizeof(motor_pwm) / sizeof(PwmOut);
howanglam3 4:aa8ef06b9469 24 //motor pwm period (ms)
howanglam3 4:aa8ef06b9469 25 const int pwm_period = 20;
howanglam3 4:aa8ef06b9469 26 //motor stop pwm
howanglam3 4:aa8ef06b9469 27 const double stop_pwm = 0.5;
howanglam3 4:aa8ef06b9469 28
howanglam3 4:aa8ef06b9469 29 MotorDrive motor_drive;
howanglam3 4:aa8ef06b9469 30
howanglam3 4:aa8ef06b9469 31 void motor_init() {
howanglam3 4:aa8ef06b9469 32 //init motor
howanglam3 4:aa8ef06b9469 33 motor_enable = 0;
howanglam3 4:aa8ef06b9469 34
howanglam3 4:aa8ef06b9469 35 for(int i = 0; i < motor_num; i++){
howanglam3 4:aa8ef06b9469 36 motor_pwm[i].period_ms(pwm_period);
howanglam3 4:aa8ef06b9469 37 motor_pwm[i] = stop_pwm;
howanglam3 4:aa8ef06b9469 38 }
howanglam3 4:aa8ef06b9469 39 //ref voltage
howanglam3 4:aa8ef06b9469 40 Refer_Volt_3_3 = 1;
howanglam3 4:aa8ef06b9469 41 wait(1);
howanglam3 4:aa8ef06b9469 42 motor_enable = 1;
howanglam3 4:aa8ef06b9469 43 }
howanglam3 4:aa8ef06b9469 44
howanglam3 4:aa8ef06b9469 45 void motorMove(int motor_index, double pwm){
howanglam3 5:e9a3ee77cb97 46 motor_stop[motor_index] = 0;
howanglam3 4:aa8ef06b9469 47 motor_pwm[motor_index] = pwm;
howanglam3 4:aa8ef06b9469 48 }
howanglam3 5:e9a3ee77cb97 49 void motorStop(int motor_index){
howanglam3 5:e9a3ee77cb97 50 motor_stop[motor_index] = 1;
howanglam3 5:e9a3ee77cb97 51 motor_pwm[motor_index] = 0.5;
howanglam3 5:e9a3ee77cb97 52 }
howanglam3 4:aa8ef06b9469 53 void baseCallback(const std_msgs::Float32MultiArray& base_msg){
howanglam3 4:aa8ef06b9469 54 OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(base_msg.data[1], base_msg.data[0], base_msg.data[2]);
howanglam3 4:aa8ef06b9469 55 vector<double> omni_wheel_pwm = omni_wheel_data.getPwm();
howanglam3 4:aa8ef06b9469 56 for(int i = 0; i < motor_num; i++){
howanglam3 5:e9a3ee77cb97 57 if(omni_wheel_pwm[i]==0.5)
howanglam3 5:e9a3ee77cb97 58 motorStop(i);
howanglam3 5:e9a3ee77cb97 59 else
howanglam3 5:e9a3ee77cb97 60 motorMove(i, omni_wheel_pwm[i]);
howanglam3 4:aa8ef06b9469 61 }
howanglam3 4:aa8ef06b9469 62 }
Gary Servin 0:f48f597ef690 63
howanglam3 3:a1a69ae50c18 64 void messageCb(const std_msgs::Float32MultiArray& toggle_msg){
Gary Servin 0:f48f597ef690 65 myled = !myled; // blink the led
Gary Servin 0:f48f597ef690 66 }
Gary Servin 0:f48f597ef690 67
howanglam3 4:aa8ef06b9469 68 ros::Subscriber<std_msgs::Float32MultiArray> subax("moving_base", &baseCallback);
howanglam3 3:a1a69ae50c18 69 ros::Subscriber<std_msgs::Float32MultiArray> subbt("fcn_button", &messageCb);
Gary Servin 0:f48f597ef690 70 int main() {
howanglam3 3:a1a69ae50c18 71 nh.getHardware()->setBaud(BAUD);
Gary Servin 0:f48f597ef690 72 nh.initNode();
howanglam3 3:a1a69ae50c18 73 nh.subscribe(subax);
howanglam3 3:a1a69ae50c18 74 nh.subscribe(subbt);
howanglam3 4:aa8ef06b9469 75 motor_init();
Gary Servin 0:f48f597ef690 76 while (1) {
howanglam3 3:a1a69ae50c18 77 nh.spinOnce();
howanglam3 4:aa8ef06b9469 78
howanglam3 4:aa8ef06b9469 79
howanglam3 4:aa8ef06b9469 80 if(nh.connected())
howanglam3 4:aa8ef06b9469 81 myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
howanglam3 4:aa8ef06b9469 82 else
howanglam3 4:aa8ef06b9469 83 myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
Gary Servin 0:f48f597ef690 84 }
howanglam3 4:aa8ef06b9469 85
Gary Servin 0:f48f597ef690 86 }