File content as of revision 1:9d1ef7d7f874:

#include "mbed.h"             //Blue 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"

Timer time_;
PID angle(20.0,5.0,0.0000001,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,3,schmitt_trigger_0);
air air;
PS3 ps3(FEPTX,FEPRX);
chatteringremoval button0(0.3);
chatteringremoval button1(0.3);
chatteringremoval button3(0.3);
chatteringremoval button4(0.3);
DigitalOut sw1(hijoteisi);
DigitalOut LED(LED1);
Timer timer;

int num = 0,X,Y,b[12],stick[4],trigger[2],loli[3];
int X1,Y1,bb[12],mecha_num = 0,towel_num = 0;
double X0,Y0,valueX, valueY, value[4],mecha_power[3],spin_power=0,power,theta,IRdistance[2],TFdistance[2],direction = 0.0;

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)
};
 
Proto1 proto(500.0, 1000.0, 0.8, 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 Recovery()             //Recovery mechanism
{
    /* 洗濯物を挟む機構 */
    if(b[5] == 1 && trigger[1] < 50){
        mecha_power[1] -= 0.03;
        if(mecha_power[1] < -1.0){
            mecha_power[1] = -1.0;
        }
    }else if(b[5] == 0 && trigger[1] > 50){
        mecha_power[1] += 0.03;
        if(mecha_power[1] > 1.0){
            mecha_power[1] = 1.0;
        }
    }else{
        mecha_power[1] = 0;
    }
    if(b[4] == 1 && trigger[0] < 50){
        mecha_power[2] += 0.03;
        if(mecha_power[2] > 1.0){
            mecha_power[2] = 1.0;
        }
    }else if(b[4] == 0 && trigger[0] > 50){
        mecha_power[2] -= 0.03;
        if(mecha_power[2] < -1.0){
            mecha_power[2] = -1.0;
        }
    }else{
        mecha_power[2] = 0;
    }
    /* 洗濯物を挟む機構の首 */
    if (b[2] == 1) {
        mecha_power[0] += 0.04;
        if(mecha_power[0] > 0.8){
            mecha_power[0] = 0.8;
        }
    }else{
        mecha_power[0] = 0;
    }
    
    motor[4].setSpeed(mecha_power[0]);
    motor[5].setSpeed(mecha_power[2]);
    motor[6].setSpeed(mecha_power[1]);
}

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

void mechamove()
{
    if(towel_num == 1){
        towel.setPoint(2000);
    }else{
        towel.setPoint(3650);
    }
}
    
int main()
{
    sw1 = true;
    double nowX,nowY,t,LPF[4],lastLPF[4],start_angle,now_angle,original_angle = -3.14;
    int b2[12],b3[12],estop_num=0;
    
    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.5,0.5);
    angle.setBias(0);
    angle.setMode(1);
    angle.setSetPoint(0);
    
    button0.countreset();
    button1.countreset();
    button3.countreset();
    
    sensor.startReceive(28);
//    time_.reset();
//    time_.start();
    while(1){
//        pc.printf("%d\n\r",time_.read_ms());
//        time_.reset();
//        time_.start();
        LED = !LED;    
        
        towel.setPulse(loli[2]);
        
//      PS3 value
        for(int i = 0; i < 12; i++) {
            b[i] = ps3.getButton(i);
            b3[i] = b2[i] - b[i];
            b2[i] = b[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] - 127.0) / 127.0;
        Y0 = ((stick[1] * -1) + 127.0) / 127.0;
        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
        button0.assignvalue(b[6]);
        button1.assignvalue(b[11]);
        button3.assignvalue(b[1]);
        button4.assignvalue(b[0]);
        num = button0.getCount();
        mecha_num = button3.getCount();
        towel_num = button4.getCount() % 2;
        estop_num = button1.getCount() % 2;
        
        //estop
        if(estop_num == 1){
            sw1 = false;
        }else{
            sw1 = true;
        }
        
        /*【Action】*/
        switch(num){   
            case 0: towel.allstop();
                    stop();
                    break;
            case 1: Recovery();
                    mechamove();
                    towel.open();
                    air.solenoid1_open();
                    nowX = posi.getX();
                    nowY = posi.getY();
                    break;
            //case 2: proto.targetXY(2000,3000, nowX, nowY);
//                    towel.allstop();
//                    stop();
//                    break;
//            case 3: mechamove();
//                    stop();
//                    nowX = posi.getX();
//                    nowY = posi.getY();
//                    break;
//            case 4: proto.targetXY(0, 0, nowX, nowY);
//                    towel.allstop();
//                    stop();
//                    break;
            default: motor[0].setSpeed(0);
                     motor[1].setSpeed(0);
                     motor[2].setSpeed(0);
                     motor[3].setSpeed(0); 
                     
                     towel.allstop();
                    
                     air.solenoid1_close();
                     air.solenoid2_close();
                              
        }
        
        towel.pid_compute();
        
        if(num == 0){
            motor[0].setSpeed(0);
            motor[1].setSpeed(0);
            motor[2].setSpeed(0);
            motor[3].setSpeed(0); 
            air.solenoid1_close();
            air.solenoid2_close();
        
        }else{
            power = hypot(X0,Y0);
            theta = atan2(Y0,X0) + posi.getTheta();
            /**
             * ここまでテンプル
             */
            start_angle = posi.getTheta();
        
            if ( b3[8] == 1) original_angle -= 1.57;
            if ( b3[9] == 1) original_angle += 1.57;
            
            if ( b[3] ) original_angle = start_angle;
                                    
            now_angle = start_angle - original_angle;
            
            if ( now_angle > 3.14 ) {
                now_angle -= 6.28;
            }
            if ( now_angle < -3.14 ) {
                now_angle += 6.28;
            }
            
            angle.setProcessValue(now_angle);
            spin_power = -angle.compute();
            if(b[7] == 1){
                power *= 1.0;
            }else{
                power *= 0.3;
            }
            omni.computeCircular(power,theta,spin_power);
        
            for(int i = 0; i < 4; i++){
                value[i] = omni.wheel[i];
                LPF[i] = (1 - 0.8) * lastLPF[i] + 0.8 * value[i];
                lastLPF[i] = LPF[i];
                motor[i].setSpeed(LPF[i]);
//            pc.printf("%f ", LPF[i]);
            }
            
        //}else{
//            angle.setProcessValue(posi.getTheta());
//            spin_power = -angle.compute();
//            proto.Input_nowXY( posi.getX() , posi.getY() );
//            proto.calculate();
//            omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
//            
//            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 ",loli[2]);
//        pc.printf(" %d %d case:%d ",mecha_num,towel_num,num);
//        for(int i=0;i<12;i++) pc.printf(" %d ",b3[i]);
         //Wheel value
//         pc.printf(" stickX:%f stickY:%f power:%f θ:%f",X0,Y0,power,theta);
//        pc.printf(" vector:%f  pid:%.2f",proto.vector,spin_power);
         //Odmetry
//        pc.printf("x:%d y:%d θ:%f ",posi.getX(),posi.getY(),posi.getTheta());
        
//        pc.printf("\r\n");
    }
}