File content as of revision 0:29c024d6882f:

#include "mbed.h"
#include "SpeedController.h"
#include "EC.h"
#include "R1370P.h"
#include "enc_1multi.h"
#define BASIC_SPEED 30  //モーターはこの角速度で駆動させる

SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1);  //right  enc migi ue
SpeedControl motorL(PB_3,PB_5,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

Serial pc(USBTX, USBRX); // tx, rx //PC USB
R1370P gyro(PC_6,PC_7);


void calOmega() //角速度計算関数
{
    motorR.CalOmega();
    motorL.CalOmega();
    EC1.CalOmega();
}

DigitalIn button(USER_BUTTON);

PwmOut servo(PB_14);//servo
PwmOut motor_f(PC_9);
PwmOut motor_b(PB_9);//arm
DigitalOut denjiben(PC_0);//dennjibenn


double new_dist=0;
double old_dist=0;
double d_dist=0;
double x=250;
double y=150;//start point
Timer t;
int i=0;

int kai=0;//printf kansuu
double target_R=0,target_L=0;


double angle; //変数宣言


void tgt(double r,double l)
{
    target_R=BASIC_SPEED*r;
    target_L=BASIC_SPEED*l;
}


void susumu_y(double ay,double by,double goaly)
{

    if(y<goaly-50&&ay>=0) {
        t.start();
        pc.printf("R=%f L=%f\r\n",target_R,target_L);
        if(t.read_ms()<100) {
            pc.printf("t=%f",t.read());
            tgt(ay/3,by/3);
        }
        if(t.read_ms()>=100&&t.read_ms()<200) {
            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);
        }
    }
    if(y<goaly&&y>=goaly-50&&ay>=0) {
        tgt(ay/3,by/3);


    }
    if(y>goaly+50&&ay<0) {
        t.start();
        pc.printf("R=%f L=%f\r\n",target_R,target_L);
        if(t.read_ms()<100) {
            pc.printf("t=%f",t.read());
            tgt(ay/3,by/3);
        }
        if(t.read_ms()>=100&&t.read_ms()<200) {
            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);
        }
    }
    if(y>goaly&&y<=goaly+50&&ay<0) {
        tgt(ay/3,by/3);
    } else {
        i++;
        t.reset();
        pc.printf("owari\r\n");
    }
}



void susumu_x(double ax,double bx,double goalx)
{

    if(x<goalx-50&&ax>=0) {
        t.start();
        pc.printf("R=%f L=%f\r\n",target_R,target_L);
        if(t.read_ms()<100) {
            pc.printf("t=%f",t.read());
            tgt(ax/3,bx/3);
        }
        if(t.read_ms()>=100&&t.read_ms()<200) {
            pc.printf("t=%f",t.read());
            tgt(ax*2/3,bx*2/3);
        } else {
            tgt(ax,bx);
            t.stop();
            pc.printf("R=%f L=%f",target_R,target_L);
        }
    }
    if(x<goalx&&x>=goalx-50&&ax>=0) {
        tgt(ax/3,bx/3);

    }
    if(x>goalx+50&&ax<0) {
        t.start();
        pc.printf("R=%f L=%f\r\n",target_R,target_L);
        if(t.read_ms()<100) {
            pc.printf("t=%f",t.read());
            tgt(ax/3,bx/3);
        }
        if(t.read_ms()>=100&&t.read_ms()<200) {
            pc.printf("t=%f",t.read());
            tgt(ax*2/3,bx*2/3);
        } else {
            tgt(ax,bx);
            t.stop();
            pc.printf("R=%f L=%f",target_R,target_L);
        }
    }
    if(x>goalx&&x<=goalx+50&&ax<0) {
        tgt(ax/3,bx/3);

    } else {
        i++;
        t.reset();
        pc.printf("owari\r\n");
    }
}


