test
Dependencies: PID USBDevice mbed
Diff: main.cpp
- Revision:
- 0:3115bcc751cb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Sep 12 05:11:31 2014 +0000 @@ -0,0 +1,158 @@ +#include "mbed.h" +#include "USBSerial.h" +#include "PID.h" + +#define WHITE 0.6 + +Ticker tick; + +DigitalOut airdiode(P1_27); +AnalogIn aphotoref(P0_22); +DigitalOut birdiode(P1_20); +AnalogIn bphotoref(P0_11); +DigitalOut cirdiode(P1_26); +AnalogIn cphotoref(P0_12); +DigitalOut dirdiode(P1_21); +AnalogIn dphotoref(P0_13); +DigitalOut eirdiode(P0_22); +AnalogIn ephotoref(P0_14); + +DigitalOut myled(P1_28); +DigitalIn sw(P0_1); + +USBSerial vcom; + +void systeminit(void); + +float asens, bsens, csens, dsens, esens; + + +void flip(){ + static uint8_t cycle; + static float asenson, asensoff, bsenson, bsensoff, csenson, csensoff, dsenson, dsensoff, esenson, esensoff; + switch(cycle){ + case 0: + asensoff = aphotoref.read(); + bsensoff = bphotoref.read(); + csensoff = cphotoref.read(); + dsensoff = dphotoref.read(); + esensoff = ephotoref.read(); +// airdiode = 1; + cycle++; + break; + case 1: + asenson = aphotoref.read(); +// airdiode = 0; +// birdiode = 1; + cycle++; + break; + case 2: + bsenson = bphotoref.read(); +// birdiode = 0; +// cirdiode = 1; + cycle++; + break; + case 3: + csenson = cphotoref.read(); +// cirdiode = 0; +// dirdiode = 1; + cycle++; + break; + case 4: + dsenson = dphotoref.read(); +// dirdiode = 0; +// eirdiode = 1; + cycle++; + break; + case 5: + esenson = ephotoref.read(); +// eirdiode = 0; + cycle++; + break; + case 6: +/* asens = (asenson - asensoff) * 1.4; + bsens = (bsenson - bsensoff) * 0.95; + csens = csenson - csensoff; + dsens = dsenson - dsensoff; + esens = esenson - esensoff; +*/ + asens = (asenson) * 1.4; + bsens = (bsenson) * 0.95; + csens = csenson; + dsens = dsenson; + esens = esenson; + cycle = 0; + break; + default: + cycle = 0; + break; + } + myled = !myled; +} + +int main(){ + float position = 0, gposition = 0; //細かい分け方と全体の位置 + uint8_t lineblock = 0; //大雑把な分け方 + systeminit(); + while(1){ + vcom.printf("\033[%d;%dH" ,0,0); + vcom.printf("%7.6f\n", asens); + vcom.printf("%7.6f\n", bsens); + vcom.printf("%7.6f\n", csens); + vcom.printf("%7.6f\n", dsens); + vcom.printf("position = %7.6f\n", position); + vcom.printf("lineblock = %d\n", lineblock); + vcom.printf("gposition = %8.6f\n", gposition); + + if((asens < WHITE) && (bsens > WHITE)){ + /*一番 左*/ + position = asens - bsens; + lineblock = 0; + }else if((asens < WHITE) && (bsens < WHITE)){ + /*結構 左*/ + position = asens - bsens; + lineblock = 1; + }else if((bsens < WHITE) && (csens > WHITE)){ + /*ちょっと左*/ + position = bsens - csens; + lineblock = 2; + }else if((bsens < WHITE) && (csens < WHITE)){ + /*真ん中*/ + position = bsens - csens; + lineblock = 3; + }else if((csens < WHITE) && (dsens > WHITE)){ + /*ちょっと右*/ + position = bsens - csens; + lineblock = 4; + }else if((csens < WHITE) && (dsens < WHITE)){ + /*結構右*/ + position = csens - dsens; + lineblock = 5; + }else if((csens > WHITE) && (dsens < WHITE)){ + /*一番 右*/ + position = csens - dsens; + lineblock = 6; + } + gposition = (position + 0.6) * 14; //値1につき12mm + if(lineblock < 2){ + /*左 16.8mm*/ + }else if(lineblock < 5){ + /*真ん中 16.8mm*/ + gposition += 16.8; + }else if(lineblock < 7){ + /*右 16.8mm*/ + gposition += 33.6; + } + wait(0.012); + } +} + +void systeminit(void){ + sw.mode(PullUp); + tick.attach(&flip, 0.005); + airdiode = 1; + birdiode = 1; + cirdiode = 1; + dirdiode = 1; + wait(0.3); +}