Diff: main.cpp

Revision:

1:782534ae7166

Parent:

0:54452c8078db

Child:

3:5402a080282c

--- a/main.cpp	Fri Jul 29 14:42:35 2022 +0000
+++ b/main.cpp	Sat Jul 30 18:05:58 2022 +0000
@@ -1,7 +1,7 @@
 #define BAUD 115200
 #include "mbed.h"
 #include "math.h"
-//#include "rtos.h"
+#include "rtos.h"
 #include <ros.h>
 #include "std_msgs/Bool.h"
 #include "std_msgs/String.h"
@@ -14,29 +14,58 @@
 
 
 NodeHandle nh;
+Thread battery_thread;
+
+AnalogIn battery(A0);
 
 DigitalOut myled = LED1;
 // -> Motor PWN Pin
-PwmOut motor_pwm[] = {D6, D5, D4, D3};
+PwmOut motor_pwm[] = {D6, D5, D3, D4};
 DigitalOut LED01 = D7;
 DigitalOut LED02 = D8;
 DigitalOut LED03 = D9;
 //motor number
 
-void antiturning(float a, float b){
-    motor_pwm[0]=0;
-    motor_pwm[1]=a;
-    motor_pwm[2]=0;
-    motor_pwm[3]=b;
+void turn(float r, float l){
+    float a, b, c, d;
+    if (l < 0){
+        a = 0.0;
+        b = -l;    
+    }
+    else{
+        a = l;
+        b = 0.0;
+    }
+    if (r < 0){
+        c = 0.0;
+        d = -r;
+    }
+    else{
+        c = r;
+        d = 0.0;
+    }    
+    motor_pwm[0]=a;
+    motor_pwm[1]=b;
+    motor_pwm[2]=c;
+    motor_pwm[3]=d;
 }
 
-void turning(float a, float b){
-    motor_pwm[0]=a;
-    motor_pwm[1]=0;
-    motor_pwm[2]=b;
-    motor_pwm[3]=0;
+void resize(float &a, float &b){
+    if (a != 0 or b != 0) {
+        if (b > a){
+            a = a/b;
+            if (b > 1) 
+                b = 1;
+        }
+        else{
+            b = b/a;
+            if (a > 1)
+                a = 1;
+        }
+    }
+    
 }
-
+ 
 void r1Callback(const Bool& _msg){
     bool check = _msg.data;
     if(check){
@@ -49,83 +78,94 @@
         LED03 = 0;}
 }
 
+float sum = 0.0;
+//std_msgs::String motor_msg;
+//char buffer[50];
+//Publisher motor_pub("motor", &motor_msg);
+
+
 void twCallback(const Twist& _msg) {
     float x = _msg.linear.x;
     float y = _msg.linear.y;
+    sum =abs(x)+abs(y);
+    resize(y, sum);
+    if (y < 0)
+        sum = -sum;
+    if (x < 0)
+        turn(y, sum);
+    else
+        turn(sum, y);
+
+//    sprintf(buffer, "turn: %f, %f", sum, y);
+//    motor_msg.data = buffer;
+//    motor_pub.publish(&motor_msg);
     
-    if (x==0.0){
-        if(y<0){
-            antiturning(abs(y), abs(y));
-        }
-        else{
-            turning(y, y);
-        }
-    }
-    else{
-        float angle = atan(abs(y)/abs(x));
-        float angle45 = atan(1.0);
-        if(x<0){
-            if(angle <= angle45){
-                motor_pwm[0]=0;
-                motor_pwm[1]=abs(x + abs(y));
-                motor_pwm[2]=-x;
-                motor_pwm[3]=0;
-                
-            }
-            else{
-                if(y<0){
-                    antiturning(-y, -y+x);}
-                else{
-                    turning(y+x, y);}
-            }
-        }
-        else {
-            if(angle <= angle45){
-                motor_pwm[0]=x;
-                motor_pwm[1]=0;
-                motor_pwm[2]=0;
-                motor_pwm[3]=x - abs(y);
-                
-            }
-            else{
-                if(y<0){
-                    antiturning(-y-x,-y);}
-                else{
-                    turning(y, y-x);}
-            }
-        }
-    }
-//    if(y<0)
-//    {
-//        motor_pwm[2]=0;
-//        motor_pwm[3]=abs(y);
+//
+//    if (x==0.0){
+//        turn(y,y);
 //    }
-//    else
-//    {
-//        motor_pwm[2]=y;
-//        motor_pwm[3]=0;
+//    else{
+//        float angle = atan(abs(y)/abs(x));
+//        float angle45 = atan(1.0);
+//        if(x<0){
+//            if(angle <= angle45){
+//                motor_pwm[0]=0;
+//                motor_pwm[1]=abs(x + abs(y));
+//                motor_pwm[2]=-x;
+//                motor_pwm[3]=0;
+//                
+//            }
+//            else{
+//                if(y<0){
+//                    antiturning(-y, -y+x);}
+//                else{
+//                    turning(y+x, y);}
+//            }
+//        }
+//        else {
+//            if(angle <= angle45){
+//                motor_pwm[0]=x;
+//                motor_pwm[1]=0;
+//                motor_pwm[2]=0;
+//                motor_pwm[3]=x - abs(y);
+//                
+//            }
+//            else{
+//                if(y<0){
+//                    antiturning(-y-x,-y);}
+//                else{
+//                    turning(y, y-x);}
+//            }
+//        }
 //    }
-//   if(x<0)
-//   {
-//       motor_pwm[0]=0;
-//       motor_pwm[1]=abs(x);
-//       
-//   }
-//   else
-//   {
-//       motor_pwm[0]=x;
-//       motor_pwm[1]=0;
-//   }
 
 }
+std_msgs::String str_msg;
+Publisher pub("battery", &str_msg);
 
 Subscriber<Twist> subtw("base_twist", &twCallback);
 Subscriber<Bool> subr1("bt_r1", &r1Callback);
+
+void battery_pub(){
+    char buf[10];
+    while(1){
+        
+        sprintf(buf, "%d", int(((battery.read()*3.3f) - (6/2.68))/(2.4/268)));
+//        sprintf(buf,"%f",battery.read());
+        str_msg.data = buf;
+        pub.publish(&str_msg);
+//        nh.spinOnce();
+        Thread::wait(1000);
+        
+    }
+}
+
 int main() {
     //Rate rate(10);
     nh.getHardware()->setBaud(BAUD);
     nh.initNode();
-
+    nh.advertise(pub);
+//    nh.advertise(motor_pub);
     nh.subscribe(subtw);
     nh.subscribe(subr1);
     
@@ -133,7 +173,8 @@
     motor_pwm[1]=0;
     motor_pwm[2]=0;
     motor_pwm[3]=0;
-
+    
+    battery_thread.start(battery_pub);
     while (1) {
     nh.spinOnce();