Diff: main.cpp

Revision:

4:4a13dd263a7b

Parent:

3:0e66ce2ab2fb

Child:

5:6b74fa61198e

--- a/main.cpp	Wed Feb 17 01:58:07 2016 +0000
+++ b/main.cpp	Wed Feb 24 10:19:16 2016 +0000
@@ -1,94 +1,112 @@
 //中央can program
 
 #include "mbed.h"
-#define WAIT_LOOP_TIME 0.2
-#define YOKUTAN_DATAS_NUM 5
+#define WAIT_LOOP_TIME 0.1
+#define YOKUTAN_DATAS_NUM 4
 #define IMPUT_DATAS_NUM 2
 #define SEND_DATAS_CAN_ID 100
+#define SEND_DATAS_TIME 0.5
+#define THRESHOLD_OH_VALUE 0.14
+#define CALC_CADENCE_TIME 0.05
 
 CAN can_R(p9,p10);
 CAN can_L(p30,p29);
-Serial toKeikiSerial(p28,p27);
+Serial toKeikiSerial(p13,p14);
 Serial pc(USBTX,USBRX);
-DigitalIn eruron_R(p20);
-DigitalIn eruron_L(p22);
-DigitalIn drug_R(p19);
-DigitalIn drug_L(p23);
+DigitalIn eruron_R(p19);
+DigitalIn eruron_L(p23);
+DigitalIn drug_R(p20);
+DigitalIn drug_L(p22);
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
+Ticker sendDatasTicker;
+Ticker cadenceTicker;
+Timer cadenceTimer;
+AnalogIn p(p15);
+AnalogIn q(p16);
 
-signed char yokutanDatas_R[YOKUTAN_DATAS_NUM];
-signed char yokutanDatas_L[YOKUTAN_DATAS_NUM];
+char yokutanDatas_R[YOKUTAN_DATAS_NUM];
+char yokutanDatas_L[YOKUTAN_DATAS_NUM];
 char inputDatas_R[IMPUT_DATAS_NUM];
 char inputDatas_L[IMPUT_DATAS_NUM];
+char cadence = 0;
+bool stateP = true;
+bool stateQ = true;
+bool stateP_old = true;
+bool stateQ_old = true;
 
 CANMessage recmsg_R;
 CANMessage recmsg_L;
 
-void InputAndSentControlValues_R(){
-    inputDatas_R[0] = (char)drug_R;
-    inputDatas_R[1] = (char)eruron_R;
-    // pc.printf("R : %d\t%d\t",drug_R.read(),eruron_R.read());
-    //for(int i = 0; i < IMPUT_DATAS_NUM; i++){
-        if(can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, IMPUT_DATAS_NUM))){
-        //    pc.printf("Right successed!\n\r");
-      
-        }
-    //}
-}
-
-void ledcheck_R() {
-        myled1 = 1;
-        wait(0.2);
-        myled1 = 0;
-        wait(0.2);
+void InputControlValues(){
+    inputDatas_R[1] = (char)drug_R;
+    inputDatas_L[1] = (char)drug_L;
+    if(eruron_R){
+        inputDatas_R[0] = 0;
+        inputDatas_L[0] = 2;
+    }
+    else if(eruron_L){
+        inputDatas_R[0] = 2;
+        inputDatas_L[0] = 0;
+    }
+    else{
+        inputDatas_R[0] = 1;
+        inputDatas_L[0] = 1;
+    }
 }
 
-void ledcheck_L() {
-        myled2 = 1;
-        wait(0.2);
-        myled2 = 0;
-        wait(0.2);
+void toString_R(){
+    pc.printf("R:");
+    for(int i = 0; i < IMPUT_DATAS_NUM; i++){
+        pc.printf("%d:%i    ",i,inputDatas_R[i]);
+    }
 }
 
-void InputAndSentControlValues_L(){
-    inputDatas_L[0] = (char)drug_L;
-    inputDatas_L[1] = (char)eruron_L;
-      pc.printf("L : %d\t%d\n\r",drug_L.read(),eruron_L.read());
-    //for(int i = 0; i < IMPUT_DATAS_NUM; i++){
-        if(can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, IMPUT_DATAS_NUM))){
-          //  pc.printf("Left successed!\n\r");
-        }
-    //}
+void toString_L(){
+    pc.printf("L:");
+    for(int i = 0; i < IMPUT_DATAS_NUM; i++){
+        pc.printf("%d:%i    ",i,inputDatas_L[i]);
+    }
+    pc.printf("\n\r");
 }
 
-void SendDarasToKeiki(){
+void SendDatas(){
+    can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, IMPUT_DATAS_NUM));
+    can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, IMPUT_DATAS_NUM));
     for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
         toKeikiSerial.putc(yokutanDatas_R[i]);
         toKeikiSerial.putc(yokutanDatas_L[i]);
     }
