File content as of revision 0:c2b6f8b48076:

/*
    CityU Robocon Dream Development - 2021 TR Carbase
*/

// Mbed Library
#include "mbed.h"
#include "math.h"
// ROS Library
#include <ros.h>
#include "geometry_msgs/Twist.h"
#include "std_msgs/Bool.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/Int32.h"
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
// Header File
#include "MotorDrive.h"
#include "OmniWheel.h"

// ROS Connect Baud Rate 
#define BAUD_RATE 115200

using namespace std;
using namespace ros;
using namespace std_msgs;
using namespace geometry_msgs;

// Mbed Pin
DigitalOut led = LED1;
// -> 3.3V Reference Pin
DigitalOut reference_voltage_3_3 = PB_1;
// -> Motor Pin
DigitalOut motor_enable = PC_7;
DigitalOut motor_stop[] = {PB_6, PC_5, PC_0};
// -> Motor PWN Pin
PwmOut motor_pwm[] = {PA_9, PC_8, PA_11};

// Constant Variable
// -> Motor
const int motor_num = sizeof(motor_pwm) / sizeof(PwmOut);
// -> Motor -> PWM Wait Period
const int pwm_period_ms = 20;
// -> Motor -> Stop PWM
const double stop_pwm = 0.5;

// ROS Variable
// -> Node Handler
NodeHandle nh;
// -> ROS Msessage
Float64MultiArray motor_pwm_msg;
Twist motor_command_msg;

MotorDrive motor_drive;

// Function
// -> Motor
// -> Motor -> Intialize
void motorInit() {
    // Initialize motor
    motor_enable = 0;
    for(int i = 0; i < motor_num; i++) {
        motor_pwm[i].period_ms(pwm_period_ms);
        motor_pwm[i] = stop_pwm;
    }
    
    // Set Reference Voltage
    reference_voltage_3_3 = 1;
    wait(1);    // Wait 1s For Voltage
    
    // Enable Motor
    motor_enable = 1;
}

// -> Motor -> Send PWM
void motorMoveCallback(int _motor_index, double _pwm) {
    motor_stop[_motor_index] = 0;
    motor_pwm[_motor_index] = _pwm;
}

void motorStopCallback(int _motor_index) {
    motor_stop[_motor_index] = 1;
    motor_pwm[_motor_index] = 0.5;
}

// -> ROS Subscurber 
void velCallback(const Twist& _msg) {
    OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(_msg.linear.x, _msg.linear.y, _msg.angular.z);
    
    vector<double> omni_wheel_pwm = omni_wheel_data.getPwm();   // Get PWM data
    
    for(int i = 0; i < motor_num; i++)      // Send PWM to each motor
        if(omni_wheel_pwm[i] != stop_pwm)
            motorMoveCallback(i, omni_wheel_pwm[i]);
        else
            motorStopCallback(i);
}
Subscriber<Twist> sub_motor_pwm("base_twist", &velCallback);

// Main
int main() {
    // ROS Setup
    // -> Set Baud Rate
    nh.getHardware()->setBaud(BAUD_RATE);
    // -> Initialize ROS Node
    nh.initNode();
    nh.subscribe(sub_motor_pwm);    // Subscribe the callback function
    
    // Initialize motor
    motorInit();
    
    // Main programming
    while(true) {
        nh.spinOnce();

        if(nh.connected())
            led = 1;    // Turn on the LED if the ROS successfully connect with the Jetson
        else 
            led = 0;    // Turn off the LED if the ROS cannot connect with the Jetson
    }
}