File content as of revision 1:e7413a7b013b:

#include "mbed.h"             //Red court
#include "ikarashiMDC.h"    
#include "omni_wheel.h"
#include "proto01.h"
#include "OmniPosition.h"
#include "SerialMultiByte.h"
#include "PID.h"
#include "PS3.h"
#include "air.h"
#include "towelest.h"
#include "chatteringremoval.h"
#include "pinconfig_main.h"

PID angle(10.0,0.2,0.000095,0.01);
OmniWheel omni(4);
OmniPosition posi(main_0TX,main_0RX);
Serial serial(mdTX, mdRX, 115200);
RawSerial pc(USBTX, USBRX, 115200);
SerialMultiByte sensor(main_1TX,main_1RX);
towelest towel(&serial,2,schmitt_trigger_0,schmitt_trigger_1,schmitt_trigger_2);
air air;
PS3 ps3(FEPTX,FEPRX);
chatteringremoval button1(1.0);
DigitalOut sw1(hijoteisi);
DigitalOut LED(LED1);
Timer timer;

int num = 0, X, Y, b[12], stick[4], trigger[2], loli[3], b+[12];
int X0,Y0,X1,Y1;
double valueX, valueY, value[4],mecha_power[4],spin_power=0, IRdistance[2],TFdistance[2],x2, y2;

union UnionBytes{
    uint8_t bytes[28];
    float IRdist[7];    //0~1
    int32_t TFdist[7];  //2~3
    int32_t pulses[7];  //4~6 
};

ikarashiMDC motor[]=
{
    ikarashiMDC(0,0,SM,&serial),
    ikarashiMDC(0,1,SM,&serial),
    ikarashiMDC(0,2,SM,&serial),
    ikarashiMDC(0,3,SM,&serial),
    ikarashiMDC(1,0,SM,&serial),
    ikarashiMDC(1,1,SM,&serial),
    ikarashiMDC(1,2,SM,&serial),
    ikarashiMDC(1,3,SM,&serial)
};

Proto1 proto(500, 700, 0.4, 0.14);


void getSensorData()       //Receive sensor value 
{
    UnionBytes bytedata;
    sensor.getData(bytedata.bytes);
    for(int i=0; i<2; i++){
        IRdistance[i] = bytedata.IRdist[i];
    }
    for(int i=0; i<2; i++){
        TFdistance[i] = bytedata.TFdist[i+2];
    }
    for(int i=0; i<3; i++){
        loli[i] = bytedata.pulses[i+4];
    }
}

void clossmove0()       //Adjustment,Airfront
{
    if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
        x2=0;
        y2=0.3;
    }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
        x2=0;
        y2=-0.3;
    }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
        x2=-0.3;
        y2=0;
    }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
        x2=0.3;
        y2=0;
    }else{
        x2=0;
        y2=0;
    }
}

void clossmove1()       //Adjustment,Recoveryfront
{
    if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
        x2=0.3;
        y2=0;
    }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
        x2=-0.3;
        y2=0;
    }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
        x2=0;
        y2=0.3;
    }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
        x2=0;
        y2=-0.3;
    }else{
        x2=0;
        y2=0;
    }
}

void clossmove2()       //Adjustment,Dropfront
{
    if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
        x2=0;
        y2=-0.3;
    }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
        x2=0;
        y2=0.3;
    }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
        x2=0.3;
        y2=0;
    }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
        x2=-0.3;
        y2=0;
    }else{
        x2=0;
        y2=0;
    }
}

void clossmove3()       //Adjustment,Towelfront
{
    if(b[0]==1 && b[1]==0 && b[2]==0 && b[3]==0){
        x2=-0.3;
        y2=0;
    }else if(b[0]==0 && b[1]==1 && b[2]==0 && b[3]==0){
        x2=0.3;
        y2=0;
    }else if(b[0]==0 && b[1]==0 && b[2]==1 && b[3]==0){
        x2=0;
        y2=-0.3;
    }else if(b[0]==0 && b[1]==0 && b[2]==0 && b[3]==1){
        x2=0;
        y2=0.3;
    }else{
        x2=0;
        y2=0;
    }
}

void Recovery()             //Recovery mechanism
{
    /* 洗濯物を挟む機構 */
    if((X0 > 170)&&(Y0 > 50 && Y0 < 200)) {
        mecha_power[1] = 0.6;
    }else if((X0 < 100)&&(Y0 > 50 && Y0 < 200)) {
        mecha_power[1] = -0.6;
    }else{
        mecha_power[1] = 0;
    }
    if((X1 > 170)&&(Y1 > 50 && Y1 < 200)) {
        mecha_power[2] = 0.6;
    }else if((X1 < 100)&&(Y1 > 50 && Y1 < 200)) {
        mecha_power[2] = -0.6;
    }else{
        mecha_power[2] = 0;
    }
    /* 洗濯物を挟む機構の首 */
    if (b[10] == 1 && b[11] == 0) {
        mecha_power[0] = 0.3;
    } else if (b[10] == 0 && b[11] == 1) {
        mecha_power[0] = -0.3;
    } else {
        mecha_power[0] =0.0;
    }
    mecha_power[3] = 0;
    
    motor[4].setSpeed(mecha_power[0]);
    motor[5].setSpeed(mecha_power[1]);
    motor[6].setSpeed(mecha_power[2]);
    motor[7].setSpeed(mecha_power[3]);
}

void stop()
{
    for(int i=4;i<8;i++) motor[i].setSpeed(0);
}