+    for(int i = 0; i < IMPUT_DATAS_NUM; i++){
+        toKeikiSerial.putc(inputDatas_R[i]);
+        toKeikiSerial.putc(inputDatas_L[i]);
+    }
+    toKeikiSerial.putc(cadence);
+    pc.printf("test\n\r");
 }
 
-void toString_R(){
-    for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
-        pc.printf("R : ");
-        pc.printf("%i:",yokutanDatas_R[i]);
-    }
-    for(int i = 0; i < IMPUT_DATAS_NUM; i++){
-       // pc.printf("%d:%d",i,inputDatas_R[i]);
+void calcCadence(){
+    //pc.printf("%f,%f\n\r",p.read(),q.read());
+    if(p.read() > THRESHOLD_OH_VALUE) stateP = true;
+    else stateP = false;
+    if(q.read() > THRESHOLD_OH_VALUE) stateQ = true;
+    else stateQ = false;
+    if((stateP == true && stateP_old == false) || 
+        stateQ == true && stateQ_old == false){
+        cadenceTimer.stop();
+        cadence = (char)30.0/cadenceTimer.read();
+        pc.printf("Cadence:%i\n\r",cadence);
+        cadenceTimer.reset();
+        cadenceTimer.start();
     }
-  //  pc.printf("\t");
-}
-
-void toString_L(){
-    for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
-        pc.printf("L : ");
-        pc.printf("%i:",yokutanDatas_L[i]);
-    }
-    pc.printf("\n\r");
-}
+    stateP_old = stateP;
+    stateQ_old = stateQ;
+}        
 
 void init(){
+    cadenceTimer.start();
+    cadenceTicker.attach(calcCadence, CALC_CADENCE_TIME);
+    sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
     for(int i = 0; i < IMPUT_DATAS_NUM; i++){
         inputDatas_R[i] = 0;
         inputDatas_L[i] = 0;
@@ -98,32 +116,27 @@
 
 void receiveDatas(){
     if(can_R.read(recmsg_R)){
-        ledcheck_R();
         for(int i = 0; i < recmsg_R.len; i++){
             yokutanDatas_R[i] = recmsg_R.data[i];
-           // pc.printf("R : %d , %d\t",recmsg_R.data[i]);
         }
+        myled1 = !myled1;
     }
      if(can_L.read(recmsg_L)){
-        ledcheck_L();
         for(int i = 0; i < recmsg_L.len; i++){
             yokutanDatas_L[i] = recmsg_L.data[i];
-        //    pc.printf("L : %d , %d\t",recmsg_L.data[i]);
         }
+        myled2 = !myled2;
     }
 }
 
-
 int main(){
     init();
     while(1){
-        //servo_check();
-        InputAndSentControlValues_R();
-        InputAndSentControlValues_L();
+        InputControlValues();
         wait_us(10);
         receiveDatas();
-        toString_R();
-        toString_L();
+        //toString_R();
+//        toString_L();
         wait(WAIT_LOOP_TIME);
     }
 }
\ No newline at end of file