driver

Dependencies:   HMC6352 PID mbed

Fork of ver3_1_0 by ryo seki

Files at this revision

API Documentation at this revision

Comitter:
yusuke_robocup
Date:
Fri Jan 24 06:27:03 2014 +0000
Parent:
0:bde8ed56b164
Commit message:
driver

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Wed Jun 19 08:42:55 2013 +0000
+++ b/main.cpp	Fri Jan 24 06:27:03 2014 +0000
@@ -6,13 +6,62 @@
 #include "main.h"
 
 
+void whiteliner()
+{
+    static int tmp[5] = {0};
+    static int sum = 0;
+    
+    sum -= tmp[4];
+    sum += whiteout_flag;
+    tmp[4] = tmp[3];
+    tmp[3] = tmp[2];
+    tmp[2] = tmp[1];
+    tmp[1] = tmp[0];
+    tmp[0] = whiteout_flag;
+    
+    //return sum/5;
+}
+
+int hansya_x(int x)
+{
+    static int tmp[5] = {0};
+    static int sum = 0;
+    
+    sum -= tmp[4];
+    sum += x;
+    tmp[4] = tmp[3];
+    tmp[3] = tmp[2];
+    tmp[2] = tmp[1];
+    tmp[1] = tmp[0];
+    tmp[0] = x;
+    
+    return sum/5;
+}
+
+
+int hansya_y(int y)
+{
+    static int tmp[5] = {0};
+    static int sum = 0;
+    
+    sum -= tmp[4];
+    sum += y;
+    tmp[4] = tmp[3];
+    tmp[3] = tmp[2];
+    tmp[2] = tmp[1];
+    tmp[1] = tmp[0];
+    tmp[0] = y;
+    
+    return sum/5;
+}
+
 
 void PidUpdate()
 {   
     static uint8_t Fflag = 0;
     
     pid.setSetPoint(((int)((REFERENCE + standTu) + 360) % 360) / 1.0); 
-    inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);        
+    inputPID = (((int)(compass.sample() - ((standard) * 10.0) + 5400.0) % 3600) / 10.0);        
     
     //pc.printf("%f\n",timer1.read());
     pid.setProcessValue(inputPID);
@@ -45,7 +94,7 @@
     }
 }
 
-
+/*
 void move(int vxx, int vyy, int vss)
 {
     double motVal[MOT_NUM] = {0};
@@ -60,6 +109,37 @@
         else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
         speed[i] = (int)motVal[i];
     }
+}*/
+
+void move(int vxx, int vyy, int vss)
+{
+    double motVal[MOT_NUM] = {0};
+    
+    int angle = static_cast<int>( atan2( (double)vyy, (double)vxx ) * 180/PI + 360 ) % 360;
+    
+    double hosei1 = 1,hosei2 = 1,hosei3 = 1,hosei4 = 1;
+    
+    if(angle == 180){
+        hosei1 = 1.3;
+    }
+    
+    motVal[0] = (double)(((0.5 * vxx)  + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1)*hosei1;
+    motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2)*hosei2;
+    motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3)*hosei3;
+    motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4)*hosei4;
+    
+    for(uint8_t i = 0 ; i < MOT_NUM ; i++){
+        if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
+        else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
+        speed[i] = motVal[i];
+    }
+    
+    //pc.printf("speed1 = %d\n",speed[0]);
+    //pc.printf("speed2 = %d\n",speed[1]);
+    //pc.printf("speed3 = %d\n",speed[2]);
+    //pc.printf("speed4 = %d\n\n",speed[3]);      
+    
+    ////pc.printf("%s",StringFIN.c_str());
 }
 
 int vector(double Amp,double degree,int xy)
@@ -121,6 +201,7 @@
     //driver.attach(&Rx_interrupt, Serial::RxIrq);
     device2.attach(&dev_rx,Serial::RxIrq);
     device2.attach(&dev_tx,Serial::TxIrq);
+    underIR.attach(&whiteliner, 0.05);
     
     pid.setInputLimits(MINIMUM,MAXIMUM);
     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
@@ -232,333 +313,372 @@
     //timer1.start();
 }
 
