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
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
Generated on Tue Aug 30 2022 00:05:53 by
1.7.2