Revision 3:a96f397dde4a, committed 2020-09-09

Comitter:

sashida_h

Date:

Wed Sep 09 08:06:25 2020 +0000

Parent:

2:a217f4d56d32

Commit message:

[fix]bug

Changed in this revision

--- a/main.cpp	Wed Sep 09 02:54:36 2020 +0000
+++ b/main.cpp	Wed Sep 09 08:06:25 2020 +0000
@@ -35,7 +35,7 @@
 typedef union Float2Byte Float2Byte;
 
 union Int2Byte{
-    int _int;
+    unsigned short _int;
     char _byte[2];
 };
 typedef union Int2Byte Int2Byte;
@@ -68,7 +68,7 @@
     myled = !myled;
 }
 
-void send2(int senddata1,int senddata2,int senddata3,int id){
+void send2(unsigned short senddata1,unsigned short senddata2,unsigned short senddata3,int id){
     //pc.printf("Master send()\n\r");
     
     /*ID: 0x01*/
@@ -99,6 +99,7 @@
 
 void receive(){
     Float2Byte getFloat;
+    Int2Byte getInt;
     
     if(can.read(msg)){
         /*ID: 0x03*/
@@ -114,13 +115,28 @@
         }
         
         /*ID: 0x02*/
-        if(msg.id == 0x02){
+        if(msg.id == 0x06){
             //pc.printf("ID: 0x02\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]);
+            getInt._byte[0] = msg.data[0];
+            getInt._byte[1] = msg.data[1];
+            pc.printf("HIKARI:%.2f", (float)getInt._int /10.0f);
+            
+            getInt._byte[0] = msg.data[2];
+            getInt._byte[1] = msg.data[3];
+            pc.printf(",%.2f", (float)getInt._int /10.0f);
+            
+            getInt._byte[0] = msg.data[4];
+            getInt._byte[1] = msg.data[5];
+            pc.printf(",%.2f\r\n", (float)getInt._int /10.0f);
+        }
+        
+        if(msg.id == 0x05){
+            //pc.printf("ID: 0x05\n\r");
+            for(int i=0;i<6;++i){
+                msg.data[i];
+                pc.printf("%d,",msg.data[i]);
             }
-            pc.printf("HIKARI:%.1f\n\r", getFloat._float);
+            pc.printf("\n\r");
             myled = !myled;
         }
     }
@@ -133,7 +149,7 @@
     int c=0;
     float world_time=0.0;
     wait(1.0);
-    pc.printf("Start\r\n");
+    pc.printf("Start%d\r\n",sizeof(c));
     can.attach(receive, CAN::RxIrq);
     while(1){
         if(gps.readable()){
@@ -197,13 +213,17 @@
             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;
+            
+            if(x < 0)x+360.0;
+            if(y < 0)y+360.0;
+            if(z < 0)z+360.0;
             //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));
+            send2((unsigned short)(x*10.0f),(unsigned short)(y*10.0f),(unsigned short)(z*10.0f),0x02);
+            //send(world_time+(float)i*0.5,0x01);
+            pc.printf("%d,%d,%d\r\n",(unsigned short)(x*10.0f),(unsigned short)(y*10.0f),(unsigned short)(z*10.0f));
             i++;
             wait(0.5);
-            if(i==10) c=1;
+            if(i==5) c=1;
             
         }
     }