Diff: LF2.h

Revision:

3:04f6fe153dbc

Parent:

1:f9c953ddc87a

Child:

4:795055e031c3

--- a/LF2.h	Sun Feb 28 15:43:11 2016 +0000
+++ b/LF2.h	Mon Feb 29 15:10:24 2016 +0000
@@ -2,10 +2,10 @@
 
 /*設定するもの*/
 #define MAX 3000  //白よりも少し大きい値
-#define MIN 1100  //暗い色よりも少し小さい値
-#define naka 1450 //サーボの真ん中
-float kp=0.2;     //P係数
-float kdir=0;     //傾きにかける係数 Dのような役割をする
+#define MIN 1500  //暗い色よりも少し小さい値
+#define naka 1525 //サーボの真ん中
+float kp=0.05;//0.2;     //P係数
+float kdir=0.025;     //傾きにかける係数 Dのような役割をする
 
 SPI mcp(p5, p6, p7); // mosi, miso, sclk
 DigitalOut mcs(p8);
@@ -34,10 +34,11 @@
 
 int zure1,zure2,zure3,zure4,zure5;
 
-float sa;  //CdSの両隣の値の差
+float sa; 
 int now;
 int bef; 
 int dir; 
+int Dir;
 int out;
 int i;
 int mcp_i;
@@ -93,8 +94,13 @@
         led3=1;
         led4=1;
         
