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);
+}