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