+
 int main()
 {
     int vx=0,vy=0,vs=0;
+    int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0;
+    int state_off = NONE;
+    int direction_av = 0;
+    int direction_past = 0;
+    int past_state_off = 0;
+    int accelera_Distance = 0;
     uint8_t whiteFlag = 0;
+    int save_vx = 0,save_vy = 0;
+    
+    int movein = 0;
     
     init();
            
-    while(1){
-        whiteFlag = 0;
-        hold_flag = 0;
-        vx = 0;
-        vy = 0;
-        vs = (int)compassPID;
-        //vs = 0;
+    while(1) {
+        
+        vx=0;
+        vy=0;
         
-        //move(vx,vy,vs);
+        //tuning = 0;
+    
+        x_dista = 0;
+        y_dista = 0;
+        x_turn  = 0;
+        y_turn  = 0;
+        //turn_flag = 0;
         
-        /*********************************************************************************************************
-        **********************************************************************************************************
-        ********************Change state *************************************************************************
-        **********************************************************************************************************
-        *********************************************************************************************************/
-        for(uint8_t i = 2;i < 6;i++)
+        vs = compassPID;
+        
+        //direction_av = moving_ave_5point(direction);
+        
+        
+        
+        for(uint8_t i = 0;i < 6;i++)
         {
             AnalogIn adcIn(adc_num[i]); /* 今回更新する赤外線の個体を呼び出す */
             irDistance[i] = ((adcIn.read_u16() >> 4) - stand[i]);
-            if(irDistance[i] >= 100)
+            if(irDistance[i] >= 30)
             {
                 whiteFlag = 1;
+                movein = 1;
+                whiteout = 10;
+            }        
+            
+            //pc.printf("%d\n",irDistance[0]);
+        }       
+        
+        if(lineStateX == XNORMAL){
+            if((LEFT_SONIC < 350) && (whiteFlag)){
+                lineStateX = LEFT_OUT;
             }
-            //pc.printf("%d\n",irDistance[0]);
+            if((LEFT_SONIC < 350) && (RIGHT_SONIC < 100) && (whiteFlag)){
+                lineStateX = LEFT_OUT;
+            }
+            
+            if((RIGHT_SONIC < 350) && (whiteFlag)){    
+                lineStateX = RIGHT_OUT;         
+            }
+            if((RIGHT_SONIC < 350) && (LEFT_SONIC < 100) && (whiteFlag)){
+                lineStateX = RIGHT_OUT;
+            }
+            
+        }else if(lineStateX == LEFT_OUT){
+            /*
+            if((LEFT_SONIC > 450) && (!whiteFlag)){
+                lineStateX = XNORMAL;
+                whiteFlag = 0;
+            }
+            */
+            if((LEFT_SONIC > 450)){
+                lineStateX = XNORMAL;
+                whiteFlag = 0;
+            }
+        }else if(lineStateX == RIGHT_OUT){
+            /*
+            if((RIGHT_SONIC > 450) && (!whiteFlag)){
+                lineStateX = XNORMAL;
+                whiteFlag = 0;
+            }
+            */
+            if((RIGHT_SONIC > 450)){
+                lineStateX = XNORMAL;
+                whiteFlag = 0;
+            }
         }
         
-        if(lineState == NORMAL){
-            if((LEFT_SONIC < 350) && (whiteFlag)){
-                lineState = LEFT_OUT;
-            }else if((RIGHT_SONIC < 350) && (whiteFlag)){    
-                lineState = RIGHT_OUT;         
-            }else if((FRONT_SONIC < 400) && (whiteFlag)){
-                lineState = FRONT_OUT;
-            }else if((BACK_SONIC < 400) && (whiteFlag)){
-                lineState = BACK_OUT;
+        
+        if(lineStateY == YNORMAL){
+            if((FRONT_SONIC < 400) && (whiteFlag)){
+                lineStateY = FRONT_OUT;
+            }
+            if((FRONT_SONIC < 400)&& (BACK_SONIC < 100) && (whiteFlag)){
+                lineStateY = FRONT_OUT;
+            }
+            if((BACK_SONIC < 400) && (whiteFlag)){
+                lineStateY = BACK_OUT;
+            }
+            if((BACK_SONIC < 400) && (FRONT_SONIC < 100) && (whiteFlag)){
+                lineStateY = BACK_OUT;
             }
-        }else if(lineState == LEFT_OUT){
-            if((LEFT_SONIC > 450) && (!whiteFlag)){
-                lineState = NORMAL;
+        }else if(lineStateY == FRONT_OUT){
+            /*
+            if((FRONT_SONIC > 500) && (!whiteFlag)){
+                lineStateY = YNORMAL;
+                whiteFlag = 0;
             }
-        }else if(lineState == RIGHT_OUT){
-            if((RIGHT_SONIC > 450) && (!whiteFlag)){
-                lineState = NORMAL;
+            */
+            if((FRONT_SONIC > 500)){
+                lineStateY = YNORMAL;
+                whiteFlag = 0;
             }
-        }else if(lineState == FRONT_OUT){
-            if((FRONT_SONIC > 500) && (!whiteFlag)){
-                lineState = NORMAL;
+        }else if(lineStateY == BACK_OUT){
+            /*
+            if((BACK_SONIC > 500) && (!whiteFlag)){
+                lineStateY = YNORMAL;
+                whiteFlag = 0;
             }
-        }else if(lineState == BACK_OUT){
-            if((BACK_SONIC > 500) && (!whiteFlag)){
-                lineState = NORMAL;
+            */
+            if((BACK_SONIC > 500)){
+                lineStateY = YNORMAL;
+                whiteFlag = 0;
             }
         }
         
         
-        if(state == HOLD){
-            if(FRONT_SONIC < 100)hold_flag = 1;
-            
-            if(Distance > 140){ //審判のボール持ち上げ判定
-                state = HOME_WAIT;
-            }else if(!((direction == 0) || (direction == 15) || (direction == 1) || (direction == 14) || (direction == 2))){//ボールを見失った
-                state = DIFFENCE;
-            }else if(!(Distance <= 40)){//ボールを見失った
-                state = DIFFENCE;
-            }
+        if((state_off == ATTACK)&&(Distance == 10)){
+            state = 1;
         }else{
-            standTu = 0;
-            if(state == DIFFENCE){
-                if((direction == 0) && (Distance <= 20) && (IR_found) && (!xbee)){
-                    state = HOLD;
-                }else if((Distance < 180) && (IR_found) && (!xbee)){
-                    state = DIFFENCE;
-                }else{
-                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
-                        if((IR_found) && (!xbee)){
-                            state = DIFFENCE;
-                        }
-                    }else if((diff > 15) && (!xbee) && (IR_found)){
-                         state = DIFFENCE;
-                    }else{
-                        state = HOME_WAIT;
-                    }
-                }
-                
-            }else{  
-                if((direction == 0) && (Distance <= 30) && (IR_found) && (!xbee)){
-                    state = HOLD;
-                }else if((Distance <= 150) && (IR_found) && (!xbee)){
-                    state = DIFFENCE;
-                }else{
-                    if((direction == 4) || (direction == 6) || (direction == 8) || (direction == 10)|| (direction == 12)){
-                        if((IR_found) && (!xbee)){
-                            state = DIFFENCE;
-                        }       
-                    }else if((diff > 15) && (!xbee) && (IR_found)){
-                         state = DIFFENCE;
-                    }else{
-                        state = HOME_WAIT;
-                    }
-                }
+            state = 0;
+        }
+           
+        if(((direction == 0)||(direction == 1)||(direction == 15))){
+            state_off = ATTACK;
+        }
+        if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 90)){
+            if((past_state_off != SNAKE)){
+                accelera_Distance = Distance;    
             }
+            state_off = SNAKE;
         }
-        /**/
-        /*********************************************************************************************************
-        **********************************************************************************************************
-        ********************Change state *************************************************************************
-        **********************************************************************************************************
-        *********************************************************************************************************/
+        if(Distance >= 120){
+            state_off = SEARCH;
+        } 
+        
+        past_state_off = state_off;
         
-        //state = HOME_WAIT;
-        if(state == HOME_WAIT){
-            mbedleds = 1;
-            /*
-            if(((RIGHT_SONIC + LEFT_SONIC) < 1100.0) && ((RIGHT_SONIC + LEFT_SONIC) > 850.0)){
-                if((LEFT_SONIC > 425.0) && (RIGHT_SONIC > 425.0)){
-                    vx = 0;
-                }else if(RIGHT_SONIC < 425.0){
-                    vx = (int)((RIGHT_SONIC - 425.0) * 0.02 - 10.0);
-                    if(vx < -15)vx = -15;
-                }else if(LEFT_SONIC < 425.0){
-                    vx = (int)((425.0 - LEFT_SONIC ) * 0.02 + 10.0);
-                    if(vx > 15)vx = 15;
-                }
-                
-                if((RIGHT_SONIC < 330.0) || (LEFT_SONIC < 330.0)){
-                    if((BACK_SONIC > 15.0) && (BACK_SONIC < 45.0)){
-                        vy = 0;
-                    }else if((BACK_SONIC <= 15.0) || (BACK_SONIC == PING_ERROR)){
-                        vy = 5;
-                    }else{
-                        vy = (int)(0.015 * (30.0 - BACK_SONIC) - 4);
-                        if(vy < -10)vy = -10;
+        if(IR_found){
+            if(state_off == SNAKE){
+                if((Distance == 30)||(Distance == 90)){
+                    x_dista = 12*ball_sankaku[direction][0];
+                    y_dista = 12*ball_sankaku[direction][1];            
+                    
+                    x_turn = 18*(turn_sankaku[direction][0]);
+                    y_turn = 18*(turn_sankaku[direction][1]);
+                    
+                    if((direction == 2)||(direction == 14)){
+                        //x_turn *= 0.7;
+                        y_turn *= 0.7;
                     }
-                }else{
-                    if((BACK_SONIC > 95.0) && (BACK_SONIC < 125.0)){
+                    
+                    if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
+                        x_turn = 7*(turn_sankaku[direction][0]);
+                        y_turn = 7*(turn_sankaku[direction][1]);
+                        
+                        x_dista = 15*ball_sankaku[direction][0];
+                        y_dista = 10*ball_sankaku[direction][1];        
+                    
+                    }
+                    
+                    if(direction == 1){
+                        vx = 15;
                         vy = 0;
-                    }else if((BACK_SONIC <= 95.0) || (BACK_SONIC == PING_ERROR)){
-                        vy = 5;
-                    }else{
-                        vy = (int)(0.015 * (110.0 - BACK_SONIC) - 4);
-                        if(vy < -10)vy = -10;
                     }
-                }
-            }else if((RIGHT_SONIC + LEFT_SONIC) <= 850.0){
-                if(BACK_SONIC < 200.0){
-                    if(RIGHT_SONIC > LEFT_SONIC){
-                        vx = 15;
-                        vy = 5;
-                    }else{
+                    
+                    if(direction == 15){
                         vx = -15;
-                        vy = 5;
+                        vy = 0;
                     }
-                }else{
-                    vx = 0;
-                    vy = -10;
-                }
-            }else{
-                if(BACK_SONIC > 500.0){
-                    if(RIGHT_SONIC > LEFT_SONIC){
-                        vx = 10;
+                    
+                    if(direction == 2){
+                        vx = 20;
                         vy = -10;
-                    }else{                    
-                        vx = -10;
+                    }
+                    
+                    if(direction == 14){
+                        vx = -20;
                         vy = -10;
                     }
+                    
                 }
-            }
-            */
-        }else if(state == DIFFENCE){
-            mbedleds = 4;
-            if((direction == 6) || (direction == 4)){
-                vx = 20;
-            
-                if(LEFT_SONIC < 500){
-                    if((BACK_SONIC > 370) && (BACK_SONIC < 400)){
-                        vy = 0;
-                    }else if((BACK_SONIC <= 370) || (BACK_SONIC == PING_ERROR)){
-                        vy = 5;
-                    }else{
-                        vy = (int)(0.015 * (400.0 - BACK_SONIC) - 4);
-                        if(vy < -15)vy = -15;
+                
+                if(Distance == 10){
+                    x_dista = 8*(-ball_sankaku[direction][0]);
+                    y_dista = 8*(-ball_sankaku[direction][1]);            
+                
+                    x_turn = 22*(turn_sankaku[direction][0]);
+                    y_turn = 22*(turn_sankaku[direction][1]);
+                    
+                    
+                    if((direction == 2)||(direction == 14)){
+                        y_turn *= 0.7;
                     }
-                }else if(RIGHT_SONIC < 500){
-                    vx = 15;
-                    vy = -10;
-                }else{
-                    if((BACK_SONIC > 70) && (BACK_SONIC < 100)){
-                        vy = 0;
-                    }else if((BACK_SONIC <= 70) || (BACK_SONIC == PING_ERROR)){
-                        vy = 5;
-                    }else{
-                        vy = (int)(0.015 * (100.0 - BACK_SONIC) - 4);
-                        if(vy < -15)vy = -15;
+                    
+                    if(direction == 2){
+                        vx = 20;
+                        vy = -15;  
                     }
+                    if(direction == 14){
+                        vx = -20;
+                        vy = -15;
+                    }         
                 }
-            }else if((direction == 10) || (direction == 12)){
-                 vx = -20;
-            
-                if(RIGHT_SONIC < 500){
-                    if((BACK_SONIC > 370) && (BACK_SONIC < 400)){
-                        vy = 0;
-                    }else if((BACK_SONIC <= 370) || (BACK_SONIC == PING_ERROR)){
-                        vy = 5;
-                    }else{
-                        vy = (int)(0.015 * (400.0 - BACK_SONIC) - 4);
-                        if(vy < -15)vy = -15;
-                    }
-                }else if(LEFT_SONIC < 500){
-                    vx = -15;
-                    vy = -10;
-                }else{
-                    if((BACK_SONIC > 70) && (BACK_SONIC < 100)){
-                        vy = 0;
-                    }else if((BACK_SONIC <= 70) || (BACK_SONIC == PING_ERROR)){
-                        vy = 5;
-                    }else{
-                        vy = (int)(0.015 * (100.0 - BACK_SONIC) - 4);
-                        if(vy < -15)vy = -15;
-                    }
+                
+                if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
+                    x_dista = 0;
+                    y_dista = 0;    
                 }
                 
-            }else if(direction == 8){
-            
-                if(LEFT_SONIC > RIGHT_SONIC){
-                    vx = -20;
-                }else{
+                if(direction == 2){
                     vx = 20;
+                    vy = -15;  
                 }
-                if((RIGHT_SONIC < 500) || (LEFT_SONIC < 500)){
-                    if(BACK_SONIC < 500){
-                        vy = -4;
-                    }else{
-                        vy = (int)(0.015 * (500.0 - BACK_SONIC) - 4);
-                        if(vy < -15)vy = -15;
-                    }
-                }else{
-                    if(BACK_SONIC < 200){
-                        vy = -4;
-                    }else{
-                        vy = (int)(0.015 * (200.0 - BACK_SONIC) - 4);
-                        if(vy < -15)vy = -15;
-                    }
+                if(direction == 14){
+                    vx = -20;
+                    vy = -15;
+                }         
+                    
+                vx = x_turn + x_dista;
+                vy = y_turn + y_dista;
+                
+                /*
+                if((accelera_Distance == 90)){
+                    if(Distance == 10){
+                        vx *= 0.3;
+                        vy *= 0.3;
+                    }    
+                    
+                    if(Distance == 30){
+                        vx *= 0.4;
+                        vy *= 0.4;
+                    }  
+                }*/
+                /*
+                if((accelera_Distance == 10)){
+                    if((direction == 4)||(direction == 12)){
+                        vx = 0;
+                        vy = -10;
+                    }   
+                }*/
+                
+            }else if(state_off == ATTACK){
+                if(direction == 0){
+                    vx = 10*ball_sankaku[direction][0];
+                    vy =20*ball_sankaku[direction][1];
+                    /*
+                    if(ultrasonicVal[1] < 380){
+                        vy = 10;
+                        vx = -20;
+                    }  
+                    
+                    if(ultrasonicVal[3] < 380){
+                        vy = 10;
+                        vx = 20;
+                    } */    
+                }
+                if(direction == 1){
+                    vx = 15*1.3;
+                    vy = 20*1.3;
+                }
+                if(direction == 15){
+                    vx = -15*1.3;
+                    vy = 20*1.3;  
+                }
+                if(direction == 2){
+                    vx = 25;
+                    vy = 0;
+                }
+                if(direction == 14){
+                    vx = -25;
+                    vy = 0;
                 }
                 
-            }else if(direction == 2){
-                
-                vx = 25;
-                vy = 0;     //0
+                if(Distance > 30){
+                    if(direction == 2){
+                        vx = 20;
+                        vy = 10;
+                    }
+                    if(direction == 14){
+                        vx = -20;
+                        vy = 10;
+                    }    
+                }
                 
-            }else if(direction == 14){
-                
-                vx = -25;
-                vy = 0;    //-4 
+            }else if(state_off == SEARCH){
+                vx = 24*ball_sankaku[direction][0];
+                vy = 24*ball_sankaku[direction][1];
                 
-            }else if(direction == 1){
+                if(direction == 2){
+                    vx = 20;  
+                }
+                if(direction == 14){
+                    vx = -20;
+                }          
+                if(direction == 0){
+                    vx = 20*ball_sankaku[direction][0];
+                    vy = 15*ball_sankaku[direction][1];  
+                }
+                if(direction == 1){
+                    vx = 20*ball_sankaku[direction][0];
+                    vy = 10*ball_sankaku[direction][1];  
+                }
+                if(direction == 15){
+                    vx = 20*ball_sankaku[direction][0];
+                    vy = 10*ball_sankaku[direction][1];  
+                }
+                if(direction == 4){
+                    vx = 0;
+                    vy = -15;
+                }
+                if(direction == 12){
+                    vx = 0;
+                    vy = -15;
+                }                         
+            }
             
-                
+            if(direction == 2){
                 vx = 20;
-                vy = 0;     //0
-                
-                
-            }else if(direction == 15){
-            
+                vy = 0;
+                  
+            }
+            if(direction == 14){
                 vx = -20;
-                vy = 0;    //-3
-               
-            }else if(direction == 0){
-            
-                vx = 0;
-                vy = 20;
-                
-            }else{//error
-            
-                vx = 0;
                 vy = 0;
-            
-            }
-        }else if(state == HOLD){
-            mbedleds = 15;
-            
-            vy = 20;
-            
-            if(((RIGHT_SONIC + LEFT_SONIC) < 1800.0) && ((RIGHT_SONIC + LEFT_SONIC) > 1400.0)){
-                standTu = (RIGHT_SONIC - LEFT_SONIC) / 25.0;
-            }
+            } 
+             
+        }else{
+            vx = 0;
+            vy = 0;
         }
         
-        if(lineState == NORMAL){
-            //mbedleds = 1;
-           
-        }else if(lineState == LEFT_OUT){
-            //mbedleds = 2;
-            
-            vx = 30;
-        }else if(lineState == RIGHT_OUT){
-            //mbedleds = 4;
-            
-            vx = -30;
-        }else if(lineState == FRONT_OUT){
-            //mbedleds = 8;
-            
-            vy = -40;
-        }else if(lineState == BACK_OUT){
-            //mbedleds = 12;
-            
-            vy = 40;
+        vx *= 1.3* 0.8;
+        vy *= 0.7 * 0.8;
+        
+
+         
+        if(lineStateX == LEFT_OUT){    
+            vx += 20;
+        }else if(lineStateX == RIGHT_OUT){
+            vx -= 20;
         }
-        //vx = vector(10,45,X);
-        //vy = vector(10,45,Y);
-        //vx = 40;
-        //vy = 0;
-        //pc.printf("%d\t%d\n",vx,vy);
         
-        //vy = -3;
-        //vs = 0;
-        //vx = 0; 
-        //vx = 10;
-        //vx = 25;
-        //vy = 0;
+        if(lineStateY == FRONT_OUT){
+            vy -= 15;
+        }else if(lineStateY == BACK_OUT){
+            vy += 15;
+        } 
+        
+        //vx *= 0.8;
+        //vy *= 0.8;
+        
+        direction_past = direction;
+    
         
         move(vx,vy,vs);
     }
-}
\ No newline at end of file
+}
--- a/main.h	Wed Jun 19 08:42:55 2013 +0000
+++ b/main.h	Fri Jan 24 06:27:03 2014 +0000
@@ -29,9 +29,9 @@
 
 #define PID_CYCLE   0.06    //s
 
-#define P_GAIN  0.75    //0.78   
+#define P_GAIN  0.65    //0.78   
 #define I_GAIN  0.0     //0.0
-#define D_GAIN  0.006   //0.009
+#define D_GAIN  0.009   //0.009
     
 #define OUT_LIMIT   30.0
 #define MAX_POW     100
@@ -53,14 +53,20 @@
 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);      //battery is low power version 30.0 0.58 1.0 0.015    battery is high power version   30.0 0.42, 1.0, 0.013   power is low but perfect 20.0 0.45, 0.0, 0.010
 Ticker pidUpdata;
 Ticker irDistanceUpdata;
+Ticker underIR;
 Timer timer1;
 Timer Survtimer;
+Timer lasttimer;
 LocalFileSystem local("local");  // マウントポイントを定義(ディレクトリパスになる)
 
 enum{
-    NORMAL,
+    XNORMAL,
     LEFT_OUT,
     RIGHT_OUT,
+};
+
+enum{
+    YNORMAL,
     FRONT_OUT,
     BACK_OUT,
 };
@@ -72,6 +78,13 @@
     HOLD,
 };
 
+enum{
+    NONE,
+    ATTACK,
+    SNAKE,
+    SEARCH
+};
+
 PinName adc_num[6] = {
     p15,
     p16,
@@ -80,16 +93,25 @@
     p19,
     p20,
 };
+
+int under_val[6] = {0};
+
+int whiteout_flag = 0;
+
 double standTu = 0;
 int speed[MOT_NUM] = {0};
 uint8_t hold_flag = 0;
 uint8_t state = HOME_WAIT;
-uint8_t lineState = NORMAL;
+uint8_t lineStateY = YNORMAL;
+uint8_t lineStateX = XNORMAL;
 
 double inputPID = 180.0;
 static double standard;
 double compassPID = 0.0;
 
+int whiteout;
+int whitevalue;
+
 extern int diff;
 
 extern string StringFIN;
@@ -116,3 +138,44 @@
 #define LEFT_SONIC  ultrasonicVal[3]    
 
 
+double ball_sankaku[16][2] = {
+    {0     , 1    },
+    {0.390 , 0.920},
+    {0.866 , 0.500},
+    {0.927 , 0.374},
+    {1     , 0    },
+    {0.920 ,-0.390},
+    {0.707 ,-0.707},
+    {0.374 ,-0.927},
+    {0     ,-1    },
+    {-0.390,-0.920},
+    {-0.707,-0.707},
+    {-0.927,-0.374},
+    {-1    , 0    },
+    {-0.920, 0.390},
+    {-0.866, 0.500},
+    {-0.374, 0.927}
+};
+
+double turn_sankaku[16][2] = {
+    { 0    ,0      },
+    /*{ 1    ,0      },*/{ 0.920,-0.390 },
+    { 0.707,-0.707 }, //{ 0.500,-0.866 },
+    { 0.374,-0.927 },
+    { 0    ,-0.8   },
+    {-0.390,-0.920 },
+    {-0.707,-0.707 },
+    {-0.927,-0.374 },
+    {-0.927,-0.374 },
+    {0.920 ,-0.390 },
+    {0.707 ,-0.707 },
+    {0.374 ,-0.927 },
+    {0     ,-0.8   },
+    {-0.390,-0.920 },
+    {-0.707,-0.707 }, //{-0.500,-0.866 },
+    /*{-1    ,0      }*/{-0.927,-0.374 }
+};
+
+
+
+