File content as of revision 8:7b368096fed8:

#include "mbed.h"
#include "SpeedController.h"
#include "EC.h"
#include "R1370P.h"
#include "enc_1multi.h"
#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

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,PullUp);
DigitalIn reset_f(PC_1,PullUp);
DigitalIn reset_a(PA_4,PullUp);

PwmOut servo(PB_7);//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;
double y;
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=300;
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)//void de sika tsukawanaino ha mottainai kimosuru
{
    target_R=BASIC_SPEED*r;
    target_L=BASIC_SPEED*l;
}


//printf wo dousa kakunin sidai kesou toha omotte imasu
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-120&&ay>=0) {
        t.start();
        //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()>=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);
        }
    } else if(y<goaly-20&&y>=goaly-120&&ay>=0) {
        tgt(ay/2,by/2);


    } else if(y>goaly+120&&ay<0) {
        t.start();
        //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()>=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);
        }
    } else if(y>goaly+20&&y<=goaly+120&&ay<0) {
        tgt(ay/2,by/2);
    } else {
        i++;
        t.reset();
        pc.printf("owari%d\r\n",i);
    }
}



void susumu_xl(double axl,double bxl,double goalxl)//hidarikara mokuhyoutenn ni toutatsu surutameno dousa
{

    if(x<goalxl-120) {
        t.start();
        //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()>=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);
        }
    } else if(x<goalxl-20&&x>=goalxl-120) {
        tgt(axl/2,bxl/2);

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

void susumu_xr(double axr,double bxr,double goalxr)//migikara mokuhyoutenn ni toutatsu surutameno dousa
{

    if(x>goalxr+120) {
        t.start();
        //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()>=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);
        }
    } else if(x>goalxr+20&&x<=goalxr+120) {
        tgt(axr/2,bxr/2);

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

void susumu_ang(double a,double b,double goal)//kakudo
{
    if(goal-30>angle&&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);
        } else 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);
        }
    } else if(angle<goal-5&&angle>=goal-30&&a<b) {
        tgt(a/2,b/2);

    } else if(angle>goal+30&&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);
        } else 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);
        }
    } else if(angle>goal+5&&angle<=goal+30&&a>b) {
        tgt(a/2,b/2);

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





int main(void)
{

    gyro.initialize();    //main関数の最初に一度だけ実行
    gyro.acc_offset();    //やってもやらなくてもいい

    printf("start\r\n");
    motor_tick.attach(&calOmega,0.05);
    motorR.setDOconstant(31.2);
    motorL.setDOconstant(30);//c
    motorR.setPDparam(0,0);
    motorL.setPDparam(0,0);//pd//akirameta


    EC1.setDiameter_mm(48);//sokuteirinnhannkei
    double  getDistance_mm();
    //int EC1.getCount()=0;
    void reset  ();
    EC1.reset();


    //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
    
    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の値を変えることで直進・後退・右回転・左回転を行う
        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;

        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);;
            //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) {
            servo.pulsewidth_us(950);
            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_a.read()==0&&button.read()==0) {
                    denjiben=1;
                    
                    x=180;
                    y=2674;

                    i=12;
                }
            }//x,y
        }
        if(i==1) {
            servo.pulsewidth_us(950);
            susumu_y(0.8,0.8,857);//x,x+178
        }
        if(i==2) {
            susumu_ang(0,0.7,90);//x+155,x+471.5//219.2,65.8//534.2,730.8//180,576.6
        }
        if(i==3) {
            susumu_xl(0.8,0.8,745);//728.5,1078.5
        }
        if(i==4) {
            susumu_ang(0.7,0,0);//850,1372
        }
        if(i==5) {
            susumu_y(0.8,0.8,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++;
            }
        }//gatiasari


        if(i==7) {
            wait(2);
            t.start();
            if(t<2) {
                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++;
        }
        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.8,0.8,asari_x);//x,y
        }
        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(i==16) {
            susumu_xr(-1,-1,555);//700,y
        }
        if(i==17) {
            susumu_ang(0,-0.7,0);//390,y-310
        }
        if(i==18) {
            susumu_y(-1,-1,goal_y+155);//390,y+310
        }
        if(i==19) {
            susumu_ang(0,-0.7,-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=1;
            wait(0.5);

            break;
        }
        if(i==30) {
            tgt(-1,-1);
        }


    }//while
    tgt(0,0);
    return 0;

}//intmain