Revision 0:d3db2b0afc76, committed 2019-09-23

Comitter:

ec30109b

Date:

Mon Sep 23 12:25:26 2019 +0000

Commit message:

madamadadame

Changed in this revision

diff -r 000000000000 -r d3db2b0afc76 OmniPosition.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OmniPosition.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/highfieldsnj/code/OmniPosition/#893d1d146757

diff -r 000000000000 -r d3db2b0afc76 PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19

diff -r 000000000000 -r d3db2b0afc76 PS3.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PS3.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/PS3/#78827486d24f

diff -r 000000000000 -r d3db2b0afc76 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/QEI/#fe23b32e62ca

diff -r 000000000000 -r d3db2b0afc76 SerialMultiByte.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialMultiByte.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/SerialMultiByte/#ca2a6fdb24af

diff -r 000000000000 -r d3db2b0afc76 air.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/air.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ec30109b/code/air2/#c11087e1926d

diff -r 000000000000 -r d3db2b0afc76 chatteringremoval.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/chatteringremoval.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/chatteringremoval/#dce990775412

diff -r 000000000000 -r d3db2b0afc76 ikarashiMDC_2byte_ver.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ikarashiMDC_2byte_ver.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/ikarashiMDC_2byte_ver/#97bb662f1e1f

diff -r 000000000000 -r d3db2b0afc76 main.cpp
--- /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");
+    }
+}

diff -r 000000000000 -r d3db2b0afc76 mbed-os.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#1bf6b20df9d3cd5f29f001ffc6f0d0fcbbb96118

diff -r 000000000000 -r d3db2b0afc76 omni_wheel.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omni_wheel.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/omni_wheel/#cfec945ea421

diff -r 000000000000 -r d3db2b0afc76 pinconfig_main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pinconfig_main.h	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,35 @@
+#ifndef PINCONFIG_H
+#define PINCONFIG_H
+const PinName switchTX = PA_9;
+const PinName switchRX = PB_7;
+const PinName schmitt_trigger_0 = PB_5;
+const PinName schmitt_trigger_1 = PB_4;
+const PinName schmitt_trigger_2 = PB_10;
+const PinName schmitt_trigger_3 = PB_8;
+const PinName schmitt_trigger_4 = PB_9;
+const PinName schmitt_trigger_5 = PA_6;
+const PinName schmitt_trigger_6 = PA_4;
+const PinName schmitt_trigger_7 = PB_0;
+const PinName schmitt_trigger_8 = PC_1;
+const PinName schmitt_trigger_9 = PB_12;
+const PinName schmitt_trigger_10 = PA_11;
+const PinName schmitt_trigger_11 = PB_6;
+const PinName schmitt_trigger_12 = PC_3;
+const PinName schmitt_trigger_13 = PC_2;
+const PinName schmitt_trigger_14 = PC_0;
+const PinName mdTX = PC_10;
+const PinName mdRX = PC_11;
+const PinName solenoid_0 = PB_2;
+const PinName solenoid_1 = PB_8;
+const PinName solenoid_2 = PC_8;
+const PinName solenoid_3 = PA_7;
+const PinName solenoid_4 = PC_5;
+const PinName solenoid_5 = PA_12;
+const PinName FEPTX = PA_0;
+const PinName FEPRX = PA_1;
+const PinName main_0TX = PC_6;
+const PinName main_0RX = PC_7;
+const PinName main_1TX = PC_12;
+const PinName main_1RX = PD_2;
+const PinName hijoteisi = PB_1;
+#endif
\ No newline at end of file

diff -r 000000000000 -r d3db2b0afc76 prototype01.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/prototype01.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/prototype01/#16a24200ab68

diff -r 000000000000 -r d3db2b0afc76 towelest.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/towelest.lib	Mon Sep 23 12:25:26 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ec30109b/code/towelest/#1d0e66198bb4