albatross
/
ControlMain2017
2017年度の製作を開始します
Fork of Control_Main_Full_20160608 by
Diff: main.cpp
- Revision:
- 31:3a6be197e98f
- Parent:
- 30:eb7b7af1070a
- Child:
- 32:60af346703d3
--- a/main.cpp Tue Mar 21 23:23:32 2017 +0000 +++ b/main.cpp Thu Mar 23 08:50:19 2017 +0000 @@ -3,7 +3,7 @@ #include "mbed.h" #define WAIT_LOOP_TIME 0.001 #define YOKUTAN_DATAS_NUM 7 -#define INPUT_DATAS_NUM 5 +#define INPUT_DATAS_NUM 7 #define SEND_DATAS_CAN_ID 100 #define SEND_DATAS_TIME 0.5 #define THRESHOLD_OH_VALUE 0.14 @@ -35,9 +35,9 @@ Serial toKeikiSerial(p28,p27); Serial pc(USBTX,USBRX); AnalogIn rollPin(p15); -AnalogIn pitchPin(p20); +AnalogIn pitchPin(p18); DigitalIn drug_R(p14); -DigitalIn drug_L(p19); +DigitalIn drug_L(p17); DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led4(LED4); @@ -110,12 +110,12 @@ if(Upper > Lower) { if(value < neutral) { value = neutral - ((neutral - value) * (Upper / Lower)); - // rollLowerDiff = (rollLowerDiff)* (Upper / Lower); //現状範囲値を変えれてないから、ここは要対応。0で割ってしまってnanが出るようになったからコメントアウトした。 + rollLowerDiff = -rollUpperDiff; } } else { if(value > neutral) { value = neutral + ((value - neutral) * (Lower / Upper)); - // rollUpperDiff = rollUpperDiff* (Upper / Lower); + rollUpperDiff = -rollLowerDiff; } } return value; @@ -123,12 +123,12 @@ if(Upper > Lower) { if(value < neutral) { value = neutral - ((neutral - value) * (Upper / Lower)); - // pitchLowerDiff = (pitchLowerDiff)* (Upper / Lower); + pitchLowerDiff = -pitchUpperDiff; } } else { if(value > neutral) { value = neutral + ((value - neutral) * (Lower / Upper)); - // pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower); + pitchUpperDiff= -pitchLowerDiff; } } return value; @@ -144,10 +144,6 @@ result= max; else if(value < min) result = min; - else if(value > 1) - result = 1; - else if(value < 0) - result = 0; else result = value; return result; @@ -198,12 +194,14 @@ else if(*(int *)inputDatas_L > PHASE_NUM) *(int *)inputDatas_L =PHASE_NUM; - pc.printf("input_R:%d input_L:%d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L); - inputDatas_R[4] = (char)drug_R; + // pc.printf("DR :%d DL:%d input_R:%d input_L:%d\n\r",drug_R.read(),drug_L.read(),*(int *)inputDatas_R,*(int *)inputDatas_L); + inputDatas_R[sizeof(int)+ 2] = (char)drug_R; led4 =! led4; // pc.printf("%c",*(char *)inputDatas_R[4]); //pc.printf("%c",(char)drug_R); - inputDatas_L[4] = (char)drug_L; + inputDatas_L[sizeof(int)+2] = (char)drug_L; + // pc.printf("eruron:%f drug:%f\n\r,",); + pc.printf(" DL:%d input_R:%d\n\r",inputDatas_L[sizeof(int)+2],*(int *)inputDatas_L); // pc.printf(""); } @@ -213,7 +211,7 @@ 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(yokutanDatas_R[i]); toKeikiSerial.putc(yokutanDatas_L[i]); }