File content as of revision 0:b63ffda48275:

#include "mbed.h"
Serial pc(USBTX,USBRX,9600);

AnalogIn y2(A6);
AnalogIn x2(A5);
AnalogIn y1(A4);
AnalogIn x1(A3);
DigitalIn SW(D0);
//上下モータ
PwmOut UDS(A1);
DigitalOut UDP(A0);

//左前モータ
PwmOut MS1(D7);
DigitalOut MP1(D6);

//右前モータ
PwmOut MS2(D10);
DigitalOut MP2(D9);

//左後ろモータ
PwmOut MS3(D12);
DigitalOut MP3(D11);

//右後ろモータ
PwmOut MS4(A2);
DigitalOut MP4(D2);

//ハンド開閉
PwmOut HAND(D1);

int count=0;//ここ怪しい

int main()
{
    int sw,pre_sw;
    while(1) {
        double Y2 = y2.read();
        double X2 = x2.read();
        double Y1 = y1.read();
        double X1= x1.read();
        sw=SW.read();

        if(sw<pre_sw) {
            count++;
        }

        pre_sw=sw;
        //駆動系
        if(X1>0.49 && X1<0.51 && Y1>=0.6) { //
            MS1=Y1*2.5-1.5;
            MP1=1.0;
            MS2=Y1*2.5-1.5;
            MP2=0.0;
            MS3=Y1*2.5-1.5;
            MP3=0.0;
            MS4=Y1*2.5-1.5;
            MP4=0.0;
        } else if(X1>0.49 && X1<0.51 && Y1<0.4) { //
            MS1=-2.5*Y1+1;
            MP1=0.0;
            MS2=-2.5*Y1+1;
            MP2=1.0;
            MS3=-2.5*Y1+1;
            MP3=1.0;
            MS4=-2.5*Y1+1;
            MP4=1.0;

        } else if(X1>0.70 && Y1>0.49 && Y1<0.51 ) {
            MS1=3.3*X1-2.6;
            MP1=0.0;
            MS2=3.3*x1-2.6;
            MP2=0.0;
            MS3=3.3*X1-2.6;
            MP3=1.0;
            MS4=3.3*X1-2.6;
            MP4=1.0;

        } else if(X1<0.30 && Y1>0.49 && Y1<0.51) {
            MS1=-3.3*X1+1;
            MP1=1.0;
            MS2=-3.3*X1+1;
            MP2=1.0;
            MS3=-3.3*X1+1;
            MP3=0.0;
            MS4=-3.3*X1+1;
            MP4=0.0;
        } 
        //////////////////////////////////////
        else if(X2>0.70 && Y2>0.49 && Y2<0.51 ) {
            MS1=3.3*X2-2.6;
            MP1=0.0;
            MS2=3.3*x2-2.6;
            MP2=0.0;
            MS3=3.3*X2-2.6;
            MP3=0.0;
            MS4=3.3*X2-2.6;
            MP4=0.0;

        }

        else if(X2<0.30 && Y2>0.49 && Y2<0.51) {
            MS1=-3.3*X2+1;
            MP1=1.0;
            MS2=-3.3*X2+1;
            MP2=1.0;
            MS3=-3.3*X2+1;
            MP3=1.0;
            MS4=-3.3*X2+1;
            MP4=1.0;
        } 
        /////////////////////////////////////
        else {
            MS1=0.0;
            MP1=0.0;
            MS2=0.0;
            MP2=0.0;
            MS3=0.0;
            MP3=0.0;
            MS4=0.0;
            MP4=0.0;

        }
        //pc.printf("%lf\r\n",Y2);


//ハンド開閉
        if(count%2==1) {
            HAND.pulsewidth(0.00145);//閉じる
        } else {
            HAND.pulsewidth(0.002);//開
        }
        if(Y2<0.2) {
            UDS=1.0;
            UDP=0.0;
            pc.printf("B");
        } else if(Y2>0.9) {
            UDS=1.0;
            UDP=1.0;
            pc.printf("A");
        } else  {
            UDS=0.0;
            UDP=0.0;

        }
        pre_sw=sw;

    }
}