void Drop()
{
    if(b[4] == 1 && b[5] == 0){              
        mecha_power[3] = 0.5;
    }else if(b[4] == 0 && b[5] == 1) {
        mecha_power[3] = -0.5;
    }else{
        mecha_power[3] = 0;
    }
    motor[7].setSpeed(mecha_power[3]);
}

int main()
{
    sw1 = 1;
    double nowX,nowY,t;
    
    for(int i=0;i<4;i++)motor[i].braking = false;
    omni.wheel[0].setRadian(PI * 1 / 4);
    omni.wheel[1].setRadian(PI * 3 / 4);
    omni.wheel[2].setRadian(PI * 5 / 4);
    omni.wheel[3].setRadian(PI * 7 / 4);
    angle.setInputLimits(-6.28,6.28);
    angle.setOutputLimits(-0.4,0.4);
    angle.setBias(0);
    angle.setMode(1);
    angle.setSetPoint(0);
    
    button1.countreset();
    
    sensor.startReceive(28);
    
    while(1){
        LED = !LED;
        
        towel.setPulse(loli[2]);
        
//      PS3 value
        for(int i = 0; i < 12; i++) {
            b[i] = ps3.getButton(i);
            pc.printf("%2d", b[i] );
        }
        for(int i = 0; i < 4; i++) {
            stick[i] = ps3.getStick(i);
            pc.printf("%4d", stick[i] );
        }
        X0 = stick[0];
        Y0 = stick[1];
        X1 = stick[2];
        Y1 = stick[3];
        
        for(int i = 0; i < 2; i++) {
            trigger[i] = ps3.getTrigger(i);
//            pc.printf("%4d",trigger[i]);
        }

        //Sensor value
        getSensorData();

        //Count
        button1.assignvalue(b[9]);
        num = button1.getCount();
        
        /*【Action】*/
        switch(num){       
            case 0: angle.setSetPoint(0);
                    clossmove0();
                    Recovery();
                    towel.open();
                    air.solenoid1_open();
                    nowX = posi.getX();
                    nowY = posi.getY();
                    break;
            case 1: proto.targetXY(-3000, 0, nowX ,nowY);
                    towel.allstop();
                    stop();
                    break;
            case 2: angle.setSetPoint(1.57);
                    Drop();
                    clossmove3();
                    towel.allstop();
                    stop();
                    nowX = posi.getX();
                    nowY = posi.getY();
                    break;
            case 3: proto.targetXY(-2000, 3000, nowX ,nowY);
                    towel.allstop();
                    stop();
                    if((posi.getX()>-2200 && posi.getX()<-1800) && (posi.getY()>2800 && posi.getY()<3200)){
                        num = 4;
                        angle.setSetPoint(0);
                    }
                    break;
            case 4: clossmove0();
//                    Recovery();
//                    nowX = posi.getX();
//                    nowY = posi.getY();
//                    break;
//            case 5: proto.targetXY(-3000, 0, nowX ,nowY);
//                    break;
//            case 6: angle.setSetPoint(1.57);
//                    Drop();
//                    clossmove3();
//                    nowX = posi.getX();
//                    nowY = posi.getY();
//                    break;
//            case 7: proto.targetXY(-3000,2600,-3000,0);
//                    if(posi.getX()>-3200 && posi.getX()<-2800 && posi.getY()>2400 && posi.getY()<2800){
//                        num = 8;
//                    }
//                    break;
//            case 8: angle.setSetPoint(1.57);
//                    clossmove3();
//                    nowX = posi.getX();
//                    nowY = posi.getY();
//                    
//                    towel.drop();
//                    t = timer.read();
//                    if(b[4] == 1 && b[5] == 1){
//                        timer.start();
//                    }
//                    if(t > 5){
//                        towel.close();
//                    }else if(t > 20){
//                        angle.setSetPoint(0);
//                        air.solenoid2_open();
//                    }
//                    break;      
//            case 9: angle.setSetPoint(0);
//                    proto.targetXY(0, 0,nowX ,nowY);
//                    break;
            default: motor[0].setSpeed(0);
                     motor[1].setSpeed(0);
                     motor[2].setSpeed(0);
                     motor[3].setSpeed(0); 
                     
                     air.solenoid1_close();
                     air.solenoid2_close();
                              
        }
        
        angle.setProcessValue(posi.getTheta());
        spin_power = -angle.compute();

        if(num == 0 || num == 2 || num == 4 || num == 6 || num == 8){
            omni.computeXY(x2,y2,spin_power);
        }else{
            proto.Input_nowXY(posi.getX(),posi.getY());
            proto.calculate();
            omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
        }
        /*
        omni.conputeCircular(proto.getValue(), proto.gettheta(), spin_power);
        機体の向きに関係ないスティック操作をしたい場合はspin_powerに、スティックの角度（atan2(スティックのy座標,スティックのx座標)）を足せば良い
        */
        
        for(int i = 0; i < 4; i++){
            value[i] = omni.wheel[i];
            motor[i].setSpeed(value[i]);
//            pc.printf("%f ", value[i]);
        }
        
//        wait(0.001);
        pc.printf(" loli:%d case:%d",loli[2],num);
        
         //Wheel value
//        pc.printf(" vector:%f  pid:%.2f",proto.vector,spin_power);
         //Odmetry
//        pc.printf("x:%d y:%d θ:%f now:%f",posi.getX(),posi.getY(),posi.getTheta(),proto.nowDis);
        
        pc.printf("\r\n");
        for(int i = 0; i < 12; i++) {
            b+[i] = ps3.getButton(i);
        }
    }
}