cansat-d_2018
（多分）全部+フライトピン+新しい加速度
Dependencies: mbed
Diff: main.cpp
- Revision:
- 40:d0b0d2a26cac
- Parent:
- 39:a0c63ae63c99
--- a/main.cpp Sat Dec 15 17:10:52 2018 +0000
+++ b/main.cpp Sun Dec 16 02:11:48 2018 +0000
@@ -59,7 +59,7 @@
int main() {
printf("cansat start\r\n");
- /* flight==1;
+ flight==1;
FET = 0;
SW = 1; //p23をhigh(3.3V)にする。
while(1) {
@@ -88,18 +88,18 @@
break;
}
}
- }*/
+ }
motorStopR();
motorStopL();
- wait(1); //確認用//デフォ20秒
+ wait(20); //確認用//デフォ20秒
// FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
printf("GPS start\r\n");
FILE *fp = fopen("/local/gps,foto.txt", "w"); // Open "gps.txt" on the local file system for writing
- /* fprintf(fp, "GPS Start\r\n");
+ fprintf(fp, "GPS Start\r\n");
int n;
for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半)
{
@@ -119,9 +119,9 @@
// fclose(fp); // GPSの測定終了 */
motorSpeedR.period_ms(4); //モーター調節
- motorSpeedR = 0.255;
+ motorSpeedR = 0.555;
motorSpeedL.period_ms(4); //モーター調節
- motorSpeedL = 0.255;
+ motorSpeedL = 0.655;
compass.init(); //地磁気センサー動作
@@ -155,7 +155,7 @@
printf("compass carriblation\r\n"); //キャリブレーション終了
//float mcn1,mcn2;
-double heading,w;
+double heading,wx,wy,wz;
// mcn1=1.0;
// mcn2=1.0;
@@ -164,28 +164,28 @@
printf("right\r\n");
motorForwardL();//右回転
motorReverseR();
- w=(270-heading)*0.004448;
- wait(w); //角度のずれ*1度回転するのにかかる時間
+ wx=(270-heading)*0.004448;
+ wait(wx); //角度のずれ*1度回転するのにかかる時間
motorStopR();
motorStopL();
wait(1);
}
else if(0<=heading<=90.0){
printf("left\r\n");
- motorReverseL();//左回転
- motorForwardR();
- w=(heading+90.0)*0.004448;
- wait(w);
+ motorForwardL();//左回転
+ motorReverseR();
+ wy=(270-heading)*0.004448;
+ wait(wy);
motorStopR();
motorStopL();
wait(1);
}
else if(272.5<heading<360){
printf("left\r\n");
- motorReverseL();//左回転
- motorForwardR();
- w=(heading-270)*0.004448;
- wait(w);
+ motorForwardL();//左回転
+ motorReverseR();
+ wz=(360-heading)*0.004448;
+ wait(wz);
motorStopR();
motorStopL();
wait(1);
@@ -208,6 +208,7 @@
leftrun=0.0;
int accel[3];//accelを3つの配列で定義。*/
+ int gyro[3];
int tt=0;
float run=0;
@@ -220,9 +221,13 @@
printf("%d\r\n",distance);
mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
- int x = accel[0]-123;//x軸方向の加速度
- int y = accel[1]+60;//y軸方向の加速度
- int z = accel[2]+1110 ;//z軸方向の加速度
+ int x = accel[0];//x軸方向の加速度
+ int y = accel[1];//y軸方向の加速度
+ int z = accel[2] ;//z軸方向の加速度
+ mpu.readGyroData(gyro);//加速度の値をaccel[3]に代入
+ int gx = gyro[0];//x軸方向の加速度
+ int gy = gyro[1];//y軸方向の加速度
+ int gz = gyro[2];//z軸方向の加速度
printf("x=%d,y=%d,z=%d\r\n",x,y,z); //加速度の表示
@@ -234,8 +239,8 @@
}
else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
flag = 0; //まずこれでスイッチを0にして入れる。
- //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
- rightrun += 56.5625; //総距離runに52.5を加算する
+ //こうすることで同じスリットの中でtestが複数回0を返した時に何回も56.5mmを加算しつづけるということがなくなる
+ rightrun += 113; //総距離runに113mmを加算する(タイヤの全周452mm)
printf("test.read else\r\n");
}
if (test2.read() == 1 and flag2 == 0){
@@ -244,13 +249,16 @@
}
else if (test2.read() == 0 and flag2 == 1){
flag2 = 0;
- leftrun += 56.5625;
+ leftrun += 113;
printf("test2.read else\r\n");
}
- printf("%f %f", leftrun,leftrun/452.5);
- printf("\t%f %f\r\n", rightrun,rightrun/452.5);
+ printf("%f %g rot ", leftrun,leftrun/452);
+ printf("%f %g rot\r\n", rightrun,rightrun/452);
run=culculate_distance_3(rightrun,leftrun);
- if(5000< run && run<5050 && k!=1){//中間地点で北を向きなおす
+ if(run >= 30000){//もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+ break; //つまりゴールについたらこのループからぬける
+ }
+ else if(15000< run && run<15050 && k!=1){//中間地点で北を向きなおす
motorStopR();
motorStopL();
wait(1);
@@ -258,39 +266,43 @@
wait(2);
k=1;
+heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
if(90<heading<267.5){
+ printf("right\r\n");
motorForwardL();//右回転
motorReverseR();
- w=(270-heading)*0.004448;
- wait(w); //角度のずれ*1度回転するのにかかる時間
- motorStopR();
- motorStopL();
- wait(1);
-}else if(0<=heading<=90.0){
- motorReverseL();//左回転
- motorForwardR();
- w=(heading+90.0)*0.004448;
- wait(w);
+ wx=(270-heading)*0.004448;
+ wait(wx); //角度のずれ*1度回転するのにかかる時間
motorStopR();
motorStopL();
wait(1);
-}else if(272.5<heading<360){
- motorReverseL();//左回転
- motorForwardR();
- w=(heading-270)*0.004448;
- wait(w);
+}
+else if(0<=heading<=90.0){
+ printf("left\r\n");
+ motorForwardL();//左回転
+ motorReverseR();
+ wy=(270-heading)*0.004448;
+ wait(wy);
motorStopR();
motorStopL();
wait(1);
-}else{
+}
+else if(272.5<heading<360){
+ printf("left\r\n");
+ motorForwardL();//左回転
+ motorReverseR();
+ wz=(360-heading)*0.004448;
+ wait(wz);
+ motorStopR();
+ motorStopL();
+ wait(1);
+}
+else{
wait(5);
-
}
printf("searchN\r\n"); //機体が北を向く
}
- else if(run >= 10000){//もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
- break; //つまりゴールについたらこのループからぬける
- }
+
/* if(difference >= 0.3)
{
@@ -306,7 +318,7 @@
wait(0.01);
- fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z, leftrun, rightrun);//加速度とフォトインタラプタによる距離を出力
+ fprintf(fp,"%5d, %5d, %5d, %5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z,gx,gy,gz, leftrun, rightrun);//加速度とフォトインタラプタによる距離を出力
if(450 < distance && distance < 550) //障害物発見
{
@@ -327,7 +339,7 @@
motorForwardL(); //機体を時計回りに９０度回転
motorReverseR();
- wait(0.5);
+ wait(1);
printf("mortor rotation\r\n");
motorStopR();
@@ -337,7 +349,7 @@
motorForwardL(); //直進
motorForwardR();
- wait(2);
+ wait(3);
printf("mortor straight\r\n");
motorStopR();
@@ -347,7 +359,7 @@
motorReverseL(); //機体を反時計回りに９０度回転
motorForwardR();
- wait(0.5);
+ wait(1);
printf("mortor rotation\r\n");
motorStopR();
@@ -371,14 +383,14 @@
else if (test.read() == 0 and flag == 1)
{
flag=0;
- rightrun += 56.5625;
+ rightrun += 113;
}
if (test2.read() == 1 and flag2 == 0){
flag2 = 1;
}
else if (test2.read() == 0 and flag2 == 1){
flag2 = 0;
- leftrun += 56.5625;
+ leftrun += 113;
}
printf("%f", leftrun);
printf("\t%f\r\n", rightrun);
@@ -386,12 +398,16 @@
mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
- int x = accel[0] - 123;//x軸方向の加速度
- int y = accel[1] + 60;//y軸方向の加速度
- int z = accel[2] + 1110 ;//z軸方向の加速度
- fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z, leftrun, rightrun);//加速度とフォトインタラプタによる距離を出力
+ mpu.readGyroData(gyro);
+ int x = accel[0];
+ int y = accel[1];
+ int z = accel[2];
+ int gx = gyro[0];
+ int gy = gyro[1];
+ int gz = gyro[2];
+ fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf, %5d, %5d, %5d\r\n", x, y, z, leftrun, rightrun,gx, gy ,gz);//加速度とフォトインタラプタによる距離を出力
- if (run >= 10000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+ if (run >= 300000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
break; //つまりゴールについたらこのループからぬける
}
@@ -410,7 +426,7 @@
motorReverseL(); //機体を反時計回りに９０度回転
motorForwardR();
- wait(0.5);
+ wait(1);
printf("mortor rotation\r\n");
motorStopR();
@@ -430,7 +446,7 @@
motorForwardL(); //機体を時計回りに９０度回転
motorReverseR();
- wait(0.5);
+ wait(1);
printf("mortor rotation\r\n");
motorStopR();
@@ -438,28 +454,38 @@
wait(2);
printf("mortor stop\r\n");
+heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
if(90<heading<267.5){
+ printf("right\r\n");
motorForwardL();//右回転
motorReverseR();
- wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
+ wx=(270-heading)*0.004448;
+ wait(wx); //角度のずれ*1度回転するのにかかる時間
motorStopR();
motorStopL();
wait(1);
-}else if(0<=heading<=90.0){
- motorReverseL();//左回転
- motorForwardR();
- wait((heading+90.0)*0.004448);
+}
+else if(0<=heading<=90.0){
+ printf("left\r\n");
+ motorForwardL();//左回転
+ motorReverseR();
+ wy=(270-heading)*0.004448;
+ wait(wy);
motorStopR();
motorStopL();
wait(1);
-}else if(272.5<heading<360){
- motorReverseL();//左回転
- motorForwardR();
- wait((heading-270)*0.004448);
+}
+else if(272.5<heading<360){
+ printf("left\r\n");
+ motorForwardL();//左回転
+ motorReverseR();
+ wz=(360-heading)*0.004448;
+ wait(wz);
motorStopR();
motorStopL();
wait(1);
-}else{
+}
+else{
wait(5);
}
printf("searchN\r\n"); //機体が北を向く