Diff: main.cpp

Revision:

1:b81c22891370

Parent:

0:4c7c138f3891

--- a/main.cpp	Sat Oct 19 04:50:26 2013 +0000
+++ b/main.cpp	Sun Oct 20 01:39:55 2013 +0000
@@ -1,7 +1,7 @@
 //カラーセンサ6つ同時に動かすプログラム
 //大会用ロボ
 //ジャンプのプログラムも入ってる
-//10月17日に使う
+//ジャンプ命令のプログラムを変更
 #include "mbed.h"
 #include "ColorSensor.h"
 #include "Servo.h"
@@ -11,11 +11,17 @@
 
 
 
-int cAve,roboF, jumI;
+int cAve[COLOR_NUM] = {0}, roboF, jumI;
 
 void tic_sensor()
 {
     colorUpdate(1);
+    
+    
+    for(int i=0; i<COLOR_NUM; i++){
+        cAve[i] = moving_ave(i, bluep[i]);
+    }
+    
     roboF = robotFront();
     jumI = jumpInstruction(roboF);
     //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
@@ -117,16 +123,10 @@
 uint8_t robotFront()//縄回しロボの正面にいるか
 {
     uint8_t threshold = 0;
-    
+        
     for(int i=0; i<COLOR_NUM; i++){
-        if(moving_ave(i, bluep[i]) >= 8) threshold++;
+        if(cAve[i] >= 8) threshold++;
     }
-    
-    
-    /*for(int i=0; i<COLOR_NUM; i++){
-        cAve = moving_ave(i, bluep[i]);
-        if(cAve >= 8) threshold++;
-    }*/
 
     if(threshold >= 1){
         return 1;
@@ -139,10 +139,8 @@
 {
     uint8_t threshold = 0;
     
-    if(front == 1){
-        for(int i=0; i<COLOR_NUM; i++){
-            if(redp[i] >= R_THR) threshold++;
-        }
+    for(int i=0; i<COLOR_NUM; i++){
+        if((cAve[i] >= 8) && (redp[i] >= R_THR))  threshold++;
     }
     
     if(threshold >= 1){
@@ -210,11 +208,34 @@
     
 }*/
 
+void jumpAction(uint8_t threshold)
+{
+    static int jumpFlag;
+    
+    
+    if(sw1 == 0){//フォトインタラプタがオン
+        jumpFlag = 1;
+    }
+    
+    
+    if(threshold){
+        jumpFlag = 0;
+        led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
+        myservo1 = 0;
+        wait(0.4);
+    }
+    
+    
+    if(jumpFlag){
+        myservo1 = 0.5; led[0] = 1;led[1] = 0; led[2] = 0; led[3] = 0;
+    }
+
+    
+}
 
 
 
-
-
+/*
 #define SEND_DATA_NUM       7
 #define RECEIVE_DATA_NUM    7
 
@@ -240,7 +261,7 @@
     
     led[4] = 1;
 }
-
+*/
 
 
 
@@ -259,11 +280,14 @@
     //sw1.mode(PullUp);
     //init end
 
+    myservo1 = 0.1; 
+
     while(1) {
-        SendData0[1] = jumI;    
+        //SendData0[1] = jumI;    
         
         pc.printf("R:%d G:%d B:%d %t moving_ave():%d %t robotFront():%d %t jumpInstruction():%d\n", redp[2], greenp[2], bluep[2], cAve, roboF, jumI);
         
-        //jumpAction(jumI);
+               
+        jumpAction(jumI);
     }
 }