naoto tanaka / Mbed 2 deprecated line_centering

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?

UserRevisionLine numberNew 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 }