2021 nhk A team

Dependencies:   mbed QEI led beep softPWM Servo_softpwm IR2302 lpf

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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