main.cpp

00001 //中央can program
00002 
00003 #include "mbed.h"
00004 #define WAIT_LOOP_TIME 0.1
00005 #define YOKUTAN_DATAS_NUM 4
00006 #define IMPUT_DATAS_NUM 2
00007 #define SEND_DATAS_CAN_ID 100
00008 #define SEND_DATAS_TIME 0.1
00009 #define THRESHOLD_OH_VALUE 0.14
00010 #define CALC_CADENCE_TIME 0.05
00011 
00012 CAN can_R(p30,p29);
00013 CAN can_L(p9,p10);
00014 Serial toKeikiSerial(p13,p14);
00015 Serial pc(USBTX,USBRX);
00016 DigitalIn eruron_R(p22);
00017 DigitalIn eruron_L(p20);
00018 DigitalIn drug_R(p23);
00019 DigitalIn drug_L(p19);
00020 DigitalOut myled1(LED1);
00021 DigitalOut myled2(LED2);
00022 Ticker sendDatasTicker;
00023 Ticker cadenceTicker;
00024 Timer cadenceTimer;
00025 AnalogIn p(p15);
00026 AnalogIn q(p16);
00027 
00028 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
00029 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
00030 char inputDatas_R[IMPUT_DATAS_NUM];
00031 char inputDatas_L[IMPUT_DATAS_NUM];
00032 char cadence = 0;
00033 bool stateP = true;
00034 bool stateQ = true;
00035 bool stateP_old = true;
00036 bool stateQ_old = true;
00037 
00038 CANMessage recmsg_R;
00039 CANMessage recmsg_L;
00040 
00041 void InputControlValues(){
00042     inputDatas_R[1] = (char)drug_R;
00043     inputDatas_L[1] = (char)drug_L;
00044     if(eruron_R){
00045         inputDatas_R[0] = 0;
00046         inputDatas_L[0] = 2;
00047     }
00048     else if(eruron_L){
00049         inputDatas_R[0] = 2;
00050         inputDatas_L[0] = 0;
00051     }
00052     else{
00053         inputDatas_R[0] = 1;
00054         inputDatas_L[0] = 1;
00055     }
00056 }
00057 
00058 void toString_R(){
00059     pc.printf("R:");
00060     for(int i = 0; i < IMPUT_DATAS_NUM; i++){
00061         pc.printf("%d:%i    ",i,inputDatas_R[i]);
00062     }
00063 }
00064 
00065 void toString_L(){
00066     pc.printf("L:");
00067     for(int i = 0; i < IMPUT_DATAS_NUM; i++){
00068         pc.printf("%d:%i    ",i,inputDatas_L[i]);
00069     }
00070     pc.printf("\n\r");
00071 }
00072 
00073 void SendDatas(){
00074     can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, IMPUT_DATAS_NUM));
00075     can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, IMPUT_DATAS_NUM));
00076     for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
00077         toKeikiSerial.putc(yokutanDatas_R[i]);
00078         toKeikiSerial.putc(yokutanDatas_L[i]);
00079     }
00080     for(int i = 0; i < IMPUT_DATAS_NUM; i++){
00081         toKeikiSerial.putc(inputDatas_R[i]);
00082         toKeikiSerial.putc(inputDatas_L[i]);
00083     }
00084     toKeikiSerial.putc(cadence);
00085     pc.printf("test\n\r");
00086 }
00087 
00088 void calcCadence(){
00089     //pc.printf("%f,%f\n\r",p.read(),q.read());
00090     if(p.read() > THRESHOLD_OH_VALUE) stateP = true;
00091     else stateP = false;
00092     if(q.read() > THRESHOLD_OH_VALUE) stateQ = true;
00093     else stateQ = false;
00094     if((stateP == true && stateP_old == false) || 
00095         stateQ == true && stateQ_old == false){
00096         cadenceTimer.stop();
00097         cadence = (char)30.0/cadenceTimer.read();
00098         pc.printf("Cadence:%i\n\r",cadence);
00099         cadenceTimer.reset();
00100         cadenceTimer.start();
00101     }
00102     stateP_old = stateP;
00103     stateQ_old = stateQ;
00104 }        
00105 
00106 void init(){
00107     cadenceTimer.start();
00108     cadenceTicker.attach(calcCadence, CALC_CADENCE_TIME);
00109     sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
00110     for(int i = 0; i < IMPUT_DATAS_NUM; i++){
00111         inputDatas_R[i] = 0;
00112         inputDatas_L[i] = 0;
00113     }
00114 }
00115 
00116 
00117 void receiveDatas(){
00118     if(can_R.read(recmsg_R)){
00119         for(int i = 0; i < recmsg_R.len; i++){
00120             yokutanDatas_R[i] = recmsg_R.data[i];
00121         }
00122         myled1 = !myled1;
00123     }
00124      if(can_L.read(recmsg_L)){
00125         for(int i = 0; i < recmsg_L.len; i++){
00126             yokutanDatas_L[i] = recmsg_L.data[i];
00127         }
00128         myled2 = !myled2;
00129     }
00130 }
00131 
00132 int main(){
00133     init();
00134     while(1){
00135         InputControlValues();
00136         wait_us(10);
00137         receiveDatas();
00138         //toString_R();
00139 //        toString_L();
00140         wait(WAIT_LOOP_TIME);
00141     }
00142 }