2021 nhk A team
Dependencies: mbed QEI led beep softPWM Servo_softpwm IR2302 lpf
main.cpp
00001 //2021/10/23 紐掴むだけ 00002 00003 #include "mbed.h" 00004 00005 #include "jy901.h" 00006 #include "Servo.h" 00007 #include "QEI.h" 00008 #include "IR2302.h" 00009 00010 #include "pin_config.h" 00011 #include "control_parameter.h" 00012 00013 #include "lpf.h" 00014 #include "beep.h" 00015 00016 Serial pc(USBTX,USBRX,115200); 00017 00018 Beep buzzer(BEEP); 00019 JY901 jy(SDA, SCL); 00020 //DigitalOut stop(STOP); 00021 00022 QEI encoder(QEI1 ,QEI2 ,NC ,100 ,QEI::X4_ENCODING); 00023 00024 lpf yAcc(LPF_C_TIME, LPF_A_TIME); 00025 lpf zAcc(LPF_C_TIME, LPF_A_TIME); 00026 lpf mSp(MOT_C_TIME, MOT_A_TIME); 00027 Servo servo[3] = {SERVO1, SERVO2, SERVO3}; 00028 IR2302 ir2302(MD_A,MD_B);//梅沢追加 IR2302 00029 Timer setWait,nextWait,jumpWait,buzzerTime; 00030 AnalogIn ph[3] = {PHOTO1,PHOTO2,PHOTO3}; 00031 DigitalOut light[3] = {ROPE1,ROPE2,ROPE3}; 00032 DigitalIn ub(USER_BUTTON); 00033 DigitalIn ab(PA_15); 00034 00035 int main() 00036 { 00037 int i=0,j=0,k=3,l=0;//0,0,2,0 00038 double acc[3]; //加速度 00039 double abac[3]={0};//初期座標軸での加速度 00040 double angle[3]; 00041 float lpfY,lpfZ; //ローパス後の加速度 00042 float lpfM; //モータのローパス 00043 jy.calibrateAll(50); 00044 00045 int nowPalse; 00046 int zeroPalse=0; 00047 00048 float phI[3] = {0};//フォトインタラプタ 00049 bool rope = 0; 00050 00051 float freq = 3000;//ブザーのやつ 00052 00053 bool setFlag = 1;//1 00054 bool firstFlag = 0;//0 00055 bool secondFlag= 0;//0 00056 bool thirdFlag = 0;//0 00057 bool swingFlag = 1;//1 00058 bool flyFlag = 0;//0 00059 bool catchFlag = 0;//0 00060 int8_t way = 1;//1or-1 00061 bool resetFlag = 0;//0 00062 bool buzzerFlag= 0;//0 00063 float fiveYAcc[5]={0}; 00064 00065 float servoAngle[3]; 00066 float motor=0; 00067 00068 for(int i=0;i < 3; i++)servo[i].calibrate(0.00095, 90); 00069 servoAngle[0] = 0.2; 00070 servoAngle[1] = 0.2; 00071 servoAngle[2] = 0.8; 00072 for(i=0;i<3;i++) servo[i].write(servoAngle[i]); 00073 for(i=0; i<3; i++) light[i] = true; 00074 00075 ub.mode(PullDown); 00076 00077 while(1) 00078 { 00079 //センシング 00080 /* 00081 //jy901 00082 acc[1] = jy.getYaxisAcceleration()*10; 00083 acc[2] = jy.getZaxisAcceleration()*10; 00084 angle[0] = jy.getXaxisAngle(); 00085 angle[1] = jy.getYaxisAngle(); 00086 angle[2] = jy.getZaxisAngle(); 00087 for(i=0; i<3; i++) 00088 { 00089 if(acc[i] > 1567)acc[i] -= 3134; 00090 } 00091 abac[1] = acc[1]*cos(angle[0]/180.0*PI) - acc[2] * sin(angle[0]/180.0*PI); 00092 abac[2] = acc[2]*cos(angle[0]/180.0*PI) + acc[1] * sin(angle[0]/180.0*PI); 00093 00094 //ローパスフィルタ 00095 lpfY = yAcc.path_value(abac[1]); 00096 lpfZ = zAcc.path_value(abac[2]); 00097 00098 //swingで使うやつ 00099 fiveYAcc[j%5]= lpfY; 00100 j++; 00101 */ 00102 //ロータリーエンコーダ 向き変ったので-1掛け 00103 nowPalse = (encoder.getPulses() + zeroPalse) * -1; 00104 00105 //紐センサ 00106 for(i=0; i<3; i++) phI[i]=ph[i]; 00107 00108 //処理部 00109 00110 // buzzerFlag = 0; 00111 00112 //セッティング 00113 if(setFlag) 00114 { 00115 if(!ub && !ab) motor=0.25; 00116 else if(ab && ub) motor=-0.25; 00117 else if(!ab && ub) motor=0; 00118 else if(!ub && ab) 00119 { 00120 motor = 0; 00121 setFlag = 0; 00122 firstFlag = 1; 00123 buzzer.beep(freq,1.0); 00124 // buzzerFlag=1; 00125 zeroPalse = nowPalse; 00126 wait(5); 00127 buzzer.nobeep(); 00128 // buzzerFlag = 0; 00129 } 00130 00131 } 00132 00133 //加速1段目 00134 if(firstFlag) 00135 { 00136 motor = FIRST_POWER * way * swingFlag; //振り子を振る、wayで反転を制御 00137 motor = mSp.path_value(motor); 00138 if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら(少し間をおいて)反転 00139 { 00140 way *= -1; 00141 nextWait.start(); 00142 setWait.start(); 00143 // buzzerFlag=1; 00144 swingFlag = 0; 00145 if(nextWait > SECOND_LINE) //一定時間経過したらsecondに移行 00146 { 00147 firstFlag = 0; 00148 secondFlag = 1; 00149 swingFlag = 0; 00150 nextWait.stop(); 00151 nextWait.reset(); 00152 } 00153 } 00154 if(setWait > FIRST_WAIT) 00155 { 00156 setWait.stop(); 00157 setWait.reset(); 00158 swingFlag = 1; 00159 } 00160 } 00161 00162 //加速2段目 00163 if(secondFlag) 00164 { 00165 motor = SECOND_POWER * way * swingFlag; //振り子を振る、wayで反転を制御 00166 nextWait.start(); 00167 if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら(少し間をおいて)反転 00168 { 00169 way *= -1; 00170 setWait.start(); 00171 swingFlag = 0; 00172 // buzzerFlag = 1; 00173 if(nextWait > THIRD_LINE) //一定時間経過したらthirdに移行 00174 { 00175 secondFlag = 0; 00176 thirdFlag = 1; 00177 swingFlag = 0; 00178 nextWait.stop(); 00179 nextWait.reset(); 00180 } 00181 } 00182 if(setWait > SECOND_WAIT) 00183 { 00184 setWait.stop(); 00185 setWait.reset(); 00186 swingFlag = 1; 00187 } 00188 } 00189 00190 //加速3段目 00191 if(thirdFlag) 00192 { 00193 jumpWait.start(); 00194 motor = THIRD_POWER * way * swingFlag; //振り子を振る、wayで反転を制御 00195 if(nowPalse * way > FURIKO_STOP_LINE) //エンコーダーの値が一定以上なら(少し間をおいて)反転 00196 { 00197 way *= -1; 00198 nextWait.start(); 00199 setWait.start(); 00200 swingFlag = 0; 00201 // buzzerFlag = 1; 00202 } 00203 if(setWait > THIRD_WAIT) 00204 { 00205 setWait.stop(); 00206 setWait.reset(); 00207 swingFlag = 1; 00208 } 00209 /* 00210 if(jumpWait > JUMP_TIME) 00211 { 00212 buzzerFlag = 1; 00213 freq = 5000; 00214 } 00215 */ 00216 if((jumpWait > JUMP_TIME) && (nowPalse < JUMP_LINE) && (nowPalse > JUMP_LINE-10) && (way == -1)) //一定時間たったら前に飛ぶ 00217 { 00218 flyFlag = 1; 00219 thirdFlag = 0; 00220 } 00221 } 00222 00223 00224 //ジャンプ 00225 if(flyFlag) 00226 { 00227 // if(lpfZ < RELEASE_LINE) 00228 { 00229 servoAngle[(k-1)%3] = 0.1;//サーボ離す 00230 servoAngle[k%3] = 0.7; 00231 // l++; 00232 l=1; 00233 flyFlag = 0; 00234 catchFlag=1; 00235 } 00236 motor=0; 00237 00238 } 00239 00240 00241 //キャッチ 00242 if(catchFlag) 00243 { 00244 if(phI[k%3] < ROPE_ACRROS[k%3]) 00245 { 00246 catchFlag=0; 00247 motor = 0; 00248 resetFlag=1; 00249 k++; 00250 } 00251 00252 } 00253 00254 //掴んだ後 00255 if(resetFlag) 00256 { 00257 if(nowPalse < NEXT_PALSE) 00258 { 00259 motor=0.35; 00260 } 00261 else 00262 { 00263 motor=0; 00264 resetFlag = 0; 00265 // firstFlag = 1; 00266 } 00267 } 00268 00269 //実験用記述----------- 00270 /* 00271 if(flyFlag) 00272 { 00273 // if(lpfZ < RELEASELINE) 00274 { 00275 for(i=0; i < 3; i++) servoAngle[i] = -1.0;//サーボ離す 00276 // k++; 00277 flyFlag = 0; 00278 } 00279 motor=0; 00280 } 00281 */ 00282 /* 00283 for(i=0; i<3; i++) 00284 { 00285 if(phI[i] < ROPE_ACRROS[i]) servoAngle[i] = -0.8; 00286 00287 } 00288 */ 00289 /* 00290 if(phI[k%3] < ROPE_ACRROS[k%3]) 00291 { 00292 servoAngle[k%3] = 0.85; 00293 servoAngle[(k+1)%3] = 0.4; 00294 k++; 00295 } 00296 */ 00297 // if(!b) motor=0.3; 00298 // else motor=0; 00299 00300 //------------------- 00301 00302 //駆動系 00303 00304 //サーボ 00305 for(i=0;i<3;i++) servo[i].write(servoAngle[i]); 00306 00307 //モーター 00308 ir2302.SetMotorSpeed(motor); 00309 /* 00310 //ブザー 00311 if(buzzerFlag) 00312 { 00313 buzzer.beep(freq,0.5); 00314 } 00315 else 00316 { 00317 buzzer.nobeep(); 00318 } 00319 */ 00320 //表示系 00321 // pc.printf("%.2f,%.2f,%.2f",angle[0],angle[1],angle[2]); 00322 // pc.printf("%.2f,%.2f,%.2f,",servoAngle[0],servoAngle[1],servoAngle[2]); 00323 // pc.printf("%.4f,",motor); 00324 // pc.printf("%.2f,%.2f,",abac[1],abac[2]); 00325 // pc.printf("%.2f,%.2f,",lpfY,lpfZ); 00326 pc.printf("%d,",nowPalse); 00327 // pc.printf("%d,",setFlag); 00328 // pc.printf("%d,",resetFlag); 00329 // pc.printf("%.2f,",lpfM); 00330 pc.printf("%.3f,%.3f,%.3f,",phI[0],phI[1],phI[2]); 00331 // pc.printf("%d,",k); 00332 pc.printf("\r\n"); 00333 00334 } 00335 } 00336 00337
Generated on Thu Jul 21 2022 18:57:01 by
1.7.2