can.cpp

00001 #include "mbed.h"
00002 #include "PathFollowing.h"
00003 #include "movement.h"
00004 #include "maxonsetting.h"
00005 #include "manual.h"
00006 #include "can.h"
00007 
00008 CAN can1(p30,p29);
00009 Ticker can_ticker;  //can用ticker
00010 
00011 DigitalOut cansend_led(LED1);  //cansend -> on
00012 DigitalOut canread_led(LED2);  //canread -> on
00013 
00014 int t1_r=0, T1=0; //動作番号（受け取った値、送信する値（int型））
00015 int usw_flag;
00016 
00017 void can_readsend()
00018 {
00019     CANMessage msg;
00020    // printf("readsend\n\r");
00021 
00022     if(can1.read(msg)) {
00023         //printf("read\n\r");
00024 
00025         canread_led = 1;
00026 
00027         if(msg.id == 1) { //from main
00028 
00029             id1_value[0] = (msg.data[0]>>6)%4;  //decide wait/auto/manual(0~2)
00030             id1_value[1] = msg.data[1];         //angle of left joystick(0~359)
00031             id1_value[2] = msg.data[2];
00032             id1_value[3] = (msg.data[0]>>5)%2;  //BOTTONR1 off/on(0 or 1)
00033             id1_value[4] = (msg.data[0]>>3)%4;  //state of right joystick(1~3)
00034             id1_value[5] = (msg.data[0]>>2)%2;  //left joystick neutral position(0 or 1)
00035             id1_value[6] = (msg.data[0]>>1)%2;  //decide right/left(0 or 1)
00036             t1_r = msg.data[3];                 //value of t(0~7)
00037             
00038            // debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r"
00039                     //     ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]);
00040            // debug_printf("t1_r=%d T1=%d can_ashileddata[1]=%d\n\r",t1_r,T1,can_ashileddata[1]);
00041         }
00042 
00043         if(msg.id == 3) {
00044             usw_flag = 0;
00045             
00046             usw_data1 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]);
00047             //debug_printf("usw_data1 = %f\n\r",usw_data1);
00048 
00049             usw_data2 = 0.1 * (short)((msg.data[2]<<8) | msg.data[3]);
00050             //debug_printf("usw_data2 = %f\n\r",usw_data2);
00051 
00052             usw_data3 = 0.1 * (short)((msg.data[4]<<8) | msg.data[5]);
00053             //debug_printf("usw_data3 = %f\n\r",usw_data3);
00054 
00055             usw_data4 = 0.1 * (short)((msg.data[6]<<8) | msg.data[7]);
00056             //debug_printf("usw_data4 = %f\n\r",usw_data4);
00057             //debug_printf("usw1=%f usw2=%f usw3=%f usw4=%f\n\r",usw_data1,usw_data2,usw_data3,usw_data4);
00058         }else{
00059             usw_flag = 1;    
00060         }
00061 
00062     } else {
00063         canread_led = 0;
00064     }
00065 
00066 
00067     can_ashileddata[1] = T1;  //動作番号（id節約のため、can_ashileddata[]と一緒に送る）
00068 
00069     if(can1.write(CANMessage(4,can_ashileddata,2))) {  //IDを4にして送信
00070        // printf("send\n\r");
00071         cansend_led = 1;
00072     } else {
00073         //printf("not_send\n\r");
00074         cansend_led = 0;
00075     }
00076 
00077     if(t1_r > T1) {
00078         T1 = t1_r;
00079     }
00080 }
00081 
00082 void can_start()
00083 {
00084 
00085     while(1) {
00086 
00087         CANMessage msg;
00088 
00089         debug_printf("wait\n\r");
00090         wait(0.1);
00091         if(can1.read(msg)) {
00092             break;
00093         }
00094     }
00095 }
00096 
00097 void UserLoopSetting_can()
00098 {
00099     can1.frequency(1000000);
00100     can_ticker.attach(&can_readsend,0.01);  //遅かったら早める
00101 }