Diff: main.cpp

Revision:

4:aa8ef06b9469

Parent:

3:a1a69ae50c18

Child:

5:e9a3ee77cb97

--- a/main.cpp	Tue Dec 29 07:23:51 2020 +0000
+++ b/main.cpp	Sat Jan 30 12:05:58 2021 +0000
@@ -7,23 +7,78 @@
 #include <ros.h>
 #include <std_msgs/Float32MultiArray.h>
 #include <std_msgs/Int32MultiArray.h>
+//motor header
+#include "MotorDrive.h"
+#include "OmniWheel.h"
 
 ros::NodeHandle nh;
+
 DigitalOut myled(LED1);
+DigitalOut Refer_Volt_3_3 = PB_1;
+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};
+//motor number
+const int motor_num = sizeof(motor_pwm) / sizeof(PwmOut);
+//motor pwm period (ms)
+const int pwm_period = 20;
+//motor stop pwm
+const double stop_pwm = 0.5;
+
+MotorDrive motor_drive;
+
+void motor_init() {
+    //init motor
+    motor_enable = 0;
+    
+    for(int i = 0; i < motor_num; i++){
+        motor_pwm[i].period_ms(pwm_period);
+        motor_pwm[i] = stop_pwm;
+    }
+    //ref voltage
+    Refer_Volt_3_3 = 1;
+    wait(1);
+    motor_enable = 1;
+}
+
+void motorMove(int motor_index, double pwm){
+    if(pwm == 0.5)
+        motor_stop[motor_index] = 1;
+    else
+        motor_stop[motor_index] = 0;
+    motor_pwm[motor_index] = pwm;
+}
+
+void baseCallback(const std_msgs::Float32MultiArray& base_msg){
+    OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(base_msg.data[1], base_msg.data[0], base_msg.data[2]);
+    vector<double> omni_wheel_pwm = omni_wheel_data.getPwm();
+    for(int i = 0; i < motor_num; i++){
+        motorMove(i, omni_wheel_pwm[i]);
+    }
+}
 
 void messageCb(const std_msgs::Float32MultiArray& toggle_msg){
     myled = !myled;   // blink the led
 }
 
-ros::Subscriber<std_msgs::Float32MultiArray> subax("moving_base", &messageCb);
+ros::Subscriber<std_msgs::Float32MultiArray> subax("moving_base", &baseCallback);
 ros::Subscriber<std_msgs::Float32MultiArray> subbt("fcn_button", &messageCb);
 int main() {
     nh.getHardware()->setBaud(BAUD);
     nh.initNode();
     nh.subscribe(subax);
     nh.subscribe(subbt);
+    motor_init();
+    
     while (1) {
     nh.spinOnce();
-        wait_ms(1);
+ 
+    
+    if(nh.connected())
+        myled = 1;    // Turn on the LED if the ROS successfully connect with the Jetson
+    else 
+        myled = 0;    // Turn off the LED if the ROS cannot connect with the Jetson
     }
+    
 }