Diff: main.cpp

Revision:

0:54452c8078db

Child:

1:782534ae7166

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jul 29 14:42:35 2022 +0000
@@ -0,0 +1,147 @@
+#define BAUD 115200
+#include "mbed.h"
+#include "math.h"
+//#include "rtos.h"
+#include <ros.h>
+#include "std_msgs/Bool.h"
+#include "std_msgs/String.h"
+#include "geometry_msgs/Twist.h"
+
+
+using namespace ros;
+using namespace std_msgs;
+using namespace geometry_msgs;
+
+
+NodeHandle nh;
+
+DigitalOut myled = LED1;
+// -> Motor PWN Pin
+PwmOut motor_pwm[] = {D6, D5, D4, D3};
+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 turning(float a, float b){
+    motor_pwm[0]=a;
+    motor_pwm[1]=0;
+    motor_pwm[2]=b;
+    motor_pwm[3]=0;
+}
+
+void r1Callback(const Bool& _msg){
+    bool check = _msg.data;
+    if(check){
+        LED01 = 1;
+        LED02 = 1;
+        LED03 = 1;}
+    else{
+        LED01 = 0;
+        LED02 = 0;
+        LED03 = 0;}
+}
+
+void twCallback(const Twist& _msg) {
+    float x = _msg.linear.x;
+    float y = _msg.linear.y;
+    
+    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);
+//    }
+//    else
+//    {
+//        motor_pwm[2]=y;
+//        motor_pwm[3]=0;
+//    }
+//   if(x<0)
+//   {
+//       motor_pwm[0]=0;
+//       motor_pwm[1]=abs(x);
+//       
+//   }
+//   else
+//   {
+//       motor_pwm[0]=x;
+//       motor_pwm[1]=0;
+//   }
+
+}
+
+Subscriber<Twist> subtw("base_twist", &twCallback);
+Subscriber<Bool> subr1("bt_r1", &r1Callback);
+int main() {
+    //Rate rate(10);
+    nh.getHardware()->setBaud(BAUD);
+    nh.initNode();
+
+    nh.subscribe(subtw);
+    nh.subscribe(subr1);
+    
+    motor_pwm[0]=0;
+    motor_pwm[1]=0;
+    motor_pwm[2]=0;
+    motor_pwm[3]=0;
+
+    while (1) {
+    nh.spinOnce();
+    
+    
+    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
+    }
+    
+}