Diff: main.cpp

Revision:

0:3a98a198e6d1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Oct 05 02:08:45 2013 +0000
@@ -0,0 +1,184 @@
+//5_1を通信用にカラーセンサだけに変えたプログラム
+#include "mbed.h"
+#include "ColorSensor.h"
+
+#include "main.h"
+
+
+void tic_sensor()
+{
+
+    //if(sw == 0)step=2;
+    colorUpdate(step);
+    
+    /*lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+    */
+}
+
+
+
+////////////////////////////////////////カラーセンサの////////////////////////////////////////
+////////////////////////////////////////補正プログラム////////////////////////////////////////
+void rivisedate()
+{
+    unsigned long red = 0,green = 0,blue =0;
+    static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
+    
+    //最初の20回だけ平均を取る
+    for (int i=0;i<=20;i++){
+         color0.getRGB(R[0],G[0],B[0]);
+         red       += R[0] ;
+         green     += G[0] ;
+         blue      += B[0] ;
+         //pc.printf(" %d  %d\n",ptm(sum),sum);
+    }
+    
+    rir = (double)green/ red ;
+    rib = (double)green/ blue ;
+}
+
+void colorUpdate(uint8_t mode)
+{
+    double colorSum[COLOR_NUM];
+    unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
+
+
+    for (int i=0; i<COLOR_NUM; i++){
+        R[i] = 0;
+        G[i] = 0;
+        B[i] = 0;
+        redp[i]   = 0;
+        greenp[i] = 0;
+        bluep[i]  = 0;
+    }
+    
+    //カラーセンサの切り替え    
+    if(mode == 1){
+        color0.getRGB(R[0],G[0],B[0]);
+        color1.getRGB(R[1],G[1],B[1]);
+        color2.getRGB(R[2],G[2],B[2]);
+    }else{
+        color3.getRGB(R[3],G[3],B[3]);
+        color4.getRGB(R[4],G[4],B[4]);
+        color5.getRGB(R[5],G[5],B[5]);
+    }
+    
+    for (int i=0; i<COLOR_NUM; i++){
+        colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
+        redp[i]   = R[i]* rir * 100 / colorSum[i];
+        greenp[i] = G[i]      * 100 / colorSum[i];
+        bluep[i]  = B[i]* rib * 100 / colorSum[i];
+    }
+}
+
+////////////////////////////////////////ジャンププログラム////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+uint8_t jumpcondition()
+{
+    uint8_t threshold = 0, t[3] = {0};
+    
+    //青から赤に0.5秒以内に反応したらジャンプ
+    for(int i=0; i<COLOR_NUM; i++){
+        if(bluep[i] >= B_THR){
+            color_t[i].reset();
+            color_t[i].start();
+            t[i] = 0;
+        }else if(redp[i] >= R_THR){
+            t[i] = color_t[i].read_ms();
+        }else{
+            t[i] = 0;
+        }
+ 
+        if((t[i] <= 500) && (t[i] != 0)){
+            threshold++;
+        }
+    }
+    
+    return threshold;
+}
+
+/*
+void jumping(uint8_t threshold)
+{
+    //超音波でジャンプのタイミング合わせる
+    if(threshold >= 1){
+            jump_t.reset();
+            jump_t.start();
+            while(ultrasonicVal[0] < 1700){
+                led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
+                air[0] = 1;  air[1] = 0;
+                
+                if(jump_t.read_ms() > 1000)break;
+            }
+            led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
+            air[0] = 0;  air[1] = 1;
+            wait(0.5);
+    }else{
+            led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
+    }
+}*/
+
+
+uint8_t robostop()
+{
+    if(bluep[1] >= B_THR){
+        return 1;
+    }else{
+        return 0;
+    }
+}
+
+
+
+
+
+
+
+
+int main()
+{
+    rivisedate();
+
+    timer2.start();
+    ping_t.start();
+    
+        
+    //unsigned R, G, B;
+    int vl;
+    double vs;
+    uint8_t button, state=0;
+    
+    //pc.baud(115200);
+    //air[0] = 0; air[1] = 1;
+    step = 1;
+    
+    //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    //pidUpdata.attach(&PidUpdate, PID_CYCLE);
+    interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
+  
+  while(1)
+  {
+    
+    
+    
+    pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+    /*
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]);
+    lcd.locate(0,1);
+    lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]);
+     */
+    //pc.printf("%d\n".., ultrasonicVal[0]);
+    
+    
+    
+    
+    if(robostop() == 1) state = STOP;
+    jumpcondition();
+    
+
+  }
+}
\ No newline at end of file