Diff: main.cpp

Revision:

0:d3db2b0afc76

Child:

1:e7413a7b013b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,354 @@
+#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];
+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);
+        }
+        
+        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");
+    }
+}