main.cpp

00001 //｢変更｣と書かれている場所の数値は、自分で決めて変える。（主にセンサー数値の補正）
00002 //気圧は基盤のミスのため、今回は使わない
00003 #include "mbed.h"
00004 #include "MPU6050.h"
00005 //#include "LPS.h"
00006 #include "SDFileSystem.h"
00007 #include "Servo.h"
00008 
00009 const double dt = 0.01; //微小時間(変更)
00010 
00011 const double Kp =11; //変更
00012 const double Ki =7; //変更
00013 const double Kd =5; //変更
00014 
00015 //const double k =0.25; //プロペラのための定数(今回使わない)
00016 
00017 const double mc = 3000; //最大トルク(変更)
00018 
00019 const double T1 = 90; //目標角度°(変更)
00020 const double T2 = -90; //目標角度°(変更)
00021 
00022 DigitalOut led[]={LED1,LED2,LED3,LED4};
00023 
00024 //I2C i2c(p28,p27);
00025 //LPS ps(i2c);
00026 MPU6050 mpu(p9,p10);
00027 SDFileSystem sd(p5, p6, p7, p8, "sd");
00028 Serial xbee(p13,p14);
00029 Serial pc(USBTX,USBRX);
00030 
00031 Servo myservo1(p21);
00032 Servo myservo2(p22);
00033 Servo myservo3(p23);
00034 Servo myservo4(p24);
00035 
00036 Timer ti;
00037 
00038 int gyro[3];
00039 int accel[3];
00040 
00041 int count;
00042 double GX,GY,GZ;
00043 double Tgx,Tgy,Tgz;
00044 
00045 int cmd;
00046 
00047 int main() {
00048     xbee.printf("motor start\r\n");
00049     led[0]=1;
00050     
00051     //モーター準備開始(一時待つ事が必要)
00052     myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
00053     wait(0.5);
00054     myservo1 = myservo2 = myservo3 = myservo4 = 1.0;
00055     wait(8);
00056     myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
00057     wait(8);
00058     xbee.printf("motor end\r\n");
00059     //モーター準備終了
00060     
00061     led[1]=1;
00062     //約5秒で開始
00063     wait(5);
00064     
00065     wait(1.0);
00066     led[0]=1;
00067     wait(1.0);
00068     led[0]=0;
00069 
00070     led[1]=0;
00071         xbee.printf("start\r\n");
00072         pc.printf("start\r\n");
00073     
00074         count = 0;
00075         GX = 0,GY = 0,GZ = 0;
00076         Tgx = 0,Tgy = 0,Tgz = 0;
00077         
00078         /*
00079         if (!ps.init()){
00080             pc.printf("Failed to autodetect pressure sensor!\r\n");
00081             while (1);
00082         }
00083      
00084         ps.enableDefault();
00085         */
00086         
00087         mkdir("/sd/test1", 0777);
00088         FILE *fp = fopen("/sd/test1/sdtest1.txt", "w");
00089     
00090         fprintf(fp,"start\r\n");
00091     
00092         ti.start();
00093         while(1){
00094             if(fp == NULL) {
00095             error("Could not open file for write\n");
00096             }
00097         
00098         
00099             //角度算出
00100             mpu.readGyroData(gyro);
00101             int gx = gyro[0]+420; //変更
00102             int gy = gyro[1]-1424+185+26-34+53; //変更
00103             int gz = gyro[2]; //変更
00104             //printf("%d %d %d\r\n",gx,gy,gz);
00105             
00106             double gX = gx*0.02562; //0.0128114995(測定レンジ±500) //変更
00107             double gY = gy*0.02562; //0.02562299(±250) //変更
00108             double gZ = gz*0.02562; //変更
00109          
00110             int gX1 = gX;
00111             int gY1 = gY;
00112             int gZ1 = gZ;
00113          
00114             double gX2 = gX1*dt;
00115             double gY2 = gY1*dt;
00116             double gZ2 = gZ1*dt;
00117          
00118             GX = GX + gX2;
00119             GY = GY + gY2;
00120             GZ = GZ + gZ2;
00121          
00122             Tgx = Tgx + abs(gX2);
00123             Tgy = Tgy + abs(gY2);
00124             Tgz = Tgz + abs(gZ2);
00125          
00126             //和の補正(精度はよくない)
00127             if(Tgx > 3){
00128                 if(GX > 0){
00129                     GX = GX - 0.3;//変更
00130                     }else{
00131                         GX = GX + 0.3;//変更
00132                     }
00133                 Tgx = 0;
00134                 }
00135             if(Tgy > 3){
00136                 if(GY >0){
00137                     GY = GY - 0.3;//変更
00138                     }else{
00139                         GY = GY + 0.3;//変更
00140                     } 
00141                 Tgy = 0;
00142                 }
00143             if(Tgz > 3){
00144                 if(GZ >0){
00145                     GZ = GZ - 0.3;//変更
00146                     }else{
00147                         GZ = GZ + 0.3;//変更
00148                         }
00149                     Tgz = 0;
00150                 }
00151             printf("%.2f %.2f %.2f\r\n",GX,GY,GZ);             
00152         
00153             //加速度算出
00154             mpu.readAccelData(accel);
00155             int ax = accel[0]-123; //変更
00156             int ay = accel[1]+60; //変更
00157             int az = accel[2]+1110 ; //変更
00158             float AX = ax*0.000597964111328125; //変更
00159             float AY = ay*0.000597964111328125; //変更
00160             float AZ = az*0.000597964111328125; //変更 
00161             double a = AX*AX+AY*AY+AZ*AZ-95.982071137936;
00162             
00163             /*
00164             //気圧・標高・温度算出
00165             float pres = ps.readPressureMillibars();
00166             float altit = ps.pressureToAltitudeMeters(pres);
00167             float tempe = ps.readTemperatureC();
00168             */
00169         
00170             //PID開始
00171             //必要なトルクを算出
00172             double c1,c2,c3,c4;
00173             double f1,f2,f3,f4;
00174             
00175             //GX
00176             f1 = T1 - GX;
00177             f2 = T2 - GX;
00178             //GY
00179             f3 = T1 - GY;
00180             f4 = T2 - GY;
00181             
00182             //GX
00183             c1 = Kp*f1+Ki*f1*dt-Kd*gX1; //トルク
00184             c2 = Kp*f2+Ki*f2*dt-Kd*gX1;
00185             //GY
00186             c3 = Kp*f3+Ki*f3*dt-Kd*gY1;
00187             c4 = Kp*f4+Ki*f4*dt-Kd*gY1;
00188             //printf("%.2f %.2f %.2f %.2f\r\n",c1,c2,c3,c4);　//トルク
00189         
00190             //回転トルク→duty比を算出
00191             double d1,d2,d3,d4;
00192             d1 = c1/mc;
00193             d2 = c2/mc;
00194             d3 = c3/mc;
00195             d4 = c4/mc;
00196             
00197             //出力の限界
00198             if(d1 > 0.8){ //変更
00199                 d1 = 0.8; //変更
00200                 }
00201             if(d2 > 0.8){ //変更
00202                 d2 = 0.8; //変更
00203                 }
00204             if(d3 > 0.8){ //変更
00205                 d3 = 0.8; //変更
00206                 }
00207             if(d4 > 0.8){ //変更
00208                 d4 = 0.8; //変更
00209                 }
00210                 
00211             //printf("%.2f %.2f %.2f %.2f\r\n",d1,d2,d3,d4); //duty比
00212             
00213             //試行回数
00214             count  = count + 1;
00215             
00216             //duty比からmotorを動かす。
00217             myservo1 = abs(d1);
00218             myservo2 = abs(d2);
00219             myservo3 = abs(d3);
00220             myservo4 = abs(d4);
00221             
00222             
00223             /* duty比が２つだけのとき(定数kを使用)
00224             if(GX > 0){
00225             myservo1 = abs(d1);
00226             myservo2 = abs(d2);//abs(d1)*k;;
00227             }else if(GX < 0){
00228                 myservo2 = abs(d2);
00229                 myservo1 = ; abs(d1); //abs(d1)*k;; //abs(d1)*k;
00230                 }else{
00231                     myservo1 = myservo2 = ;
00232                     }
00233                 
00234                 if(GY > 0){
00235                 myservo3 = abs(d2)+0.025;
00236                 myservo4 = 0.025; //abs(d2)*k; //abs(d2)*k;
00237                 }else if(GY < 0){
00238                 myservo4 = abs(d2)+0.025;
00239                 myservo3 = 0.025; //abs(d2)*k;
00240                 }else{
00241                     myservo3 = myservo4 = 0.025;
00242                     }
00243             */
00244             
00245             /*主なセンサー表示
00246             pc.printf("T %.2f\r\n",ti.read());
00247             pc.printf("c %d\r\n",count);
00248             pc.printf("ax%.2f ay%.2f az%.2f |a|%.2f\r\n",AX,AY,AZ,a);
00249             pc.printf("gx%.2f gy%.2f gz%.2f\r\n",GX,GY,GZ);
00250             pc.printf("p%.2f alt%.2f t%.2f\r\n",pres,altit,tempe);
00251             */
00252         
00253             pc.printf("c%d, T%.2f, ax%.2f,ay%.2f,az%.2f,|a|%.2f, gx%.2f,gy%.2f,gz%.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ);
00254             xbee.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",AX,AY,AZ,a,GX,GY,GZ);
00255             fprintf(fp, "%d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f\r\n",count,ti.read(),AX,AY,AZ,a,GX,GY,GZ,c1,c2,d1,d2);
00256             //fprintf(fp,"%.2f %.2f %.2f %.2f %.2f\r\n",ti.read(),c1,c2,d1,d2); //調整用
00257         
00258             wait(dt);
00259         
00260             if(AZ > 9 && AZ < 11 && a > 8 && a < 11 && 5 < ti.read()){
00261                 fprintf(fp,"end\r\n");
00262                 fclose(fp); //注意：ここまで行わないと、SDカードに保存されない
00263                 xbee.printf("end\r\n");
00264                 pc.printf("end\r\n");
00265                 ti.stop();
00266                 myservo1 = myservo2 = myservo3 = myservo4 = 0.0;
00267                 led[2]=1;
00268                 break;}
00269               
00270                 }
00271             
00272             //led[1]=0;
00273             //led[3]=1;
00274             
00275         }