Diff: main.cpp

Revision:

3:5402a080282c

Parent:

1:782534ae7166

diff -r 39a46889c9c3 -r 5402a080282c main.cpp
--- a/main.cpp	Sat Jul 30 18:08:51 2022 +0000
+++ b/main.cpp	Thu Aug 18 07:35:33 2022 +0000
@@ -1,4 +1,4 @@
-#define BAUD 115200
+#define BAUD 256000
 #include "mbed.h"
 #include "math.h"
 #include "rtos.h"
@@ -21,16 +21,21 @@
 DigitalOut myled = LED1;
 // -> Motor PWN Pin
 PwmOut motor_pwm[] = {D6, D5, D3, D4};
+
 DigitalOut LED01 = D7;
 DigitalOut LED02 = D8;
 DigitalOut LED03 = D9;
 //motor number
+double round(double number)
+{
+return number < 0.0 ? ceil(number - 0.5): floor(number + 0.5);
+};
 
 void turn(float r, float l){
     float a, b, c, d;
     if (l < 0){
         a = 0.0;
-        b = -l;    
+        b = abs(l);    
     }
     else{
         a = l;
@@ -38,7 +43,7 @@
     }
     if (r < 0){
         c = 0.0;
-        d = -r;
+        d = abs(r);
     }
     else{
         c = r;
@@ -85,16 +90,40 @@
 
 
 void twCallback(const Twist& _msg) {
-    float x = _msg.linear.x;
-    float y = _msg.linear.y;
+    float x = round(_msg.linear.x*100.0)/100.0;
+    float y = round(_msg.linear.y*100.0)/100.0;
     sum =abs(x)+abs(y);
     resize(y, sum);
-    if (y < 0)
-        sum = -sum;
-    if (x < 0)
-        turn(y, sum);
-    else
-        turn(sum, y);
+    if (x <= 0){
+        if (y<=0){
+            motor_pwm[0]= 0;
+            motor_pwm[1]= abs(y);
+            motor_pwm[2]= 0;
+            motor_pwm[3]= sum;
+            //turn(y, sum);
+        }
+        else{
+            motor_pwm[0]= sum;
+            motor_pwm[1]= 0;
+            motor_pwm[2]= y;
+            motor_pwm[3]= 0;
+        }
+}
+    else{
+        if (y<=0){
+            motor_pwm[0]= 0;
+            motor_pwm[1]= sum;
+            motor_pwm[2]= 0;
+            motor_pwm[3]= abs(y);
+        }
+        else{
+            motor_pwm[0]= y;
+            motor_pwm[1]= 0;
+            motor_pwm[2]= sum;
+            motor_pwm[3]= 0;                    
+        }
+    }
+//        turn(sum, y);
 
 //    sprintf(buffer, "turn: %f, %f", sum, y);
 //    motor_msg.data = buffer;
@@ -161,7 +190,11 @@
 }
 
 int main() {
-    //Rate rate(10);
+//    ros::Rate rate(10);
+    motor_pwm[0]=0;
+    motor_pwm[1]=0;
+    motor_pwm[2]=0;
+    motor_pwm[3]=0;
     nh.getHardware()->setBaud(BAUD);
     nh.initNode();
     nh.advertise(pub);
@@ -169,11 +202,6 @@
     nh.subscribe(subtw);
     nh.subscribe(subr1);
     
-    motor_pwm[0]=0;
-    motor_pwm[1]=0;
-    motor_pwm[2]=0;
-    motor_pwm[3]=0;
-    
     battery_thread.start(battery_pub);
     while (1) {
     nh.spinOnce();
@@ -184,5 +212,5 @@
     else 
         myled = 0;    // Turn off the LED if the ROS cannot connect with the Jetson
     }
-    
+    Thread::wait(10);
 }