-  while(1){if(switch_1==1||switch_2==1||switch_3==1)break;}     //白に移動させた後スイッチを入れる
-     // wait(3);
+        wait(3);
+        
+        led1=1;
+        led2=0;
+        led3=0;
+        led4=1;
+        
     
     while(1){       //白色で合わせる
         PIN();
@@ -140,14 +146,14 @@
         e10=c10+d10;
         e11=c11+d11;
         e12=c12+d12;
+        
+        MAX_MIN=MAX-MIN;
     
+    while(1){if(switch_1==1||switch_2==1||switch_3==1)break;}     //白に移動させた後スイッチを入れる
         led1=0;
         led2=0;
         led3=0;
         led4=0;
-        
-        MAX_MIN=MAX-MIN;
-    
     }
 
 void SA(void){
@@ -253,105 +259,108 @@
    hosei();
    if(i==0){                
      Find_White();
-     dir+=now;  //加算式にする。
-     if(dir>1000)dir=1000;
-     else if(dir<-1000)dir=-1000;
-    // if(dir>3)i=0;  //外乱対策
      }
      
    if(i==1){
+      dir+=bef;  //加算式にする。
+      if(dir>1000)dir=1000;
+      else if(dir<-1000)dir=-1000;
+      Dir=dir*kdir;
+     if(Dir>50)Dir=50;
+     else if(Dir<-50)Dir=-50;
+     
      switch(now){
-      case -6:        
+      case -6:       
        sa=zure5+(s1-s2)*kp;
        SA();
-       out=naka-dir*kdir-sa;
+       out=naka-Dir-sa;
        bef=now; i=0;
        break;
        
        case -5:
        if(s1>s3)sa=zure5+(s1-s2)*kp;
-       else sa=zure4+(s2-s3)*kp; 
+       else {sa=zure4+(s2-s3)*kp; if(sa>zure5)sa=zure5;}
        SA();
-       out=naka-dir*kdir-sa;
+       out=naka-Dir-sa;
        bef=now; i=0;
        break;
        
        case -4: 
-       if(s2>s4)sa=zure4+(s2-s3)*kp;
-       else sa=zure3+(s3-s4)*kp;
+       if(s2>s4){sa=zure4+(s2-s3)*kp; if(sa>zure5)sa=zure5;}
+       else {sa=zure3+(s3-s4)*kp; if(sa>zure4)sa=zure4;}
        SA();
-       out=naka-dir*kdir-sa;
+       out=naka-Dir-sa;
        bef=now; i=0;
        break;
        
        case -3: 
-       if(s3>s5)sa=zure3+(s3-s4)*kp;
-       else sa=zure2+(s4-s5)*kp;
+       if(s3>s5){sa=zure3+(s3-s4)*kp; if(sa>zure4)sa=zure4;}
+       else {sa=zure2+(s4-s5)*kp; if(sa>zure3)sa=zure3;}
        SA();
-       out=naka-dir*kdir-sa;
+       out=naka-Dir-sa;
        bef=now; i=0;
        break;
        
        case -2: 
-       if(s4>s6)sa=zure2+(s4-s5)*kp;
-       else sa=zure1+(s5-s6)*kp;
+       if(s4>s6){sa=zure2+(s4-s5)*kp; if(sa>zure3)sa=zure3;}
+       else {sa=zure1+(s5-s6)*kp; if(sa>zure2)sa=zure2;}
        SA();
-       out=naka-dir*kdir-sa;
+       out=naka-Dir-sa;
        bef=now; i=0;
        break;
        
        case -1: 
-       if(s5>s7)sa=zure1+(s5-s6)*kp;
-       else sa=(s6-s7)*kp;
+       if(s5>s7){sa=zure1+(s5-s6)*kp; if(sa>zure2)sa=zure2;}
+       else {sa=(s6-s7)*kp; if(sa>zure1)sa=zure1;}
        SA();
-       out=naka-dir*kdir-sa;
+       out=naka-Dir-sa;
        bef=now; i=0;
        break;
        
        case 1: 
-       if(s6>s8)sa=(s7-s6)*kp;
-       else sa=zure1+(s8-s7)*kp;
+       if(s6>s8){sa=(s7-s6)*kp; if(sa>zure1)sa=zure1;}
+       else {sa=zure1+(s8-s7)*kp; if(sa>zure2)sa=zure2;}
        SA();
-       out=naka+dir*kdir+sa;
+       out=naka+Dir+sa;
        bef=now; i=0;
        break;
        
        case 2: 
-       if(s7>s9)sa=zure1+(s8-s7)*kp;
-       else sa=zure2+(s9-s8)*kp;
+       if(s7>s9){sa=zure1+(s8-s7)*kp; if(sa>zure2)sa=zure2;}
+       else {sa=zure2+(s9-s8)*kp; if(sa>zure3)sa=zure3;}
        SA();
-       out=naka+dir*kdir+sa;
+       out=naka+Dir+sa;
        bef=now; i=0;
        break;
        
        case 3: 
-       if(s8>s10)sa=zure2+(s9-s8)*kp;
-       else sa=zure3+(s10-s9)*kp;
+       if(s8>s10){sa=zure2+(s9-s8)*kp; if(sa>zure3)sa=zure3;}
+       else {sa=zure3+(s10-s9)*kp; if(sa>zure4)sa=zure4;}
        SA();
-       out=naka+dir*kdir+sa;
+       out=naka+Dir+sa;
        bef=now; i=0;
        break;
        
        case 4: 
-       if(s9>s11)sa=zure3+(s10-s9)*kp;
-       else sa=zure4+(s11-s10)*kp;
+       if(s9>s11){sa=zure3+(s10-s9)*kp; if(sa>zure4)sa=zure4;}
+       else {sa=zure4+(s11-s10)*kp; if(sa>zure5)sa=zure5;}
        SA();
-       out=naka+dir*kdir+sa;
+       out=naka+Dir+sa;
        bef=now; i=0;
        break;
        
        case 5: 
-       if(s10>s12)sa=zure4+(s11-s10)*kp;
+       if(s10>s12){sa=zure4+(s11-s10)*kp; if(sa>zure5)sa=zure5;}
        else sa=zure5+(s12-s11)*kp;
        SA();
-       out=naka+dir*kdir+sa;
+       out=naka+Dir+sa;
        bef=now; i=0;
        break;
        
        case 6: 
        sa=zure5+(s12-s11)*kp;
        SA();
-       out=naka+dir*kdir+sa;
+       out=naka+Dir+sa;
        bef=now; i=0;
        break;