Diff: main.cpp

Branch:

test

Revision:

10:b7159feb11fd

Parent:

9:c32999b4d2f9

Child:

13:79eb74eb86c4

--- a/main.cpp	Sat Jan 21 11:46:19 2017 +0000
+++ b/main.cpp	Thu Feb 16 14:51:53 2017 +0000
@@ -2,9 +2,9 @@
 
 #include "mbed.h"
 //#include "BufferedSoftSerial.h"
-#define WAIT_LOOP_TIME 0.1
+#define WAIT_LOOP_TIME 0.001
 #define YOKUTAN_DATAS_NUM 7
-#define IMPUT_DATAS_NUM 2
+#define INPUT_DATAS_NUM 5
 #define SEND_DATAS_CAN_ID 100
 #define SEND_DATAS_TIME 0.5
 #define THRESHOLD_OH_VALUE 0.14
@@ -28,23 +28,25 @@
 Serial toKeikiSerial(p13,p14);
 //BufferedSoftSerial toKeikiSerial(p17,p18);
 Serial pc(USBTX,USBRX);
-DigitalIn eruron_R(p22);
-DigitalIn eruron_L(p20);
+//DigitalIn eruron_R(p22);
+//DigitalIn eruron_L(p20);
+AnalogIn rollPin(p15);
+AnalogIn pichPin(p16);
 DigitalIn drug_R(p23);
 DigitalIn drug_L(p19);
 DigitalOut myled1(LED1);
 DigitalOut myled2(LED2);
 //Ticker sendDatasTicker;
-Ticker cadenceTicker;
-Timer cadenceTimer;
-AnalogIn p(p15);
-AnalogIn q(p16);
+//Ticker cadenceTicker;
+//Timer cadenceTimer;
+//AnalogIn p(p15);
+//AnalogIn q(p16);
 
 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;
+char inputDatas_R[INPUT_DATAS_NUM];
+char inputDatas_L[INPUT_DATAS_NUM];
+//char cadence = 0;
 bool stateP = true;
 bool stateQ = true;
 bool stateP_old = true;
@@ -53,41 +55,51 @@
 CANMessage recmsg_R;
 CANMessage recmsg_L;
 
+float get_analogin_ave(AnalogIn pin, int n){
+    float val = 0;
+    for(int i = 0; i<n; i++){
+        val += pin.read();
+    }return val/n;
+}
+
 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;
-    }
+    *(float *)inputDatas_R = get_analogin_ave(pichPin,8) + get_analogin_ave(rollPin,8);
+    *(float *)inputDatas_L = get_analogin_ave(pichPin,8) - get_analogin_ave(rollPin,8);
+    pc.printf("input_R:%f   input_L:%f\n\r",*(float *)inputDatas_R,*(float *)inputDatas_L);
+    inputDatas_R[4] = (char)drug_R;
+    inputDatas_L[4] = (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 toString_R(){
     pc.printf("R:");
-    for(int i = 0; i < IMPUT_DATAS_NUM; i++){
+    for(int i = 0; i < INPUT_DATAS_NUM; i++){
         pc.printf("%d:%i    ",i,inputDatas_R[i]);
     }
 }
 
 void toString_L(){
     pc.printf("L:");
-    for(int i = 0; i < IMPUT_DATAS_NUM; i++){
+    for(int i = 0; i < INPUT_DATAS_NUM; i++){
         pc.printf("%d:%i    ",i,inputDatas_L[i]);
     }
     pc.printf("\n\r");
 }
 
 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));
+    can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM));
+    can_L.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_L, INPUT_DATAS_NUM));
     toKeikiSerial.putc(';');
     for(int i = 0; i < YOKUTAN_DATAS_NUM; i++){
 //        toKeikiSerial.putc(i);
@@ -95,7 +107,7 @@
         toKeikiSerial.putc(yokutanDatas_R[i]);
         toKeikiSerial.putc(yokutanDatas_L[i]);
     }
-    for(int i = 0; i < IMPUT_DATAS_NUM; i++){
+    for(int i = 0; i < INPUT_DATAS_NUM; i++){
 //        toKeikiSerial.putc(4+i);
 //        toKeikiSerial.putc(14+i);
         toKeikiSerial.putc(inputDatas_R[i]);
@@ -105,33 +117,33 @@
     pc.printf("test\n\r");
 }
 
-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();
-    }
-    stateP_old = stateP;
-    stateQ_old = stateQ;
-}        
-
+//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();
+//    }
+//    stateP_old = stateP;
+//    stateQ_old = stateQ;
+//}        
+//
 void init(){
 //--------------------------------------(resetInterrupt init)
     resetPin.rise(resetInterrupt);
     resetPin.mode(PullDown);
 //-----------------------------------------------------------
-    cadenceTimer.start();
-    cadenceTicker.attach(calcCadence, CALC_CADENCE_TIME);
+//    cadenceTimer.start();
+//    cadenceTicker.attach(calcCadence, CALC_CADENCE_TIME);
 //    sendDatasTicker.attach(SendDatas,SEND_DATAS_TIME);
-    for(int i = 0; i < IMPUT_DATAS_NUM; i++){
+    for(int i = 0; i < INPUT_DATAS_NUM; i++){
         inputDatas_R[i] = 0;
         inputDatas_L[i] = 0;
     }
@@ -157,10 +169,10 @@
     init();
     while(1){
         InputControlValues();
-        wait_us(10);
+        wait_us(5);
         receiveDatas();
         SendDatas();
-        //toString_R();
+//        toString_R();
 //        toString_L();
         wait(WAIT_LOOP_TIME);
     }