File content as of revision 2:d01bc3dcb247:

//#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);
//        */
//        
//        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);
//        }
//    }
//}
#include "mbed.h"          //たかそうのロボット
#include "ikarashiMDC.h"
#include "omni_wheel.h"
#include "proto01.h"
#include "OmniPosition.h"
#include "PID.h"
#include "pinconfig_main.h"

PID angle(20.0,5.0,0.0000001,0.01);
OmniWheel omni(4);
OmniPosition posi(main_0TX, main_0RX);
Serial serial(mdTX, mdRX, 115200);
DigitalOut serialcontrol(D10);
RawSerial pc(USBTX, USBRX, 115200);
DigitalOut sw1(PB_1);
DigitalOut sw2();

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

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

int main()
{
    sw1 = 1;
    int cnt=0;
    motor[0].braking = true;
    double valueX, valueY, value[4];
    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(-3.14,3.14);
    angle.setOutputLimits(-0.4,0.4);
    angle.setBias(0);
    angle.setMode(1);
    angle.setSetPoint(0);
    while(1){
        angle.setProcessValue(posi.getTheta());
        double spin_power = -angle.compute();
        /*ここから*/
        if  (cnt==700) proto.targetXY(2000, 3000, posi.getX(), -posi.getY());
        if (cnt==1400) proto.targetXY(2000, 6000, posi.getX(), -posi.getY());
        if (cnt==2100) proto.targetXY(0, 6000, posi.getX(), -posi.getY());
        if (cnt==2800) proto.targetXY(0, 3000, posi.getX(), -posi.getY());
        if (cnt==3500) proto.targetXY(0, 0, posi.getX(), -posi.getY());
//        if (cnt==3000) proto.targetXY(0, 0, posi.getX(), posi.getY());
        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];
//            pc.printf("%.2f || ",value[i]);
        }
        pc.printf("x:%d || y:%d || θ:%f || vector:%f || now:%f || c:%f"
        , posi.getX(), -posi.getY(), posi.getTheta(), proto.vector, proto.nowDis, proto.counter);
        
        for(int i = 0; i < 4; i++) {
            if (fabs(value[i]) < 0.05) value[i] = 0;
            motor[i].setSpeed(value[i]);
        }
        cnt++;
        pc.printf("||pid:%.2f\r\n",spin_power);
    }
}