File content as of revision 14:66c5b0b26ba0:

/*
void pmovex2(int length)
{
    int px,py,ptheta,dx,dy,dtheta;
    int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
    int k_theta=5;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。

    update();
    px=coordinateX();
    py=coordinateY();
    ptheta=coordinateTheta();
    move(rightspeed,leftspeed);

    while(1) {
        update();
        
        dx = coordinateX() - px;
        dy = coordinateY() - py;
        dtheta = coordinateTheta() - ptheta;

        if(dy>7) {
            dy=7;
        }
        else if(dy<-7) {
            dy=-7;
        }
        
        if(-3 < dy && dy < 3){
            move(rightspeed - k_y*dy - k_theta*dtheta, leftspeed + k_y*dy + k_theta*dtheta);
        }
        else{
            move(rightspeed -50 - k_y*dy*dy - k_theta*dtheta, leftspeed -50 + k_y*dy*dy + k_theta*dtheta);
       }     
        if(dx>length) {
            move(0,0);
            break;
        }
    }
}
*/


/*
void pmovey_to(int x, int y)
{
    int px,py,ptheta,dx,dy,dtheta;
    int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。

    update();
    px=coordinateX();
    py=coordinateY();
    ptheta=coordinateTheta();
    move(rightspeed,leftspeed);

    while(1) {
        update();
        dx=coordinateX()-px;
        dy=coordinateY()-py;
        dtheta=coordinateTheta()-ptheta;

        if(dx>7) {
            dx=7;
        } else if(dx<-7) {
            dx=-7;
        }
        move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);

        if(dy>length) {
            move(0,0);
            break;
        }
    }
}

void pmovex_minus_to(int x, int y)
{
    int px,py,ptheta,dx,dy,dtheta;
    int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。

    update();
    px=coordinateX();
    py=coordinateY();
    ptheta=coordinateTheta();
    move(rightspeed,leftspeed);

    while(1) {
        update();
        dx=coordinateX()-px;
        dy=coordinateY()-py;
        dtheta=coordinateTheta()-ptheta;

        if(dy>7) {
            dy=7;
        }
        if(dy<-7) {
            dy=-7;
        }
        move(rightspeed - k_y * dy - k_theta * dtheta, leftspeed + k_y *dy + k_theta * dtheta);

        if(dx < length) {
            move(0,0);
            break;
        }
    }
}

void pmovey_minus_to(int x, int y)
{
    int px,py,ptheta,dx,dy,dtheta;
    int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。

    update();
    px=coordinateX();
    py=coordinateY();
    ptheta=coordinateTheta();
    move(rightspeed,leftspeed);

    while(1) {
        update();
        dx=coordinateX()-px;
        dy=coordinateY()-py;
        dtheta=coordinateTheta()-ptheta;

        if(dx>7) {
            dx=7;
        }
        if(dx<-7) {
            dx=-7;
        }
        move(rightspeed - k_x * dx - k_theta * dtheta,leftspeed + k_x *dx + k_theta * dtheta);

        if(dy<length) {
            move(0,0);
            break;
        }
    }
}
*/