Revision 7:3a7e49ed1162, committed 2018-09-15

Comitter:

aoikoizumi

Date:

Sat Sep 15 07:54:13 2018 +0000

Parent:

6:7ec6c3d3a30a

Commit message:

9/15 ???????
; ???????
; ????

Changed in this revision

--- a/main.cpp	Thu Sep 13 04:17:15 2018 +0000
+++ b/main.cpp	Sat Sep 15 07:54:13 2018 +0000
@@ -4,10 +4,11 @@
 #include "EC.h"
 #include "R1370P.h"
 #include "enc_1multi.h"
-#define BASIC_SPEED 15  //モーターはこの角速度で駆動させる
+#define BASIC_SPEED 24  //モーターはこの角速度で駆動させる
 
 SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1);  //right  enc migi ue
 SpeedControl motorL(PB_5,PB_3,NC,500,0.05,PA_5,PA_7);    //left   enc hidari sita  //ok
+
 Ec EC1(PB_4,PA_8,NC,300,0.05);    //center enc
 Ticker motor_tick;  //角速度計算用ticker
 Ticker ticker;//for enc
@@ -23,11 +24,11 @@
     EC1.CalOmega();
 }
 
-//DigitalIn button(USER_BUTTON);
+DigitalIn button(USER_BUTTON,PullUp);
 DigitalIn reset_f(PC_1,PullUp);
 DigitalIn reset_a(PA_4,PullUp);
 
-PwmOut servo(PB_14);//servo
+PwmOut servo(PB_7);//servo
 PwmOut motor_f(PC_9);
 PwmOut motor_b(PB_9);//arm
 DigitalOut denjiben(PC_0);//dennjibenn
@@ -38,14 +39,14 @@
 double d_dist=0;
 double x;
 double y;
-double asari_x=900;
-double asari_y=2500;//asariwo toru tekisetsuna zahyouwo kaeraremasu
-double goal_x=1120;
-double goal_y=1700;
+double asari_x=810;
+double asari_y=2674;//asariwo toru tekisetsuna zahyouwo kaeraremasu
+double goal_x=1020;
+double goal_y=1562;
 double start_x=185;
-double start_y=150;
+double start_y=300;
 Timer t;
-int i=1;
+int i=0;
 
 int kai=0;//printf kansuu
 double target_R=0,target_L=0;
