John Lam / Mbed 2 deprecated rosserial_mbed_carbase

carbase

Dependencies:   mbed mbed-rtos ros_lib_melodic

main.cpp@4:aa8ef06b9469, 2021-01-30 (annotated)

Committer:
howanglam3
Date:
Sat Jan 30 12:05:58 2021 +0000
Revision:
4:aa8ef06b9469
Parent:
3:a1a69ae50c18
Child:
5:e9a3ee77cb97
updated

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 4:aa8ef06b9469 46 if(pwm == 0.5)
howanglam3 4:aa8ef06b9469 47 motor_stop[motor_index] = 1;
howanglam3 4:aa8ef06b9469 48 else
howanglam3 4:aa8ef06b9469 49 motor_stop[motor_index] = 0;
howanglam3 4:aa8ef06b9469 50 motor_pwm[motor_index] = pwm;
howanglam3 4:aa8ef06b9469 51 }
howanglam3 4:aa8ef06b9469 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 4:aa8ef06b9469 57 motorMove(i, omni_wheel_pwm[i]);
howanglam3 4:aa8ef06b9469 58 }
howanglam3 4:aa8ef06b9469 59 }
Gary Servin 0:f48f597ef690 60
howanglam3 3:a1a69ae50c18 61 void messageCb(const std_msgs::Float32MultiArray& toggle_msg){
Gary Servin 0:f48f597ef690 62 myled = !myled; // blink the led
Gary Servin 0:f48f597ef690 63 }
Gary Servin 0:f48f597ef690 64
howanglam3 4:aa8ef06b9469 65 ros::Subscriber<std_msgs::Float32MultiArray> subax("moving_base", &baseCallback);
howanglam3 3:a1a69ae50c18 66 ros::Subscriber<std_msgs::Float32MultiArray> subbt("fcn_button", &messageCb);
Gary Servin 0:f48f597ef690 67 int main() {
howanglam3 3:a1a69ae50c18 68 nh.getHardware()->setBaud(BAUD);
Gary Servin 0:f48f597ef690 69 nh.initNode();
howanglam3 3:a1a69ae50c18 70 nh.subscribe(subax);
howanglam3 3:a1a69ae50c18 71 nh.subscribe(subbt);
howanglam3 4:aa8ef06b9469 72 motor_init();
howanglam3 4:aa8ef06b9469 73
Gary Servin 0:f48f597ef690 74 while (1) {
howanglam3 3:a1a69ae50c18 75 nh.spinOnce();
howanglam3 4:aa8ef06b9469 76
howanglam3 4:aa8ef06b9469 77
howanglam3 4:aa8ef06b9469 78 if(nh.connected())
howanglam3 4:aa8ef06b9469 79 myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
howanglam3 4:aa8ef06b9469 80 else
howanglam3 4:aa8ef06b9469 81 myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
Gary Servin 0:f48f597ef690 82 }
howanglam3 4:aa8ef06b9469 83
Gary Servin 0:f48f597ef690 84 }