Program used by A team in the blue coat of qualifying at NHK in 2019.

Dependencies:   SerialMultiByte QEI chatteringremoval omni_wheel prototype01 towelest PID ikarashiMDC_2byte_ver air2 OmniPosition PS3

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"             //Blue court
00002 #include "ikarashiMDC.h"    
00003 #include "omni_wheel.h"
00004 #include "proto01.h"
00005 #include "OmniPosition.h"
00006 #include "SerialMultiByte.h"
00007 #include "PID.h"
00008 #include "PS3.h"
00009 #include "air.h"
00010 #include "towelest.h"
00011 #include "chatteringremoval.h"
00012 #include "pinconfig_main.h"
00013 
00014 Timer time_;
00015 PID angle(20.0,5.0,0.0000001,0.01);
00016 OmniWheel omni(4);
00017 OmniPosition posi(main_0TX,main_0RX);
00018 Serial serial(mdTX, mdRX, 115200);
00019 RawSerial pc(USBTX, USBRX, 115200);
00020 SerialMultiByte sensor(main_1TX,main_1RX);
00021 towelest towel(&serial,3,schmitt_trigger_0);
00022 air air;
00023 PS3 ps3(FEPTX,FEPRX);
00024 chatteringremoval button0(0.3);
00025 chatteringremoval button1(0.3);
00026 chatteringremoval button3(0.3);
00027 chatteringremoval button4(0.3);
00028 DigitalOut sw1(hijoteisi);
00029 DigitalOut LED(LED1);
00030 Timer timer;
00031 
00032 int num = 0,X,Y,b[12],stick[4],trigger[2],loli[3];
00033 int X1,Y1,bb[12],mecha_num = 0,towel_num = 0;
00034 double X0,Y0,valueX, valueY, value[4],mecha_power[3],spin_power=0,power,theta,IRdistance[2],TFdistance[2],direction = 0.0;
00035 
00036 union UnionBytes{
00037     uint8_t bytes[28];
00038     float IRdist[7];    //0~1
00039     int32_t TFdist[7];  //2~3
00040     int32_t pulses[7];  //4~6 
00041 };
00042  
00043 ikarashiMDC motor[]=
00044 {
00045     ikarashiMDC(0,0,SM,&serial),
00046     ikarashiMDC(0,1,SM,&serial),
00047     ikarashiMDC(0,2,SM,&serial),
00048     ikarashiMDC(0,3,SM,&serial),
00049     ikarashiMDC(1,0,SM,&serial),
00050     ikarashiMDC(1,1,SM,&serial),
00051     ikarashiMDC(1,2,SM,&serial)
00052 };
00053  
00054 Proto1 proto(500.0, 1000.0, 0.8, 0.14);
00055 
00056 void getSensorData()       //Receive sensor value 
00057 {
00058     UnionBytes bytedata;
00059     sensor.getData(bytedata.bytes);
00060     for(int i=0; i<2; i++){
00061         IRdistance[i] = bytedata.IRdist[i];
00062     }
00063     for(int i=0; i<2; i++){
00064         TFdistance[i] = bytedata.TFdist[i+2];
00065     }
00066     for(int i=0; i<3; i++){
00067         loli[i] = bytedata.pulses[i+4];
00068     }
00069 }
00070 
00071 void Recovery()             //Recovery mechanism
00072 {
00073     /* 洗濯物を挟む機構 */
00074     if(b[5] == 1 && trigger[1] < 50){
00075         mecha_power[1] -= 0.03;
00076         if(mecha_power[1] < -1.0){
00077             mecha_power[1] = -1.0;
00078         }
00079     }else if(b[5] == 0 && trigger[1] > 50){
00080         mecha_power[1] += 0.03;
00081         if(mecha_power[1] > 1.0){
00082             mecha_power[1] = 1.0;
00083         }
00084     }else{
00085         mecha_power[1] = 0;
00086     }
00087     if(b[4] == 1 && trigger[0] < 50){
00088         mecha_power[2] += 0.03;
00089         if(mecha_power[2] > 1.0){
00090             mecha_power[2] = 1.0;
00091         }
00092     }else if(b[4] == 0 && trigger[0] > 50){
00093         mecha_power[2] -= 0.03;
00094         if(mecha_power[2] < -1.0){
00095             mecha_power[2] = -1.0;
00096         }
00097     }else{
00098         mecha_power[2] = 0;
00099     }
00100     /* 洗濯物を挟む機構の首 */
00101     if (b[2] == 1) {
00102         mecha_power[0] += 0.04;
00103         if(mecha_power[0] > 0.8){
00104             mecha_power[0] = 0.8;
00105         }
00106     }else{
00107         mecha_power[0] = 0;
00108     }
00109     
00110     motor[4].setSpeed(mecha_power[0]);
00111     motor[5].setSpeed(mecha_power[2]);
00112     motor[6].setSpeed(mecha_power[1]);
00113 }
00114 
00115 void stop()
00116 {
00117     for(int i=4;i<7;i++) motor[i].setSpeed(0);
00118 }
00119 
00120 void mechamove()
00121 {
00122     if(towel_num == 1){
00123         towel.setPoint(2000);
00124     }else{
00125         towel.setPoint(3650);
00126     }
00127 }
00128     
00129 int main()
00130 {
00131     sw1 = true;
00132     double nowX,nowY,t,LPF[4],lastLPF[4],start_angle,now_angle,original_angle = -3.14;
00133     int b2[12],b3[12],estop_num=0;
00134     
00135     for(int i=0;i<4;i++)motor[i].braking = false;
00136     omni.wheel[0].setRadian(PI * 1 / 4);
00137     omni.wheel[1].setRadian(PI * 3 / 4);
00138     omni.wheel[2].setRadian(PI * 5 / 4);
00139     omni.wheel[3].setRadian(PI * 7 / 4);
00140     angle.setInputLimits(-6.28,6.28);
00141     angle.setOutputLimits(-0.5,0.5);
00142     angle.setBias(0);
00143     angle.setMode(1);
00144     angle.setSetPoint(0);
00145     
00146     button0.countreset();
00147     button1.countreset();
00148     button3.countreset();
00149     
00150     sensor.startReceive(28);
00151 //    time_.reset();
00152 //    time_.start();
00153     while(1){
00154 //        pc.printf("%d\n\r",time_.read_ms());
00155 //        time_.reset();
00156 //        time_.start();
00157         LED = !LED;    
00158         
00159         towel.setPulse(loli[2]);
00160         
00161 //      PS3 value
00162         for(int i = 0; i < 12; i++) {
00163             b[i] = ps3.getButton(i);
00164             b3[i] = b2[i] - b[i];
00165             b2[i] = b[i];
00166 //            pc.printf("%2d", b[i] );
00167         }
00168         for(int i = 0; i < 4; i++) {
00169             stick[i] = ps3.getStick(i);
00170 //            pc.printf("%4d", stick[i] );
00171         }
00172         X0 = (stick[0] - 127.0) / 127.0;
00173         Y0 = ((stick[1] * -1) + 127.0) / 127.0;
00174         X1 = stick[2];
00175         Y1 = stick[3];
00176         
00177         for(int i = 0; i < 2; i++) {
00178             trigger[i] = ps3.getTrigger(i);
00179 //            pc.printf("%4d",trigger[i]);
00180         }
00181  
00182         //Sensor value
00183         getSensorData();
00184  
00185         //Count
00186         button0.assignvalue(b[6]);
00187         button1.assignvalue(b[11]);
00188         button3.assignvalue(b[1]);
00189         button4.assignvalue(b[0]);
00190         num = button0.getCount();
00191         mecha_num = button3.getCount();
00192         towel_num = button4.getCount() % 2;
00193         estop_num = button1.getCount() % 2;
00194         
00195         //estop
00196         if(estop_num == 1){
00197             sw1 = false;
00198         }else{
00199             sw1 = true;
00200         }
00201         
00202         /*【Action】*/
00203         switch(num){   
00204             case 0: towel.allstop();
00205                     stop();
00206                     break;
00207             case 1: Recovery();
00208                     mechamove();
00209                     towel.open();
00210                     air.solenoid1_open();
00211                     nowX = posi.getX();
00212                     nowY = posi.getY();
00213                     break;
00214             //case 2: proto.targetXY(2000,3000, nowX, nowY);
00215 //                    towel.allstop();
00216 //                    stop();
00217 //                    break;
00218 //            case 3: mechamove();
00219 //                    stop();
00220 //                    nowX = posi.getX();
00221 //                    nowY = posi.getY();
00222 //                    break;
00223 //            case 4: proto.targetXY(0, 0, nowX, nowY);
00224 //                    towel.allstop();
00225 //                    stop();
00226 //                    break;
00227             default: motor[0].setSpeed(0);
00228                      motor[1].setSpeed(0);
00229                      motor[2].setSpeed(0);
00230                      motor[3].setSpeed(0); 
00231                      
00232                      towel.allstop();
00233                     
00234                      air.solenoid1_close();
00235                      air.solenoid2_close();
00236                               
00237         }
00238         
00239         towel.pid_compute();
00240         
00241         if(num == 0){
00242             motor[0].setSpeed(0);
00243             motor[1].setSpeed(0);
00244             motor[2].setSpeed(0);
00245             motor[3].setSpeed(0); 
00246             air.solenoid1_close();
00247             air.solenoid2_close();
00248         
00249         }else{
00250             power = hypot(X0,Y0);
00251             theta = atan2(Y0,X0) + posi.getTheta();
00252             /**
00253              * ここまでテンプル
00254              */
00255             start_angle = posi.getTheta();
00256         
00257             if ( b3[8] == 1) original_angle -= 1.57;
00258             if ( b3[9] == 1) original_angle += 1.57;
00259             
00260             if ( b[3] ) original_angle = start_angle;
00261                                     
00262             now_angle = start_angle - original_angle;
00263             
00264             if ( now_angle > 3.14 ) {
00265                 now_angle -= 6.28;
00266             }
00267             if ( now_angle < -3.14 ) {
00268                 now_angle += 6.28;
00269             }
00270             
00271             angle.setProcessValue(now_angle);
00272             spin_power = -angle.compute();
00273             if(b[7] == 1){
00274                 power *= 1.0;
00275             }else{
00276                 power *= 0.3;
00277             }
00278             omni.computeCircular(power,theta,spin_power);
00279         
00280             for(int i = 0; i < 4; i++){
00281                 value[i] = omni.wheel[i];
00282                 LPF[i] = (1 - 0.8) * lastLPF[i] + 0.8 * value[i];
00283                 lastLPF[i] = LPF[i];
00284                 motor[i].setSpeed(LPF[i]);
00285 //            pc.printf("%f ", LPF[i]);
00286             }
00287             
00288         //}else{
00289 //            angle.setProcessValue(posi.getTheta());
00290 //            spin_power = -angle.compute();
00291 //            proto.Input_nowXY( posi.getX() , posi.getY() );
00292 //            proto.calculate();
00293 //            omni.computeXY(proto.getvalue_x(), proto.getvalue_y(), spin_power);
00294 //            
00295 //            for(int i = 0; i < 4; i++){
00296 //                value[i] = omni.wheel[i];
00297 //                motor[i].setSpeed(value[i]);
00298 ////                pc.printf("%f ", value[i]);
00299 //            }
00300 //            
00301         }
00302         
00303         
00304 //        wait(0.001);
00305 //        pc.printf(" loli:%d ",loli[2]);
00306 //        pc.printf(" %d %d case:%d ",mecha_num,towel_num,num);
00307 //        for(int i=0;i<12;i++) pc.printf(" %d ",b3[i]);
00308          //Wheel value
00309 //         pc.printf(" stickX:%f stickY:%f power:%f θ:%f",X0,Y0,power,theta);
00310 //        pc.printf(" vector:%f  pid:%.2f",proto.vector,spin_power);
00311          //Odmetry
00312 //        pc.printf("x:%d y:%d θ:%f ",posi.getX(),posi.getY(),posi.getTheta());
00313         
00314 //        pc.printf("\r\n");
00315     }
00316 }
00317