@@ -65,40 +66,40 @@
 void susumu_y(double ay,double by,double goaly)//y zahyou wo motiita susumikata subete
 {
 //pc.printf("ay=%f by=%f y=%f goaly=%f\r\n",ay,by,y,goaly);
-    if(y<goaly-50&&ay>=0) {
+    if(y<goaly-100&&ay>=0) {
         t.start();
-        pc.printf("R=%f L=%f\r\n",target_R,target_L);
-        if(t.read_ms()<100) {
+        //pc.printf("R=%f L=%f\r\n",target_R,target_L);
+        if(t.read_ms()<150) {
             pc.printf("t=%f",t.read());
             tgt(ay/3,by/3);
-        } else if(t.read_ms()>=100&&t.read_ms()<200) {
+        } else if(t.read_ms()>=150&&t.read_ms()<300) {
             pc.printf("t=%f",t.read());
             tgt(ay*2/3,by*2/3);
         } else {
             tgt(ay,by);
             t.stop();
-            pc.printf("R=%f L=%f",target_R,target_L);
+            //pc.printf("R=%f L=%f",target_R,target_L);
         }
-    } else if(y<goaly&&y>=goaly-50&&ay>=0) {
-        tgt(ay/3,by/3);
+    } else if(y<goaly&&y>=goaly-100&&ay>=0) {
+        tgt(ay/2,by/2);
 
 
-    } else if(y>goaly+50&&ay<0) {
+    } else if(y>goaly+100&&ay<0) {
         t.start();
-        pc.printf("R=%f L=%f\r\n",target_R,target_L);
-        if(t.read_ms()<100) {
+        //pc.printf("R=%f L=%f\r\n",target_R,target_L);
+        if(t.read_ms()<150) {
             pc.printf("t=%f",t.read());
             tgt(ay/3,by/3);
-        } else if(t.read_ms()>=100&&t.read_ms()<200) {
+        } else if(t.read_ms()>=150&&t.read_ms()<300) {
             pc.printf("t=%f",t.read());
             tgt(ay*2/3,by*2/3);
         } else {
             tgt(ay,by);
             t.stop();
-            pc.printf("R=%f L=%f",target_R,target_L);
+            //pc.printf("R=%f L=%f",target_R,target_L);
         }
-    } else if(y>goaly&&y<=goaly+50&&ay<0) {
-        tgt(ay/3,by/3);
+    } else if(y>goaly&&y<=goaly+100&&ay<0) {
+        tgt(ay/2,by/2);
     } else {
         i++;
         t.reset();
@@ -111,22 +112,22 @@
 void susumu_xl(double axl,double bxl,double goalxl)//hidarikara mokuhyoutenn ni toutatsu surutameno dousa
 {
 
-    if(x<goalxl-50) {
+    if(x<goalxl-100) {
         t.start();
-        pc.printf("R=%f L=%f\r\n",target_R,target_L);
-        if(t.read_ms()<100) {
+        //pc.printf("R=%f L=%f\r\n",target_R,target_L);
+        if(t.read_ms()<150) {
             pc.printf("t=%f",t.read());
             tgt(axl/3,bxl/3);
-        } else if(t.read_ms()>=100&&t.read_ms()<200) {
+        } else if(t.read_ms()>=150&&t.read_ms()<300) {
             pc.printf("t=%f",t.read());
             tgt(axl*2/3,bxl*2/3);
         } else {
             tgt(axl,bxl);
             t.stop();
-            pc.printf("R=%f L=%f",target_R,target_L);
+            ////pc.printf("R=%f L=%f",target_R,target_L);
         }
-    } else if(x<goalxl&&x>=goalxl-50) {
-        tgt(axl/3,bxl/3);
+    } else if(x<goalxl&&x>=goalxl-100) {
+        tgt(axl/2,bxl/2);
 
     } else {
         t.reset();
@@ -138,22 +139,22 @@
 void susumu_xr(double axr,double bxr,double goalxr)//migikara mokuhyoutenn ni toutatsu surutameno dousa
 {
 
-    if(x>goalxr+50) {
+    if(x>goalxr+100) {
         t.start();
-        pc.printf("R=%f L=%f\r\n",target_R,target_L);
-        if(t.read_ms()<100) {
+        //pc.printf("R=%f L=%f\r\n",target_R,target_L);
+        if(t.read_ms()<150) {
             pc.printf("t=%f",t.read());
             tgt(axr/3,bxr/3);
-        } else if(t.read_ms()>=100&&t.read_ms()<200) {
+        } else if(t.read_ms()>=150&&t.read_ms()<300) {
             pc.printf("t=%f",t.read());
             tgt(axr*2/3,bxr*2/3);
         } else {
             tgt(axr,bxr);
             t.stop();
-            pc.printf("R=%f L=%f",target_R,target_L);
+            ////pc.printf("R=%f L=%f",target_R,target_L);
         }
-    } else if(x>goalxr&&x<=goalxr+50) {
-        tgt(axr/3,bxr/3);
+    } else if(x>goalxr&&x<=goalxr+100) {
+        tgt(axr/2,bxr/2);
 
     } else {
         t.reset();
@@ -164,9 +165,9 @@
 
 void susumu_ang(double a,double b,double goal)//kakudo
 {
-    if(goal-5>angle&&a<b) {//usetsu
+    if(goal-30>angle&&a<b) {//usetsu
         t.start();
-        pc.printf("R=%f L=%f\r\n",target_R,target_L);
+        //pc.printf("R=%f L=%f\r\n",target_R,target_L);
         if(t.read_ms()<100) {
             pc.printf("t=%f",t.read());
             tgt(a/3,b/3);
@@ -176,14 +177,14 @@
         } else {
             tgt(a,b);
             t.stop();
-            pc.printf("R=%f L=%f",target_R,target_L);
+            ////pc.printf("R=%f L=%f",target_R,target_L);
         }
-    } else if(angle<goal&&angle>=goal-5&&a<b) {
-        tgt(a/3,b/3);
+    } else if(angle<goal&&angle>=goal-30&&a<b) {
+        tgt(a/2,b/2);
 
-    } else if(angle>goal+5&&a>b) { //sasetsu
+    } else if(angle>goal+30&&a>b) { //sasetsu
         t.start();
-        pc.printf("R=%f L=%f\r\n",target_R,target_L);
+        ////pc.printf("R=%f L=%f\r\n",target_R,target_L);
         if(t.read_ms()<100) {
             pc.printf("t=%f",t.read());
             tgt(a/3,b/3);
@@ -193,10 +194,10 @@
         } else {
             tgt(a,b);
             t.stop();
-            pc.printf("R=%f L=%f",target_R,target_L);
+            ////pc.printf("R=%f L=%f",target_R,target_L);
         }
-    } else if(angle>goal&&angle<=goal+5&&a>b) {
-        tgt(a/3,b/3);
+    } else if(angle>goal&&angle<=goal+30&&a>b) {
+        tgt(a/2,b/2);
 
     } else {
         i++;
@@ -211,7 +212,7 @@
 
 int main(void)
 {
-    
+
     gyro.initialize();    //main関数の最初に一度だけ実行
     gyro.acc_offset();    //やってもやらなくてもいい
 
@@ -220,52 +221,52 @@
     motorR.setDOconstant(34.1);
     motorL.setDOconstant(30);//c
     motorR.setPDparam(0,0);
-    motorR.setPDparam(0,0);//pd//akirameta
+    motorL.setPDparam(0,0);//pd//akirameta
 
 
-    EC1.setDiameter_mm(50);//sokuteirinnhannkei
+    EC1.setDiameter_mm(48);//sokuteirinnhannkei
     double  getDistance_mm();
     //int EC1.getCount()=0;
     void reset  ();
     EC1.reset();
 
 
-    servo.period_ms(20);
+    //servo.period_ms(30);
 
     motor_f.period_ms(30);
     motor_b.period_ms(30);//arm
     x=start_x;
     y=start_y;//start point//keisann wo sinaosu hitsuyouga arimasu
-
+    //servo.pulsewidth_us(950);
     while(1) {
+        //target_R=(-1)*BASIC_SPEED;
+        //target_L=(-1)*BASIC_SPEED;
         // motorR.turnF(0.3);
         //motorL.turnF(0.3);//for debug
-        motorR.Sc(target_R);
-        motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
+        //motorR.Sc(target_R);
+        //motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
         angle=gyro.getAngle()*(-1);    //角度の値を受け取る
 
         EC1.getDistance_mm();
         // EC1.CalOmega();
 
-
         if(target_R==0) motorR.stop();
         else motorR.Sc(target_R);
         if(target_L==0) motorL.stop();
         else motorL.Sc(target_L);
-        
         new_dist=EC1.getDistance_mm();
-            d_dist=new_dist-old_dist;
-            old_dist=new_dist;
+        d_dist=new_dist-old_dist;
+        old_dist=new_dist;
 
-        double d_x=d_dist*sin(angle);
-        double d_y=d_dist*cos(angle);
+        double d_x=d_dist*sin(angle*3.1415926535/180);
+        double d_y=d_dist*cos(angle*3.1415926535/180);
         x=x+d_x;
         y=y+d_y;
 
 
 
         if(kai>=3) {
-            
+
 
             //double d_x=d_dist*sin(angle);;
             //double d_y=d_dist*cos(angle);;
@@ -273,7 +274,7 @@
             //y=y+d_y;
 
             pc.printf("R=%f L=%f",target_R,target_L);
-            // pc.printf("omg_R=%f omg_L=%f  \r\n",motorR.getOmega(),motorL.getOmega());
+            pc.printf("omg_R=%f omg_L=%f  \r\n",motorR.getOmega(),motorL.getOmega());
             pc.printf("i=%d\r\n",i);
             //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
             pc.printf("x=%f y=%f",x,y);
@@ -283,164 +284,182 @@
         }
         kai++;
         if(i==0) {
-            /*if(reset_f.read()==1) {
+            if(reset_f.read()==0&&button.read()==0) {
+                wait(0.05);
+                if(reset_f.read()==0&&button.read()==0) {
+                    denjiben=1;
+                    i++;
+                }
+            }
+            if(reset_a.read()==0&&button.read()==0) {
                 wait(0.05);
-                if(reset_f.read()==1) {
-              */      denjiben=0;
-            i++;
-            //     }
-        //}
-        if(reset_a.read()==1) {
-            wait(0.05);
-            if(reset_a.read()==1) {
-                denjiben=0;
-                x=70;
-                y=2500;
-                i=14;
+                if(reset_a.read()==0&&button.read()==0) {
+                    denjiben=1;
+                    
+                    x=180;
+                    y=2674;
+
+                    i=6;
+                }
+            }//x,y
+        }
+        if(i==1) {
+            susumu_y(1,1,857);//x,x+178
+        }
+        if(i==2) {
+            susumu_ang(0,0.7,90);//x+155,x+471.5
+        }
+        if(i==3) {
+            susumu_xl(0.7,0.7,745);//728.5,1078.5
+        }
+        if(i==4) {
+            susumu_ang(0.7,0,0);//850,1372
+        }
+        if(i==5) {
+            susumu_y(0.7,0.7,2016);//850,1700
+        }
+        if(i==6) {
+            // motorR.stop();
+            //motorL.stop();
+            tgt(0,0);
+            wait(2);
+            t.start();
+            if(t<1) {
+                motor_f=0.82;
+                motor_b=0;
+
+                printf("motor");
+            } else {
+                motor_f=0;
+                motor_b=0;
+                printf("finish");
+                t.stop();
+                t.reset();
+                i++;
             }
-        }//x,y
-    }
-    if(i==1) {
-        susumu_y(1,1,start_x+178);//x,x+178
-    }
-    if(i==2) {
-        susumu_ang(0.5,1,45);//x+121.5,x+471.5
-    }
-    if(i==3) {
-        susumu_xl(1,1,728.5);//728.5,1078.5
-    }
-    if(i==4) {
-        susumu_ang(1,0.5,0);//850,1372
-    }
-    if(i==5) {
-        susumu_y(1,1,1700);//850,1700
-    }
-    if(i==6) {
-        motorR.stop();
-        motorL.stop();
-        t.start();
-        if(t<1) {
-            motor_f=0.82;
-            motor_b=0;
-        } else {
-            motor_f=0;
-            motor_b=0;
-            printf("finish");
-            t.reset();
+        }//gatiasari
+
+
+        if(i==7) {
+            wait(2);
+            t.start();
+            if(t<1) {
+                motor_f=0;
+                motor_b=0.82;
+
+                printf("motor");
+            } else {
+                motor_f=0;
+                motor_b=0;
+                printf("finish");
+                t.stop();
+                t.reset();
+                i++;
+            }
+            if(angle>=-89) {
+                target_R=BASIC_SPEED/2;
+                target_L=BASIC_SPEED/(-2);
+            }
+            if(angle<=-91) {
+                target_R=BASIC_SPEED/(-2);
+                target_L=BASIC_SPEED/2;
+            }
+            if(angle>-91&angle<-89) {
+                motorR.stop();
+                motorL.stop();
+                wait(2);
+                if(angle>-91&angle<-89) {
+                    i++;
+                }
+            }
+        }//kakudo tyousei
+        if(i==8) {
+            susumu_xr(1,1,555);//700,1700
+        }
+        if(i==9) {
+            susumu_ang(0,0.7,0);//390,2010
+        }
+        if(i==10) {
+            susumu_y(0.8,0.8,asari_y-155);//390,y-310
+        }
+        if(i==11) {
+            susumu_ang(0,1,90);//700,y
+        }
+        if(i==12) {
+            motorR.stop();
+            motorL.stop();
+            //servo.pulsewidth_us(2100);
+            wait(1.5);
+            //servo.pulsewidth_us(2400);
+            wait(2);
             i++;
         }
-    }//gatiasari
-
-
-    if(i==7) {
-        t.start();
-        if(t<1) {
-            motor_f=0;
-            motor_b=0.82;
-        } else {
-            motor_f=0;
-            motor_b=0;
-            printf("finish");
-            t.reset();
+        if(i==13) {
+            if(angle>=91) {
+                target_R=BASIC_SPEED/2;
+                target_L=BASIC_SPEED/(-2);
+            }
+            if(angle<=89) {
+                target_R=BASIC_SPEED/(-2);
+                target_L=BASIC_SPEED/2;
+            }
+            if(angle>89&angle<91) {
+                motorR.stop();
+                motorL.stop();
+                wait(2);
+                if(angle>89&angle<91) {
+                    i++;
+                }
+            }
+        }
+        if(i==14) {
+            susumu_xl(0.6,0.6,asari_x);//x,y
         }
-        if(angle>=-89) {
-            target_R=BASIC_SPEED/5;
-            target_L=BASIC_SPEED/(-5);
+        if(i==15) {
+            motorR.stop();
+            motorL.stop();
+            tgt(0,0);
+            wait(2);
+            wait(0.5);
+            denjiben=0;
+            wait(0.5);
+            //servo.pulsewidth_us(900);
+            wait(2);
+            i++;
         }
-        if(angle<=-91) {
-            target_R=BASIC_SPEED/(-5);
-            target_L=BASIC_SPEED/-5;
+        if(i==16) {
+            susumu_xr(-1,-1,555);//700,y
+        }
+        if(i==17) {
+            susumu_ang(0,-0.6,0);//390,y-310
         }
-        if(angle>-91&angle<-89) {
+        if(i==18) {
+            susumu_y(-1,-1,goal_y+155);//390,y+310
+        }
+        if(i==19) {
+            susumu_ang(0,-0.6,-90);//700,y
+        }
+        if(i==20) {
+            susumu_xl(-1,-1,goal_x);//x,y
+        }
+        if(i==21) {
             motorR.stop();
             motorL.stop();
             wait(0.5);
-            if(angle>-91&angle<-89) {
-                i++;
-            }
-        }
-    }//kakudo tyousei
-    if(i==8) {
-        susumu_xr(1,1,700);//700,1700
-    }
-    if(i==9) {
-        susumu_ang(1/3,1,0);//390,2010
-    }
-    if(i==10) {
-        susumu_y(1,1,asari_y-310);//390,y-310
-    }
-    if(i==11) {
-        susumu_ang(1/3,1,90);//700,y
-    }
-    if(i==12) {
-        motorR.stop();
-        motorL.stop();
-        servo.pulsewidth_us(2100);
-        wait(1.5);
-        servo.pulsewidth_us(2400);
-        wait(1);
-        i++;
-    }
-    if(i==13) {
-        if(angle>=91) {
-            target_R=BASIC_SPEED/5;
-            target_L=BASIC_SPEED/(-5);
-        }
-        if(angle<=89) {
-            target_R=BASIC_SPEED/(-5);
-            target_L=BASIC_SPEED/5;
+            denjiben=1;
+            wait(0.5);
+
+            break;
         }
-        if(angle>89&angle<91) {
-            motorR.stop();
-            motorL.stop();
-            wait(0.1);
-            if(angle>89&angle<91) {
-                i++;
-            }
+        if(i==30) {
+            tgt(-1,-1);
         }
-    }
-    if(i==14) {
-        susumu_xl(1,1,asari_x);//x,y
-    }
-    if(i==15) {
-        motorR.stop();
-        motorL.stop();
-        wait(0.5);
-        denjiben=1;
-        wait(0.5);
-        servo.pulsewidth_us(900);
-        wait(2);
-        i++;
-    }
-    if(i==16) {
-        susumu_xr(-1,-1,700);//700,y
-    }
-    if(i==17) {
-        susumu_ang(-1/3,-1,0);//390,y-310
-    }
-    if(i==18) {
-        susumu_y(-1,-1,goal_y+310);//390,y+310
-    }
-    if(i==19) {
-        susumu_ang(-1/3,-1,-90);//700,y
-    }
-    if(i==20) {
-        susumu_xl(-1,-1,goal_x);//x,y
-    }
-    if(i==21) {
-        motorR.stop();
-        motorL.stop();
-        wait(0.5);
-        denjiben=0;
-        wait(0.5);
-
-        break;
-    }
 
 
-}//while
-tgt(0,0);
-return 0;
+    }//while
+    tgt(0,0);
+    return 0;
 
 }//intmain
 
+