main.cpp

00001 //中央
00002 
00003 #include "mbed.h"
00004 #include "ValueProcessor.h"
00005 //#include "rtos.h"
00006 #define WAIT_LOOP_TIME 0.001
00007 #define YOKUTAN_DATAS_NUM 7
00008 #define INPUT_DATAS_NUM 7 //ここは1倍とまでしかCANでは一度に送れないため、8以下。そして、翼端コードと数字を合わせる必要あり。
00009 #define SEND_DATAS_CAN_ID 100
00010 #define SEND_DATAS_TIME 0.5
00011 #define THRESHOLD_OH_VALUE 0.14
00012 #define SAMPLE_INTERVAL 0.05
00013 
00014 /*
00015 roll入力とピッチ入力だとピッチの方が1.5倍効くように
00016 */
00017 
00018 //-----------------------------------(resetInterrupt def)
00019 extern "C" void mbed_reset();
00020 InterruptIn resetPin(p22);
00021 Timer resetTimeCount;
00022 void resetInterrupt()
00023 {
00024     while(resetPin) {
00025         resetTimeCount.start();
00026         if(resetTimeCount.read()>3) mbed_reset();
00027     }
00028     resetTimeCount.reset();
00029 }
00030 //-------------------------------------------------------
00031 
00032 CAN can_R(p30,p29);
00033 CAN can_L(p9,p10);
00034 Serial toKeikiSerial(p28,p27);
00035 AnalogIn rollPin(p15);
00036 AnalogIn pitchPin(p18);
00037 DigitalIn drug_R(p14);
00038 DigitalIn drug_L(p17);
00039 DigitalOut led1(LED1);
00040 DigitalOut led2(LED2);
00041 DigitalOut led4(LED4);
00042 DigitalOut led3(LED3);
00043 Serial pc(USBTX,USBRX);
00044 ValueProcess VP;
00045 //DigitalOut fusokuControl(p25);
00046 //Ticker sendDatasTicker;
00047 
00048 char yokutanDatas_R[YOKUTAN_DATAS_NUM];
00049 char yokutanDatas_L[YOKUTAN_DATAS_NUM];
00050 char inputDatas_R[INPUT_DATAS_NUM];
00051 char inputDatas_L[INPUT_DATAS_NUM];
00052 
00053 CANMessage recmsg_R;
00054 CANMessage recmsg_L;
00055 
00056 //void tickFusoku(void const * arg){
00057 //    while(1){
00058 //        fusokuControl = 1;
00059 //        wait_us(500);
00060 //        fusokuControl = 0;
00061 //        wait_us(4500);
00062 //    }
00063 //}
00064 
00065 void InputControlValues()
00066 {
00067     VP.Processing(rollPin.read(),pitchPin.read(),(int *)inputDatas_R,(int *)inputDatas_L);
00068     pc.printf("R:%d     L:%d    show Both Right %d %d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L,drug_R.read(), drug_L.read());
00069     inputDatas_R[sizeof(int)+ 2] = (char)drug_R;
00070     inputDatas_L[sizeof(int)+2] = (char)drug_L;
00071      led4 =! led4;
00072 }
00073 
00074 void SendDatas()
00075 {
00076     can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
00077     can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
00078     toKeikiSerial.putc(';');
00079     for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
00080         toKeikiSerial.putc(yokutanDatas_R[i]);
00081 
00082     }
00083     for(int i = 0; i < YOKUTAN_DATAS_NUM; i++) {
00084         toKeikiSerial.putc(yokutanDatas_L[i]);
00085     }
00086     for(int i = 0; i < INPUT_DATAS_NUM; i++) {
00087         toKeikiSerial.putc(inputDatas_R[i]);
00088     }
00089     for(int i = 0; i < INPUT_DATAS_NUM; i++) {
00090         toKeikiSerial.putc(inputDatas_L[i]);
00091     }
00092 }
00093 
00094 void init()
00095 {
00096 //--------------------------------------(resetInterrupt init)
00097     resetPin.rise(resetInterrupt);
00098     resetPin.mode(PullDown);
00099 //-----------------------------------------------------------
00100 //    sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
00101     for(int i = 0; i < INPUT_DATAS_NUM; i++) {
00102         inputDatas_R[i] = 0;
00103         inputDatas_L[i] = 0;
00104     }
00105 }
00106 
00107 void receiveDatas()
00108 {
00109     if(can_R.read(recmsg_R)) {
00110         for(int i = 0; i < recmsg_R.len; i++) {
00111             yokutanDatas_R[i] = recmsg_R.data[i];
00112         }
00113         led1 = !led1;
00114     }
00115     if(can_L.read(recmsg_L)) {
00116         for(int i = 0; i < recmsg_L.len; i++) {
00117             yokutanDatas_L[i] = recmsg_L.data[i];
00118             led3 = !led3;
00119         }
00120         led2 = !led2;
00121     }
00122 }
00123 
00124 int main()
00125 {
00126 //    Thread tickFusokuThread(&tickFusoku);
00127     init();
00128     VP.setNeutral(rollPin.read(),pitchPin.read());
00129     while(1) {
00130         InputControlValues();
00131         wait_us(5);
00132         receiveDatas();
00133         SendDatas();
00134         wait(WAIT_LOOP_TIME);
00135     }
00136 }