File content as of revision 0:df2659fd8031:

//headerではPINを直接扱わない応用関数を扱う

#include <math.h>
#include"mbed.h"
#include "locate.h"
#include"prime.h"

const int speed=100;//基本速度
const float allowdegree = 0.02;//角度許容誤差
const int speedscaler = 10;//角度と速度を変換
const int minspeed = 50;//最小速度
const int allowlength=30;





void moveto(int targetx, int targety){
    int x, y;
    float theta, phi;
    int onoff = 1;

    //よく使うかたまり
        x = coordinateX();
        y = coordinateY();
        theta = coordinateTheta();//自己位置取得
        phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得


    //まず目的地方面に回転。今回は必ず時計回りで位置合わせするが今後最短経路に直す
    while (1){
        x = coordinateX();
        y = coordinateY();
        theta = coordinateTheta();
        phi = atan(double(targety - y) / double(targetx - x));

        move((-1)*speed, speed);
        if (phi - allowdegree < theta&&theta< phi + allowdegree){ break; }//thetaにpiを足して逆ベクトルを判定
    }

    //角度を合わせながら直進
    while (onoff){
        x = coordinateX();
        y = coordinateY();
        theta = coordinateTheta();
        phi = atan(double(targety - y) / double(targetx - x));

        move(speed + 10 * (phi - theta), speed); //この10はradを回転数に影響させるスカラー。右輪のみで方向を調節するが、

        if (targetx - allowlength < x&&x < targetx + allowlength){
            //x座標が合ったら
            if (targety - allowlength < y&&y < targety + allowlength){
                onoff = 0;//whileから抜ける
            }
        }
    }
    //位置合わせ完了(最終的にはx,yどっちか先に合わせたい)
}



void back(){

    int x1, y1, x2, y2,distance;

    x1 = coordinateX();
    y1= coordinateY();


    move((-1)*speed, (-1) *speed);

    while (1){
        x2 = coordinateX();
        y2 = coordinateY();
        distance = (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
        
        if (distance > 22500){ 
            move(0, 0);
            break; }
    }


}


void fixto(int targetx,int targety){//必ずy正方向からとることとする
    int onoff=1,lr=0;
const int X=20;//左右の位置調整。回転数をどれだけ増減させるかをこの定数に入れる

    while (onoff){
            //(right,left)=(off,off),(on,off),(off,on),(on,on)
             //                 0        1         2       3    
        lr = sensor();
        
        switch (lr){
        
        case 0:
            break;
        case 1:
            move(speed-X , speed+X);
            break;
        case 2:
            move(speed+X , speed-X);
            break;
        case 3:
            move(speed,speed);
            break;
        default:
            break;
        }

    }
}





void lift(int level) {//段数を指定して回転させる。
    const int K=33;//階数と回転角度を反映させる比例点定数

    step(K*level); 
        
        
}

void release(void){

    step(100);

}

void extraordinary(){

    int x, y,targetx,targety;
    float theta, phi;

    targetx=sensor()*1819;
    targety=1750;
    moveto(targetx,0);
    
    //よく使うかたまり
        x = coordinateX();
        y = coordinateY();
        theta = coordinateTheta();//自己位置取得
        phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
        int goalx = 0, goaly = blue()*1200;//受け渡しゾーンの中心

    //まず目的地方面に回転。今回は必ず時計回りで位置合わせするが今後最短経路に直す
    while (1){
        x = coordinateX();
        y = coordinateY();
        theta = coordinateTheta();
        phi = atan(double(targety - y) / double(targetx - x));

        move((-1)*speed, speed);
        if (phi - allowdegree < theta&&theta< phi + allowdegree){ break; }//thetaにpiを足して逆ベクトルを判定
    }
    
    open_arm();
    open_hand();
        
    moveto(targetx,1750);
    
    close_hand();
    close_arm();
    lift(1);
    
    moveto(goalx,goaly);
}