Benson Sin / Mbed 2 deprecated Robocon_2021_carbase

CityU Dream Development

Dependencies:   mbed ros_lib_melodic

main.cpp@0:c2b6f8b48076, 2021-02-25 (annotated)

Committer:
bensonsinsin998
Date:
Thu Feb 25 07:41:29 2021 +0000
Revision:
0:c2b6f8b48076
hi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bensonsinsin998 0:c2b6f8b48076 1 /*
bensonsinsin998 0:c2b6f8b48076 2 CityU Robocon Dream Development - 2021 TR Carbase
bensonsinsin998 0:c2b6f8b48076 3 */
bensonsinsin998 0:c2b6f8b48076 4
bensonsinsin998 0:c2b6f8b48076 5 // Mbed Library
bensonsinsin998 0:c2b6f8b48076 6 #include "mbed.h"
bensonsinsin998 0:c2b6f8b48076 7 #include "math.h"
bensonsinsin998 0:c2b6f8b48076 8 // ROS Library
bensonsinsin998 0:c2b6f8b48076 9 #include <ros.h>
bensonsinsin998 0:c2b6f8b48076 10 #include "geometry_msgs/Twist.h"
bensonsinsin998 0:c2b6f8b48076 11 #include "std_msgs/Bool.h"
bensonsinsin998 0:c2b6f8b48076 12 #include "std_msgs/Float32.h"
bensonsinsin998 0:c2b6f8b48076 13 #include "std_msgs/Float64MultiArray.h"
bensonsinsin998 0:c2b6f8b48076 14 #include "std_msgs/Int32.h"
bensonsinsin998 0:c2b6f8b48076 15 #include "std_msgs/MultiArrayLayout.h"
bensonsinsin998 0:c2b6f8b48076 16 #include "std_msgs/MultiArrayDimension.h"
bensonsinsin998 0:c2b6f8b48076 17 // Header File
bensonsinsin998 0:c2b6f8b48076 18 #include "MotorDrive.h"
bensonsinsin998 0:c2b6f8b48076 19 #include "OmniWheel.h"
bensonsinsin998 0:c2b6f8b48076 20
bensonsinsin998 0:c2b6f8b48076 21 // ROS Connect Baud Rate
bensonsinsin998 0:c2b6f8b48076 22 #define BAUD_RATE 115200
bensonsinsin998 0:c2b6f8b48076 23
bensonsinsin998 0:c2b6f8b48076 24 using namespace std;
bensonsinsin998 0:c2b6f8b48076 25 using namespace ros;
bensonsinsin998 0:c2b6f8b48076 26 using namespace std_msgs;
bensonsinsin998 0:c2b6f8b48076 27 using namespace geometry_msgs;
bensonsinsin998 0:c2b6f8b48076 28
bensonsinsin998 0:c2b6f8b48076 29 // Mbed Pin
bensonsinsin998 0:c2b6f8b48076 30 DigitalOut led = LED1;
bensonsinsin998 0:c2b6f8b48076 31 // -> 3.3V Reference Pin
bensonsinsin998 0:c2b6f8b48076 32 DigitalOut reference_voltage_3_3 = PB_1;
bensonsinsin998 0:c2b6f8b48076 33 // -> Motor Pin
bensonsinsin998 0:c2b6f8b48076 34 DigitalOut motor_enable = PC_7;
bensonsinsin998 0:c2b6f8b48076 35 DigitalOut motor_stop[] = {PB_6, PC_5, PC_0};
bensonsinsin998 0:c2b6f8b48076 36 // -> Motor PWN Pin
bensonsinsin998 0:c2b6f8b48076 37 PwmOut motor_pwm[] = {PA_9, PC_8, PA_11};
bensonsinsin998 0:c2b6f8b48076 38
bensonsinsin998 0:c2b6f8b48076 39 // Constant Variable
bensonsinsin998 0:c2b6f8b48076 40 // -> Motor
bensonsinsin998 0:c2b6f8b48076 41 const int motor_num = sizeof(motor_pwm) / sizeof(PwmOut);
bensonsinsin998 0:c2b6f8b48076 42 // -> Motor -> PWM Wait Period
bensonsinsin998 0:c2b6f8b48076 43 const int pwm_period_ms = 20;
bensonsinsin998 0:c2b6f8b48076 44 // -> Motor -> Stop PWM
bensonsinsin998 0:c2b6f8b48076 45 const double stop_pwm = 0.5;
bensonsinsin998 0:c2b6f8b48076 46
bensonsinsin998 0:c2b6f8b48076 47 // ROS Variable
bensonsinsin998 0:c2b6f8b48076 48 // -> Node Handler
bensonsinsin998 0:c2b6f8b48076 49 NodeHandle nh;
bensonsinsin998 0:c2b6f8b48076 50 // -> ROS Msessage
bensonsinsin998 0:c2b6f8b48076 51 Float64MultiArray motor_pwm_msg;
bensonsinsin998 0:c2b6f8b48076 52 Twist motor_command_msg;
bensonsinsin998 0:c2b6f8b48076 53
bensonsinsin998 0:c2b6f8b48076 54 MotorDrive motor_drive;
bensonsinsin998 0:c2b6f8b48076 55
bensonsinsin998 0:c2b6f8b48076 56 // Function
bensonsinsin998 0:c2b6f8b48076 57 // -> Motor
bensonsinsin998 0:c2b6f8b48076 58 // -> Motor -> Intialize
bensonsinsin998 0:c2b6f8b48076 59 void motorInit() {
bensonsinsin998 0:c2b6f8b48076 60 // Initialize motor
bensonsinsin998 0:c2b6f8b48076 61 motor_enable = 0;
bensonsinsin998 0:c2b6f8b48076 62 for(int i = 0; i < motor_num; i++) {
bensonsinsin998 0:c2b6f8b48076 63 motor_pwm[i].period_ms(pwm_period_ms);
bensonsinsin998 0:c2b6f8b48076 64 motor_pwm[i] = stop_pwm;
bensonsinsin998 0:c2b6f8b48076 65 }
bensonsinsin998 0:c2b6f8b48076 66
bensonsinsin998 0:c2b6f8b48076 67 // Set Reference Voltage
bensonsinsin998 0:c2b6f8b48076 68 reference_voltage_3_3 = 1;
bensonsinsin998 0:c2b6f8b48076 69 wait(1); // Wait 1s For Voltage
bensonsinsin998 0:c2b6f8b48076 70
bensonsinsin998 0:c2b6f8b48076 71 // Enable Motor
bensonsinsin998 0:c2b6f8b48076 72 motor_enable = 1;
bensonsinsin998 0:c2b6f8b48076 73 }
bensonsinsin998 0:c2b6f8b48076 74
bensonsinsin998 0:c2b6f8b48076 75 // -> Motor -> Send PWM
bensonsinsin998 0:c2b6f8b48076 76 void motorMoveCallback(int _motor_index, double _pwm) {
bensonsinsin998 0:c2b6f8b48076 77 motor_stop[_motor_index] = 0;
bensonsinsin998 0:c2b6f8b48076 78 motor_pwm[_motor_index] = _pwm;
bensonsinsin998 0:c2b6f8b48076 79 }
bensonsinsin998 0:c2b6f8b48076 80
bensonsinsin998 0:c2b6f8b48076 81 void motorStopCallback(int _motor_index) {
bensonsinsin998 0:c2b6f8b48076 82 motor_stop[_motor_index] = 1;
bensonsinsin998 0:c2b6f8b48076 83 motor_pwm[_motor_index] = 0.5;
bensonsinsin998 0:c2b6f8b48076 84 }
bensonsinsin998 0:c2b6f8b48076 85
bensonsinsin998 0:c2b6f8b48076 86 // -> ROS Subscurber
bensonsinsin998 0:c2b6f8b48076 87 void velCallback(const Twist& _msg) {
bensonsinsin998 0:c2b6f8b48076 88 OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(_msg.linear.x, _msg.linear.y, _msg.angular.z);
bensonsinsin998 0:c2b6f8b48076 89
bensonsinsin998 0:c2b6f8b48076 90 vector<double> omni_wheel_pwm = omni_wheel_data.getPwm(); // Get PWM data
bensonsinsin998 0:c2b6f8b48076 91
bensonsinsin998 0:c2b6f8b48076 92 for(int i = 0; i < motor_num; i++) // Send PWM to each motor
bensonsinsin998 0:c2b6f8b48076 93 if(omni_wheel_pwm[i] != stop_pwm)
bensonsinsin998 0:c2b6f8b48076 94 motorMoveCallback(i, omni_wheel_pwm[i]);
bensonsinsin998 0:c2b6f8b48076 95 else
bensonsinsin998 0:c2b6f8b48076 96 motorStopCallback(i);
bensonsinsin998 0:c2b6f8b48076 97 }
bensonsinsin998 0:c2b6f8b48076 98 Subscriber<Twist> sub_motor_pwm("base_twist", &velCallback);
bensonsinsin998 0:c2b6f8b48076 99
bensonsinsin998 0:c2b6f8b48076 100 // Main
bensonsinsin998 0:c2b6f8b48076 101 int main() {
bensonsinsin998 0:c2b6f8b48076 102 // ROS Setup
bensonsinsin998 0:c2b6f8b48076 103 // -> Set Baud Rate
bensonsinsin998 0:c2b6f8b48076 104 nh.getHardware()->setBaud(BAUD_RATE);
bensonsinsin998 0:c2b6f8b48076 105 // -> Initialize ROS Node
bensonsinsin998 0:c2b6f8b48076 106 nh.initNode();
bensonsinsin998 0:c2b6f8b48076 107 nh.subscribe(sub_motor_pwm); // Subscribe the callback function
bensonsinsin998 0:c2b6f8b48076 108
bensonsinsin998 0:c2b6f8b48076 109 // Initialize motor
bensonsinsin998 0:c2b6f8b48076 110 motorInit();
bensonsinsin998 0:c2b6f8b48076 111
bensonsinsin998 0:c2b6f8b48076 112 // Main programming
bensonsinsin998 0:c2b6f8b48076 113 while(true) {
bensonsinsin998 0:c2b6f8b48076 114 nh.spinOnce();
bensonsinsin998 0:c2b6f8b48076 115
bensonsinsin998 0:c2b6f8b48076 116 if(nh.connected())
bensonsinsin998 0:c2b6f8b48076 117 led = 1; // Turn on the LED if the ROS successfully connect with the Jetson
bensonsinsin998 0:c2b6f8b48076 118 else
bensonsinsin998 0:c2b6f8b48076 119 led = 0; // Turn off the LED if the ROS cannot connect with the Jetson
bensonsinsin998 0:c2b6f8b48076 120 }
bensonsinsin998 0:c2b6f8b48076 121 }