![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |