Diff: can/can.cpp

Revision:

0:b87fd8dd4322

Child:

1:26fc1b2f1c42

diff -r 000000000000 -r b87fd8dd4322 can/can.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can/can.cpp	Tue Mar 19 13:20:23 2019 +0000
@@ -0,0 +1,91 @@
+#include "mbed.h"
+#include "PathFollowing.h"
+#include "movement.h"
+#include "maxonsetting.h"
+#include "manual.h"
+#include "can.h"
+
+CAN can1(p30,p29);
+Ticker can_ticker;  //can用ticker
+
+DigitalOut cansend_led(LED1);  //cansend -> on
+DigitalOut canread_led(LED2);  //canread -> on
+
+int t1_r=0, T1=0; //動作番号（受け取った値、送信する値（int型））
+
+void can_readsend()
+{
+    CANMessage msg;
+
+    if(can1.read(msg)) {
+
+        if(msg.id == 1) { //from main
+
+            id1_value[0] = (msg.data[0]>>6)%4;  //decide wait/auto/manual(0~2)
+            id1_value[1] = msg.data[1];         //angle of left joystick(0~359)
+            id1_value[2] = msg.data[2];
+            id1_value[3] = (msg.data[0]>>5)%2;  //BOTTONR1 off/on(0 or 1)
+            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]);
+        }
+
+        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_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;
+        }
+
+    } else {
+        canread_led = 0;
+    }
+
+
+    can_ashileddata[1] = T1;  //動作番号（id節約のため、can_ashileddata[]と一緒に送る）
+
+    if(can1.write(CANMessage(4,can_ashileddata,2))) {  //IDを4にして送信
+        cansend_led = 1;
+    } else {
+        cansend_led = 0;
+    }
+
+    if(t1_r > T1) {
+        T1 = t1_r;
+    }
+}
+
+void can_start()
+{
+
+    while(1) {
+
+        CANMessage msg;
+
+        debug_printf("wait\n\r");
+        wait(0.1);
+        if(can1.read(msg)) {
+            break;
+        }
+    }
+}
+
+void UserLoopSetting_can()
+{
+    can1.frequency(1000000);
+    can_ticker.attach(&can_readsend,0.01);  //遅かったら早める
+}
\ No newline at end of file