Diff: main.cpp

Revision:

2:a217f4d56d32

Parent:

1:03c3b1db6179

Child:

3:a96f397dde4a

--- a/main.cpp	Thu Jul 30 10:32:53 2020 +0000
+++ b/main.cpp	Wed Sep 09 02:54:36 2020 +0000
@@ -1,18 +1,45 @@
 #include "mbed.h"
+#include "LSM303D.h"
+#include <math.h>
 
 DigitalOut myled(LED1);
 Serial gps(PA_9, PA_10,38400);
-Serial pc(PA_2, PA_3,115200);
+Serial pc(PA_2, PA_3,38400);
 CAN can(PA_11, PA_12); //pin21,22 rd,td
+LSM303D lsm(PB_7,PB_6);
+DigitalIn fpin(PB_1);
 
 CANMessage msg;
 
+float aData[3],mData[3];
+float m_x,m_y,m_z,a_x,a_y,a_z,x,y,z;
+int i=0;
+float max_x = -60.0;
+float max_y = -60.0;
+float max_z = -60.0;
+float min_x = 60.0;
+float min_y = 60.0;
+float min_z = 60.0;
+
+float cal_x = -0.35207172;
+float cal_y = 0.26154809;
+float cal_z = 1.11167238;
+
+float yaw = 0;
+float pitch = 0;
+
 union Float2Byte{
     float _float;
     char _byte[4];
 };
 typedef union Float2Byte Float2Byte;
 
+union Int2Byte{
+    int _int;
+    char _byte[2];
+};
+typedef union Int2Byte Int2Byte;
+
 float _DMS2DEG(float raw_data){
     int d=(int)(raw_data/100);
     float m=(raw_data-(float)d*100);
@@ -34,7 +61,36 @@
     }
     //pc.printf("sendFloat: %f\n\r", sendFloat._float);
     if(can.write(CANMessage(id, serialData, 4))){
-        pc.printf("Send.\n\r");
+        //pc.printf("Send.\n\r");
+    } 
+    
+    
+    myled = !myled;
+}
+
+void send2(int senddata1,int senddata2,int senddata3,int id){
+    //pc.printf("Master send()\n\r");
+    
+    /*ID: 0x01*/
+    Int2Byte sendInt;
+    sendInt._int = senddata1;
+    //ここに送りたい値を入れる．
+    
+    char serialData[6];
+    
+    serialData[0] = sendInt._byte[0];
+    serialData[1] = sendInt._byte[1];
+        //pc.printf("send_char: %d\n\r", serialData[i]);
+    sendInt._int = senddata2;
+    serialData[2] = sendInt._byte[0];
+    serialData[3] = sendInt._byte[1];
+
+    sendInt._int = senddata3;
+    serialData[4] = sendInt._byte[0];
+    serialData[5] = sendInt._byte[1];
+    //pc.printf("sendFloat: %f\n\r", sendFloat._float);
+    if(can.write(CANMessage(id, serialData, 6))){
+        //pc.printf("Send.\n\r");
     } 
     
     
@@ -45,16 +101,18 @@
     Float2Byte getFloat;
     
     if(can.read(msg)){
-        /*ID: 0x01*/
-        if(msg.id == 0x01){
+        /*ID: 0x03*/
+        
+        if(msg.id == 0x03){
             //pc.printf("ID: 0x01\n\r");
             for(int i=0;i<4;++i){
                 getFloat._byte[i] = msg.data[i];
                 //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
             }
-            pc.printf("%.2f\r\n", getFloat._float);
+            pc.printf("IMU:%.2f\r\n", getFloat._float);
             myled = !myled;
         }
+        
         /*ID: 0x02*/
         if(msg.id == 0x02){
             //pc.printf("ID: 0x02\n\r");
@@ -62,7 +120,7 @@
                 getFloat._byte[i] = msg.data[i];
                 //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
             }
-            pc.printf("%.3f\n\r", getFloat._float);
+            pc.printf("HIKARI:%.1f\n\r", getFloat._float);
             myled = !myled;
         }
     }
@@ -72,7 +130,9 @@
 int main() {
     char gps_data[256];
     int cnt_gps=0;
-    char c;
+    int c=0;
+    float world_time=0.0;
+    wait(1.0);
     pc.printf("Start\r\n");
     can.attach(receive, CAN::RxIrq);
     while(1){
@@ -96,22 +156,55 @@
                         
                     }else{
                         pc.printf("%s\r\n",gps_data);
-                        send(0.0,0x01);
+                        if(sscanf(gps_data,"GPGGA,%f",&world_time)>=1){
+                            send(world_time,0x01);
+                        }else{
+                            send(0.0,0x01);
+                        }
+                        
                         //pc.printf("NoGPSSignal\r\n");
                     }
                 }else{
                     pc.printf("No_Satellite_signal\r\n");
+                    send(0.0,0x01);
                 }
             }else{
                 cnt_gps++;
             }
         }
         if(pc.readable()){
-            pc.printf("kita!\r\n");
+            /*pc.printf("kita!\r\n");
             while(1){
                 c = pc.getc(); 
                 pc.printf("%c",c);
             }
+            */
+        }
+        if(fpin == 0 && c == 0){
+            lsm.read(aData,mData);
+            m_x = mData[0]/10.0f - cal_x;
+            m_y = mData[1]/10.0f - cal_y;
+            m_z = mData[2]/10.0f - cal_z;
+        
+            a_x = aData[0] / 100.0f;
+            a_y = aData[1] / 100.0f;
+            a_z = aData[2] / 100.0f; 
+        
+            x = atan2(a_x, a_y);
+            x = x*180.0f/3.1415f;
+            y = atan2(0-a_z, (float)sqrt(a_x*a_x + a_y*a_y));
+            y = y * 180.0f / 3.14159265f;
+            z = atan2((a_x*m_x - a_y*m_y)*sqrt(a_x*a_x + a_y*a_y + a_z*a_z) , (a_x*a_x + a_y*a_y)*m_z + a_z*(a_x*m_y + a_y*m_x)) ;
+            //z = atan2((a_x*a_x + a_y*a_y)*m_z + a_z*(a_x*m_y + a_y*m_x) , (a_x*m_x - a_y*m_y)*sqrt(a_x*a_x + a_y*a_y + a_z*a_z)) ;
+            z = z * 180.0f / 3.14159265f;
+            //pc.printf("%.1f,%.1f,%.1f\r\n",x,y,z);
+            send2((int)(x*100.0f),(int)(y*100.0f),(int)(z*100.0f),0x02);
+            send(world_time+(float)i*0.5,0x01);
+            pc.printf("%d,%d,%d\r\n",(int)(x*100.0f),(int)(y*100.0f),(int)(z*100.0f));
+            i++;
+            wait(0.5);
+            if(i==10) c=1;
+            
         }
     }
 }