test
Dependencies: PID USBDevice mbed
main.cpp@0:3115bcc751cb, 2014-09-12 (annotated)
- Committer:
- NT32
- Date:
- Fri Sep 12 05:11:31 2014 +0000
- Revision:
- 0:3115bcc751cb
board_test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
NT32 | 0:3115bcc751cb | 1 | #include "mbed.h" |
NT32 | 0:3115bcc751cb | 2 | #include "USBSerial.h" |
NT32 | 0:3115bcc751cb | 3 | #include "PID.h" |
NT32 | 0:3115bcc751cb | 4 | |
NT32 | 0:3115bcc751cb | 5 | #define WHITE 0.6 |
NT32 | 0:3115bcc751cb | 6 | |
NT32 | 0:3115bcc751cb | 7 | Ticker tick; |
NT32 | 0:3115bcc751cb | 8 | |
NT32 | 0:3115bcc751cb | 9 | DigitalOut airdiode(P1_27); |
NT32 | 0:3115bcc751cb | 10 | AnalogIn aphotoref(P0_22); |
NT32 | 0:3115bcc751cb | 11 | DigitalOut birdiode(P1_20); |
NT32 | 0:3115bcc751cb | 12 | AnalogIn bphotoref(P0_11); |
NT32 | 0:3115bcc751cb | 13 | DigitalOut cirdiode(P1_26); |
NT32 | 0:3115bcc751cb | 14 | AnalogIn cphotoref(P0_12); |
NT32 | 0:3115bcc751cb | 15 | DigitalOut dirdiode(P1_21); |
NT32 | 0:3115bcc751cb | 16 | AnalogIn dphotoref(P0_13); |
NT32 | 0:3115bcc751cb | 17 | DigitalOut eirdiode(P0_22); |
NT32 | 0:3115bcc751cb | 18 | AnalogIn ephotoref(P0_14); |
NT32 | 0:3115bcc751cb | 19 | |
NT32 | 0:3115bcc751cb | 20 | DigitalOut myled(P1_28); |
NT32 | 0:3115bcc751cb | 21 | DigitalIn sw(P0_1); |
NT32 | 0:3115bcc751cb | 22 | |
NT32 | 0:3115bcc751cb | 23 | USBSerial vcom; |
NT32 | 0:3115bcc751cb | 24 | |
NT32 | 0:3115bcc751cb | 25 | void systeminit(void); |
NT32 | 0:3115bcc751cb | 26 | |
NT32 | 0:3115bcc751cb | 27 | float asens, bsens, csens, dsens, esens; |
NT32 | 0:3115bcc751cb | 28 | |
NT32 | 0:3115bcc751cb | 29 | |
NT32 | 0:3115bcc751cb | 30 | void flip(){ |
NT32 | 0:3115bcc751cb | 31 | static uint8_t cycle; |
NT32 | 0:3115bcc751cb | 32 | static float asenson, asensoff, bsenson, bsensoff, csenson, csensoff, dsenson, dsensoff, esenson, esensoff; |
NT32 | 0:3115bcc751cb | 33 | switch(cycle){ |
NT32 | 0:3115bcc751cb | 34 | case 0: |
NT32 | 0:3115bcc751cb | 35 | asensoff = aphotoref.read(); |
NT32 | 0:3115bcc751cb | 36 | bsensoff = bphotoref.read(); |
NT32 | 0:3115bcc751cb | 37 | csensoff = cphotoref.read(); |
NT32 | 0:3115bcc751cb | 38 | dsensoff = dphotoref.read(); |
NT32 | 0:3115bcc751cb | 39 | esensoff = ephotoref.read(); |
NT32 | 0:3115bcc751cb | 40 | // airdiode = 1; |
NT32 | 0:3115bcc751cb | 41 | cycle++; |
NT32 | 0:3115bcc751cb | 42 | break; |
NT32 | 0:3115bcc751cb | 43 | case 1: |
NT32 | 0:3115bcc751cb | 44 | asenson = aphotoref.read(); |
NT32 | 0:3115bcc751cb | 45 | // airdiode = 0; |
NT32 | 0:3115bcc751cb | 46 | // birdiode = 1; |
NT32 | 0:3115bcc751cb | 47 | cycle++; |
NT32 | 0:3115bcc751cb | 48 | break; |
NT32 | 0:3115bcc751cb | 49 | case 2: |
NT32 | 0:3115bcc751cb | 50 | bsenson = bphotoref.read(); |
NT32 | 0:3115bcc751cb | 51 | // birdiode = 0; |
NT32 | 0:3115bcc751cb | 52 | // cirdiode = 1; |
NT32 | 0:3115bcc751cb | 53 | cycle++; |
NT32 | 0:3115bcc751cb | 54 | break; |
NT32 | 0:3115bcc751cb | 55 | case 3: |
NT32 | 0:3115bcc751cb | 56 | csenson = cphotoref.read(); |
NT32 | 0:3115bcc751cb | 57 | // cirdiode = 0; |
NT32 | 0:3115bcc751cb | 58 | // dirdiode = 1; |
NT32 | 0:3115bcc751cb | 59 | cycle++; |
NT32 | 0:3115bcc751cb | 60 | break; |
NT32 | 0:3115bcc751cb | 61 | case 4: |
NT32 | 0:3115bcc751cb | 62 | dsenson = dphotoref.read(); |
NT32 | 0:3115bcc751cb | 63 | // dirdiode = 0; |
NT32 | 0:3115bcc751cb | 64 | // eirdiode = 1; |
NT32 | 0:3115bcc751cb | 65 | cycle++; |
NT32 | 0:3115bcc751cb | 66 | break; |
NT32 | 0:3115bcc751cb | 67 | case 5: |
NT32 | 0:3115bcc751cb | 68 | esenson = ephotoref.read(); |
NT32 | 0:3115bcc751cb | 69 | // eirdiode = 0; |
NT32 | 0:3115bcc751cb | 70 | cycle++; |
NT32 | 0:3115bcc751cb | 71 | break; |
NT32 | 0:3115bcc751cb | 72 | case 6: |
NT32 | 0:3115bcc751cb | 73 | /* asens = (asenson - asensoff) * 1.4; |
NT32 | 0:3115bcc751cb | 74 | bsens = (bsenson - bsensoff) * 0.95; |
NT32 | 0:3115bcc751cb | 75 | csens = csenson - csensoff; |
NT32 | 0:3115bcc751cb | 76 | dsens = dsenson - dsensoff; |
NT32 | 0:3115bcc751cb | 77 | esens = esenson - esensoff; |
NT32 | 0:3115bcc751cb | 78 | */ |
NT32 | 0:3115bcc751cb | 79 | asens = (asenson) * 1.4; |
NT32 | 0:3115bcc751cb | 80 | bsens = (bsenson) * 0.95; |
NT32 | 0:3115bcc751cb | 81 | csens = csenson; |
NT32 | 0:3115bcc751cb | 82 | dsens = dsenson; |
NT32 | 0:3115bcc751cb | 83 | esens = esenson; |
NT32 | 0:3115bcc751cb | 84 | cycle = 0; |
NT32 | 0:3115bcc751cb | 85 | break; |
NT32 | 0:3115bcc751cb | 86 | default: |
NT32 | 0:3115bcc751cb | 87 | cycle = 0; |
NT32 | 0:3115bcc751cb | 88 | break; |
NT32 | 0:3115bcc751cb | 89 | } |
NT32 | 0:3115bcc751cb | 90 | myled = !myled; |
NT32 | 0:3115bcc751cb | 91 | } |
NT32 | 0:3115bcc751cb | 92 | |
NT32 | 0:3115bcc751cb | 93 | int main(){ |
NT32 | 0:3115bcc751cb | 94 | float position = 0, gposition = 0; //細かい分け方と全体の位置 |
NT32 | 0:3115bcc751cb | 95 | uint8_t lineblock = 0; //大雑把な分け方 |
NT32 | 0:3115bcc751cb | 96 | systeminit(); |
NT32 | 0:3115bcc751cb | 97 | while(1){ |
NT32 | 0:3115bcc751cb | 98 | vcom.printf("\033[%d;%dH" ,0,0); |
NT32 | 0:3115bcc751cb | 99 | vcom.printf("%7.6f\n", asens); |
NT32 | 0:3115bcc751cb | 100 | vcom.printf("%7.6f\n", bsens); |
NT32 | 0:3115bcc751cb | 101 | vcom.printf("%7.6f\n", csens); |
NT32 | 0:3115bcc751cb | 102 | vcom.printf("%7.6f\n", dsens); |
NT32 | 0:3115bcc751cb | 103 | vcom.printf("position = %7.6f\n", position); |
NT32 | 0:3115bcc751cb | 104 | vcom.printf("lineblock = %d\n", lineblock); |
NT32 | 0:3115bcc751cb | 105 | vcom.printf("gposition = %8.6f\n", gposition); |
NT32 | 0:3115bcc751cb | 106 | |
NT32 | 0:3115bcc751cb | 107 | if((asens < WHITE) && (bsens > WHITE)){ |
NT32 | 0:3115bcc751cb | 108 | /*一番 左*/ |
NT32 | 0:3115bcc751cb | 109 | position = asens - bsens; |
NT32 | 0:3115bcc751cb | 110 | lineblock = 0; |
NT32 | 0:3115bcc751cb | 111 | }else if((asens < WHITE) && (bsens < WHITE)){ |
NT32 | 0:3115bcc751cb | 112 | /*結構 左*/ |
NT32 | 0:3115bcc751cb | 113 | position = asens - bsens; |
NT32 | 0:3115bcc751cb | 114 | lineblock = 1; |
NT32 | 0:3115bcc751cb | 115 | }else if((bsens < WHITE) && (csens > WHITE)){ |
NT32 | 0:3115bcc751cb | 116 | /*ちょっと左*/ |
NT32 | 0:3115bcc751cb | 117 | position = bsens - csens; |
NT32 | 0:3115bcc751cb | 118 | lineblock = 2; |
NT32 | 0:3115bcc751cb | 119 | }else if((bsens < WHITE) && (csens < WHITE)){ |
NT32 | 0:3115bcc751cb | 120 | /*真ん中*/ |
NT32 | 0:3115bcc751cb | 121 | position = bsens - csens; |
NT32 | 0:3115bcc751cb | 122 | lineblock = 3; |
NT32 | 0:3115bcc751cb | 123 | }else if((csens < WHITE) && (dsens > WHITE)){ |
NT32 | 0:3115bcc751cb | 124 | /*ちょっと右*/ |
NT32 | 0:3115bcc751cb | 125 | position = bsens - csens; |
NT32 | 0:3115bcc751cb | 126 | lineblock = 4; |
NT32 | 0:3115bcc751cb | 127 | }else if((csens < WHITE) && (dsens < WHITE)){ |
NT32 | 0:3115bcc751cb | 128 | /*結構右*/ |
NT32 | 0:3115bcc751cb | 129 | position = csens - dsens; |
NT32 | 0:3115bcc751cb | 130 | lineblock = 5; |
NT32 | 0:3115bcc751cb | 131 | }else if((csens > WHITE) && (dsens < WHITE)){ |
NT32 | 0:3115bcc751cb | 132 | /*一番 右*/ |
NT32 | 0:3115bcc751cb | 133 | position = csens - dsens; |
NT32 | 0:3115bcc751cb | 134 | lineblock = 6; |
NT32 | 0:3115bcc751cb | 135 | } |
NT32 | 0:3115bcc751cb | 136 | gposition = (position + 0.6) * 14; //値1につき12mm |
NT32 | 0:3115bcc751cb | 137 | if(lineblock < 2){ |
NT32 | 0:3115bcc751cb | 138 | /*左 16.8mm*/ |
NT32 | 0:3115bcc751cb | 139 | }else if(lineblock < 5){ |
NT32 | 0:3115bcc751cb | 140 | /*真ん中 16.8mm*/ |
NT32 | 0:3115bcc751cb | 141 | gposition += 16.8; |
NT32 | 0:3115bcc751cb | 142 | }else if(lineblock < 7){ |
NT32 | 0:3115bcc751cb | 143 | /*右 16.8mm*/ |
NT32 | 0:3115bcc751cb | 144 | gposition += 33.6; |
NT32 | 0:3115bcc751cb | 145 | } |
NT32 | 0:3115bcc751cb | 146 | wait(0.012); |
NT32 | 0:3115bcc751cb | 147 | } |
NT32 | 0:3115bcc751cb | 148 | } |
NT32 | 0:3115bcc751cb | 149 | |
NT32 | 0:3115bcc751cb | 150 | void systeminit(void){ |
NT32 | 0:3115bcc751cb | 151 | sw.mode(PullUp); |
NT32 | 0:3115bcc751cb | 152 | tick.attach(&flip, 0.005); |
NT32 | 0:3115bcc751cb | 153 | airdiode = 1; |
NT32 | 0:3115bcc751cb | 154 | birdiode = 1; |
NT32 | 0:3115bcc751cb | 155 | cirdiode = 1; |
NT32 | 0:3115bcc751cb | 156 | dirdiode = 1; |
NT32 | 0:3115bcc751cb | 157 | wait(0.3); |
NT32 | 0:3115bcc751cb | 158 | } |