Diff: main.cpp

Revision:

5:e9a3ee77cb97

Parent:

4:aa8ef06b9469

Child:

6:66de1f4abff4

--- a/main.cpp	Sat Jan 30 12:05:58 2021 +0000
+++ b/main.cpp	Thu Feb 04 08:40:48 2021 +0000
@@ -43,18 +43,21 @@
 }
 
 void motorMove(int motor_index, double pwm){
-    if(pwm == 0.5)
-        motor_stop[motor_index] = 1;
-    else
-        motor_stop[motor_index] = 0;
+    motor_stop[motor_index] = 0;
     motor_pwm[motor_index] = pwm;
 }
-
+void motorStop(int motor_index){
+    motor_stop[motor_index] = 1;
+    motor_pwm[motor_index] = 0.5;
+}
 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]);
+        if(omni_wheel_pwm[i]==0.5)
+            motorStop(i);
+        else
+            motorMove(i, omni_wheel_pwm[i]);
     }
 }
 
@@ -70,7 +73,6 @@
     nh.subscribe(subax);
     nh.subscribe(subbt);
     motor_init();
-    
     while (1) {
     nh.spinOnce();