Diff: can/can.cpp

Revision:

1:26fc1b2f1c42

Parent:

0:b87fd8dd4322

--- a/can/can.cpp	Tue Mar 19 13:20:23 2019 +0000
+++ b/can/can.cpp	Sat Nov 16 06:26:57 2019 +0000
@@ -1,7 +1,6 @@
 #include "mbed.h"
 #include "PathFollowing.h"
 #include "movement.h"
-#include "maxonsetting.h"
 #include "manual.h"
 #include "can.h"
 
@@ -17,6 +16,7 @@
 {
     CANMessage msg;
 
+    /*
     if(can1.read(msg)) {
 
         if(msg.id == 1) { //from main
@@ -28,45 +28,87 @@
             id1_value[4] = (msg.data[0]>>3)%4;  //state of right joystick(1~3)
             id1_value[5] = (msg.data[0]>>2)%2;  //left joystick neutral position(0 or 1)
             id1_value[6] = (msg.data[0]>>1)%2;  //decide right/left(0 or 1)
-            t1_r = msg.data[3];                 //value of t(0~7)
-            
-            /*debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r"
-                         ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]);*/
-            debug_printf("t1_r=%d T1=%d can_ashileddata[1]=%d\n\r",t1_r,T1,can_ashileddata[1]);
-        }
+            t1_r = msg.data[3];                 //value of t(0~7)*/
 
-        if(msg.id == 3) {
-            usw_data1 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
-            //debug_printf("usw_data1 = %f\n\r",usw_data1);
+    /*debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r"
+                 ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]);*/
+    /*            debug_printf("t1_r=%d T1=%d can_ashileddata[1]=%d\n\r",t1_r,T1,can_ashileddata[1]);
+            }
+
+            if(msg.id == 3) {
+                usw_data1 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
+                //debug_printf("usw_data1 = %f\n\r",usw_data1);
 
-            usw_data2 = 0.1 * (short)((msg.data[2]<<8) | msg.data[3]);
-            //debug_printf("usw_data2 = %f\n\r",usw_data2);
+                usw_data2 = 0.1 * (short)((msg.data[2]<<8) | msg.data[3]);
+                //debug_printf("usw_data2 = %f\n\r",usw_data2);
 
-            usw_data3 = 0.1 * (short)((msg.data[4]<<8) | msg.data[5]);
-            //debug_printf("usw_data3 = %f\n\r",usw_data3);
+                usw_data3 = 0.1 * (short)((msg.data[4]<<8) | msg.data[5]);
+                //debug_printf("usw_data3 = %f\n\r",usw_data3);
 
-            usw_data4 = 0.1 * (short)((msg.data[6]<<8) | msg.data[7]);
-            //debug_printf("usw_data4 = %f\n\r",usw_data4);
-            //debug_printf("usw1=%f usw2=%f usw3=%f usw4=%f\n\r",usw_data1,usw_data2,usw_data3,usw_data4);
-            canread_led = 1;
-        }
+                usw_data4 = 0.1 * (short)((msg.data[6]<<8) | msg.data[7]);
+                //debug_printf("usw_data4 = %f\n\r",usw_data4);
+                //debug_printf("usw1=%f usw2=%f usw3=%f usw4=%f\n\r",usw_data1,usw_data2,usw_data3,usw_data4);
+                canread_led = 1;
+            }
 
-    } else {
-        canread_led = 0;
-    }
+        } else {
+            canread_led = 0;
+        }*/
 
 
     can_ashileddata[1] = T1;  //動作番号（id節約のため、can_ashileddata[]と一緒に送る）
 
-    if(can1.write(CANMessage(4,can_ashileddata,2))) {  //IDを4にして送信
+    can_ashileddata2[0] = m1 >> 8;
+    can_ashileddata2[1] = m1 &255;
+    
+    can_ashileddata2[2] = m2 >> 8;
+    can_ashileddata2[3] = m2 &255;
+    
+    can_ashileddata2[4] = m3 >> 8;
+    can_ashileddata2[5] = m3 &255;
+    
+    can_ashileddata2[6] = m4 >> 8;
+    can_ashileddata2[7] = m4 &255;
+    
+
+//    can_ashileddata3[0] = m2 >> 8;
+//    can_ashileddata3[1] = m2 &255;
+//
+//    can_ashileddata4[0] = m3 >> 8;
+//    can_ashileddata4[1] = m3 &255;
+//
+//    can_ashileddata5[0] = m4 >> 8;
+//    can_ashileddata5[1] = m4 &255;
+
+    if(can1.write(CANMessage(4,can_ashileddata2,8))) {  //IDを4にして送信
+        //printf("success : %d \n\r",m1);
         cansend_led = 1;
     } else {
         cansend_led = 0;
+        //printf("fale\n\r");
     }
 
-    if(t1_r > T1) {
+    //if(can1.write(CANMessage(5,can_ashileddata3,2))) {  //IDを4にして送信
+//        cansend_led = 1;
+//    } else {
+//        cansend_led = 0;
+//    }
+//
+//    if(can1.write(CANMessage(6,can_ashileddata4,2))) {  //IDを4にして送信
+//        cansend_led = 1;
+//    } else {
+//        cansend_led = 0;
+//    }
+//
+//    if(can1.write(CANMessage(7,can_ashileddata5,2))) {  //IDを4にして送信
+//        cansend_led = 1;
+//    } else {
+//        cansend_led = 0;
+//    }
+
+    /*if(t1_r > T1) {
         T1 = t1_r;
-    }
+    }*/
 }
 
 void can_start()
@@ -76,7 +118,8 @@
 
         CANMessage msg;
 
-        debug_printf("wait\n\r");
+//        debug_printf("wait\n\r");
+        printf("wait\n\r");
         wait(0.1);
         if(can1.read(msg)) {
             break;