can.cpp

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