Revision 2:d01bc3dcb247, committed 2019-09-27

Comitter:

piroro4560

Date:

Fri Sep 27 10:50:13 2019 +0000

Parent:

1:e7413a7b013b

Commit message:

aa;

Changed in this revision

diff -r e7413a7b013b -r d01bc3dcb247 main.cpp
--- a/main.cpp	Mon Sep 23 23:30:38 2019 +0000
+++ b/main.cpp	Fri Sep 27 10:50:13 2019 +0000
@@ -1,362 +1,433 @@
-#include "mbed.h"             //Red court
-#include "ikarashiMDC.h"    
+//#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 "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);
+PID angle(20.0,5.0,0.0000001,0.01);
 OmniWheel omni(4);
-OmniPosition posi(main_0TX,main_0RX);
+OmniPosition posi(main_0TX, main_0RX);
 Serial serial(mdTX, mdRX, 115200);
+DigitalOut serialcontrol(D10);
 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 
-};
+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),
-    ikarashiMDC(1,0,SM,&serial),
-    ikarashiMDC(1,1,SM,&serial),
-    ikarashiMDC(1,2,SM,&serial),
-    ikarashiMDC(1,3,SM,&serial)
+    ikarashiMDC(0,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]);
-}
+Proto1 proto(500, 500, 0.4, 0.14);
 
 int main()
 {
     sw1 = 1;
-    double nowX,nowY,t;
-    
-    for(int i=0;i<4;i++)motor[i].braking = false;
+    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(-6.28,6.28);
+    angle.setInputLimits(-3.14,3.14);
     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座標)）を足せば良い
-        */
+        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];
-            motor[i].setSpeed(value[i]);
-//            pc.printf("%f ", value[i]);
+//            pc.printf("%.2f || ",value[i]);
         }
-        
-//        wait(0.001);
-        pc.printf(" loli:%d case:%d",loli[2],num);
+        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);
         
-         //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);
+        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);
     }
-}
+}
\ No newline at end of file

diff -r e7413a7b013b -r d01bc3dcb247 prototype01.lib
--- a/prototype01.lib	Mon Sep 23 23:30:38 2019 +0000
+++ b/prototype01.lib	Fri Sep 27 10:50:13 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/prototype01/#c9707baaf007
+https://os.mbed.com/users/piroro4560/code/prototype01/#b257821e4713