void susumu_ang(double a,double b,double goal)
{
    if(x<angle-5&&a>b) {//usetsu
        t.start();
        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);
        }
        if(t.read_ms()>=100&&t.read_ms()<200) {
            pc.printf("t=%f",t.read());
            tgt(a*2/3,b*2/3);
        } else {
            tgt(a,b);
            t.stop();
            pc.printf("R=%f L=%f",target_R,target_L);
        }
    }
    if(angle<goal&&angle>=goal-5&&a>b) {
        tgt(a/3,b/3);

    }
    if(angle>goal+5&&a<b) {//sasetsu
        t.start();
        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);
        }
        if(t.read_ms()>=100&&t.read_ms()<200) {
            pc.printf("t=%f",t.read());
            tgt(a*2/3,b*2/3);
        } else {
            tgt(a,b);
            t.stop();
            pc.printf("R=%f L=%f",target_R,target_L);
        }
    }
    if(angle>goal&&angle<=goal+5&&a<b) {
        tgt(a/3,b/3);

    } else {
        i++;
        t.reset();
        pc.printf("reset\r\n");
    }
}





int main(void)
{
    printf("start\r\n");
    motor_tick.attach(&calOmega,0.05);
    motorR.setDOconstant(34.1);
    motorL.setDOconstant(30);//c
    motorR.setPDparam(0,0);
    motorR.setPDparam(0,0);//pd


    gyro.initialize();    //main関数の最初に一度だけ実行
    EC1.setDiameter_mm(50);//sokuteirinnhannkei
    double  getDistance_mm();
    //int EC1.getCount()=0;
    EC1.reset();
    void reset  ();


    //    servo.period_ms(60);

    motor_f.period_ms(30);
    motor_b.period_ms(30);//arm

    while(1) {
        // motorR.turnF(0.3);
        //motorL.turnF(0.3);//for debug
        motorR.Sc(target_R);
        motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
        angle=gyro.getAngle();    //角度の値を受け取る

        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);

        double d_x=d_dist*sin(angle);
        double d_y=d_dist*cos(angle);
        x=x+d_x;
        y=y+d_y;



        if(kai>=3) {
            new_dist=EC1.getDistance_mm();
            d_dist=new_dist-old_dist;
            old_dist=new_dist;

            //double d_x=d_dist*sin(angle);;
            //double d_y=d_dist*cos(angle);;
            //x=x+d_x;
            //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("i=%d\r\n",i);
            //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
            pc.printf("x=%f y=%f",x,y);
            //pc.printf("angle=%f\r\n",angle);
            //機体の進む方向、右モーターの角速度、左モーターの角速度を表示
            kai=0;
        }
        kai++;
        if(i==0) {
            t.start();
            if(t<1) {
                motor_f=0.8;
                motor_b=0;
            } else {
                motor_f=0;
                motor_b=0;
                printf("finish");
                t.reset();
                i++;
            }
        }//motor test

//kokomade kihonnsettei


        /* if(i==0) {
             printf("a\n\r");
             target_R=0;
             target_L=0;
             i++;
             pc.printf("i=0\r\n");
             pc.printf("R=%f L=%f",target_R,target_L);
         }*/
        if(i==1) {
            susumu_y(1,1,472);
            /*        if(y<472) {
                        t.start();
                        pc.printf("R=%f L=%f\r\n",target_R,target_L);
                        if(t.read_ms()<200) {
                            pc.printf("t=%f",t.read());
                            target_R=BASIC_SPEED/3;
                            target_L=BASIC_SPEED/3;
                        } else {
                            target_R=BASIC_SPEED;
                            target_L=BASIC_SPEED;
                            t.stop();
                            pc.printf("R=%f L=%f",target_R,target_L);
                        }//250150 250472
                    } else {
                        i++;
                        t.reset();
                        pc.printf("reset\r\n");
              */      //}
        }
        if(i==2) {
            susumu_ang(1/3,1,45);
            /*          t.start();
                      if(angle<45) {
                          if(t.read_ms()<200) {
                              target_R=BASIC_SPEED/6;
                              target_L=BASIC_SPEED/2;
                          } else {
                              target_R=BASIC_SPEED/3;
                              target_L=BASIC_SPEED;
                          }//250472 341691
                      } else i++;
                      t.stop();
                      t.reset();
            */
        }//kairyoutyuu
        if(i==3) {
            susumu_x(1,1,709);
            //            if(x<709) {
            //              target_R=BASIC_SPEED;
            //            target_L=BASIC_SPEED;
            //          //341691 7091059
            //    } else i++;
        }
        if(i==4) {
            susumu_ang(1,1/3,0);
            /*   if(angle>0) {
                   target_R=BASIC_SPEED;
                   target_L=BASIC_SPEED/3;
                   //7091059 8001278
               } else i++;
            */
        }
        if(i==5) {
            susumu_y(1,1,1700);
            //                 if(y<1700) {
            //                   target_R=BASIC_SPEED;
            //                 target_L=BASIC_SPEED;
            //               //8001278 8001700
            //         } else i++;
            motorR.stop();
            motorL.stop();
        }

        if(i==6) {

            i++;
        }//gatiasari


        /*            if(i==7) {
                        if(angle>=-89) {
                            target_R=BASIC_SPEED/4;
                            target_L=BASIC_SPEED*(-4);
                            }
                        if(angle<=-91) {
                            target_R=BASIC_SPEED/4;
                            target_L=BASIC_SPEED*(-4
                            );
                        }
                        if(angle>-91&angle<89){
                            motorR.stop();
                            motorL.stop();
                            wait(0.5);
                            if(angle>-91&angle<89){
                                i++;}
                                }
                    }
                    if(i==8) {
                        if(x>700) {
                            target_R=BASIC_SPEED;
                            target_L=BASIC_SPEED;
                            //8001700 7001700
                        } else i++;
                    }
                    if(i==9) {
                        if(angle<0) {
                            target_R=BASIC_SPEED/3;
                            target_L=BASIC_SPEED;
                            //7001700 4002000
                        } else i++;
                    }
                    if(i==10) {
                        if(y<2200) {
                            target_R=BASIC_SPEED;
                            target_L=BASIC_SPEED;
                            //4002000 4002200
                        } else i++;
                    }
                    if(i==11) {
                        if(angle<90) {
                            target_R=BASIC_SPEED/3;
                            target_L=BASIC_SPEED;
                            //4002200 7002500
                        } else i++;
                    }
                    if(i==12) {
                        if(x<1000) {
                            target_R=BASIC_SPEED;
                            target_L=BASIC_SPEED;
                            //7002500 10002500
                        } else i++;
                        motorR.stop();
                        motorL.stop();
                    }

                    if(i==13){
                        i++;
                        }




                    if(i==14) {
                        if(x>700) {
                            target_R=BASIC_SPEED*(-1);
                            target_L=BASIC_SPEED*(-1);
                            //10002500 7002500
                        } else i++;
                    }
                    if(i==15) {
                        if(angle>0) {
                            target_R=BASIC_SPEED/(-3);
                            target_L=BASIC_SPEED/(-1);
                            //7002500 4002200
                        } else i++;
                    }
                    if(i==16) {
                        if(y>2000) {
                            target_R=BASIC_SPEED*(-1);
                            target_L=BASIC_SPEED*(-1);
                            //4002200 4002000
                        } else i++;
                    }
                    if(i==17) {
                        if(angle>-90) {
                            target_R=BASIC_SPEED/(-3);
                            target_L=BASIC_SPEED/(-1);
                            //4002000 7001700
                        } else i++;
                    }
                    if(i==16) {
                        if(x<1100) {
                            target_R=BASIC_SPEED*(-1);
                            target_L=BASIC_SPEED*(-1);
                            //7001700 4002000
                        } else i++;
                    }
        */
        //kairyoutyuu kokomade

    }//while
    motorR.stop();
    motorL.stop();
//LR(0,0);
    return 0;

}//intmain