Diff: main.cpp

Revision:

6:66de1f4abff4

Parent:

5:e9a3ee77cb97

Child:

7:fe1f52d848fd

--- a/main.cpp	Thu Feb 04 08:40:48 2021 +0000
+++ b/main.cpp	Wed Feb 17 09:46:01 2021 +0000
@@ -7,6 +7,7 @@
 #include <ros.h>
 #include <std_msgs/Float32MultiArray.h>
 #include <std_msgs/Int32MultiArray.h>
+#include "geometry_msgs/Twist.h"
 //motor header
 #include "MotorDrive.h"
 #include "OmniWheel.h"
@@ -50,27 +51,62 @@
     motor_stop[motor_index] = 1;
     motor_pwm[motor_index] = 0.5;
 }
+float round(float x){
+    float value = (int)(x*100+0.5);
+    return (float)value/100;
+    }
 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]);
+    double x = (double)round(base_msg.data[1]);
+    double y = (double)round(base_msg.data[0]);
+    double z = (double)round(base_msg.data[2]);
+    OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(x, y, z);
     vector<double> omni_wheel_pwm = omni_wheel_data.getPwm();
-    for(int i = 0; i < motor_num; i++){
-        if(omni_wheel_pwm[i]==0.5)
-            motorStop(i);
-        else
-            motorMove(i, omni_wheel_pwm[i]);
-    }
+    //for(int i = 0; i < motor_num; i++){
+//        if(omni_wheel_pwm[i]==0.5)
+//            motorStop(i);
+//        else
+//            motorMove(i, omni_wheel_pwm[i]);
+//    }
+
+    if(omni_wheel_pwm[0] == 0.5)
+        motorStop(0);
+    else
+        motorMove(0, omni_wheel_pwm[0]);
+        
+    if(omni_wheel_pwm[1] == 0.5)
+        motorStop(1);
+    else
+        motorMove(1, omni_wheel_pwm[1]);
+        
+    if(omni_wheel_pwm[2] == 0.5)
+        motorStop(2);
+    else
+        motorMove(2, omni_wheel_pwm[2]);
 }
 
 void messageCb(const std_msgs::Float32MultiArray& toggle_msg){
     myled = !myled;   // blink the led
 }
 
+void twCallback(const geometry_msgs::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)
+            motorMove(i, omni_wheel_pwm[i]);
+        else
+            motorStop(i);
+}
+
 ros::Subscriber<std_msgs::Float32MultiArray> subax("moving_base", &baseCallback);
 ros::Subscriber<std_msgs::Float32MultiArray> subbt("fcn_button", &messageCb);
+ros::Subscriber<geometry_msgs::Twist> subtw("base_twist", &twCallback);
 int main() {
     nh.getHardware()->setBaud(BAUD);
     nh.initNode();
-    nh.subscribe(subax);
+//    nh.subscribe(subax);
     nh.subscribe(subbt);
     motor_init();
     while (1) {