cansat-d_2018
otamesi
Dependencies: mbed
Diff: main.cpp
- Revision:
- 27:b4922048ab11
- Parent:
- 23:2ca79873ef44
- Child:
- 28:5e21ce413558
--- a/main.cpp Fri Dec 07 06:32:14 2018 +0000
+++ b/main.cpp Tue Dec 11 10:23:35 2018 +0000
@@ -5,6 +5,11 @@
#include "HMC5883L.h"
#include "MPU6050.h"
+
+PwmOut motorSpeed(p26);
+DigitalOut motorDir1(p19);
+DigitalOut motorDir2(p20);
+
Serial pc(USBTX, USBRX); //地磁気センサー,GPS
GPS gps(p28, p27); //GPS
@@ -34,15 +39,23 @@
ultrasonic mu(p11, p12, .1, 1, &dist); //Set the trigger pin to D8 and the echo pin to D9 超音波センサー
//have updates every .1 seconds and a timeout after 1
//second, and call dist when the distance changes
+
+void motorForward(void);
+void motorStop(void);
+void motorReverse(void);
LocalFileSystem local("local"); // Create the local filesystem under the name "local" データ保存
+float culculate_distance_3(float a,float b);
+
int main() {
- FET = 0;
- while(1) {
+ printf("cansat start\r\n");
+
+ // FET = 0;
+ /* while(1) {
if(flight==1) {
wait(10);
}
@@ -59,15 +72,15 @@
FET = 0;
break;
}
- }
+ } */
motor1.stop(0);
motor2.stop(0);
- wait(30); //確認用
+ wait(20); //確認用
printf("GPS start\r\n");
- FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
+ FILE *fp = fopen("/local/gps,foto.txt", "w"); // Open "gps.txt" on the local file system for writing
fprintf(fp, "GPS Start\r\n");
int n;
for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半)
@@ -84,11 +97,8 @@
printf("%d\r\n",n); //今何回目かを出力（本番ではいらない）
}
- fprintf(fp,"GPS finish\r\n");
+ fprintf(fp,"GPS finish\r\n");
// fclose(fp); // GPSの測定終了
-
-
-
compass.init(); //地磁気センサー動作
@@ -163,7 +173,13 @@
float oldAccel = 0;//ひとつ前の加速度
float difference=0;//変位
float timespan=0.01;//時間差
- int accel[3];//accelを3つの配列で定義。
+ int accel[3];//accelを3つの配列で定義。*/
+
+ int tt=0;
+ float run=0;
+
+ motorSpeed.period_ms(2); //モーター調節
+ motorSpeed = 0.9;
while(1)
{
@@ -200,7 +216,7 @@
oldSpeed = speed;
//printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
- printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示
+ printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
printf("%d\r\n", test.read()); //フォトインタラプタ
printf("%d\r\n", test2.read());
@@ -225,23 +241,33 @@
}
printf("%f", rightrun);
printf("\t%f\r\n", leftrun2);
- if (rightrun >= 4396){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+ run=culculate_distance_3(rightrun,leftrun2);
+ if (run >= 4396){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
break; //つまりゴールについたらこのループからぬける
}
- if(difference >= 0.3)
+ /* if(difference >= 0.3)
{
break;
- }
+ } */
motor1.speed(0.5); //通常走行
- motor2.speed(0.5);
+ motorForward();
//Do something else here
// mu.checkDistance(); //call checkDistance() as much as possible, as this is where
//the class checks if dist needs to be called.
wait(0.01);
+ tt++;
+ if(tt==10)
+ {
+ fprintf(fp, "accel.rightrun.leftrun2\r\n");
+
+ fprintf(fp,"(%lf, %lf,%lf)\r\n", difference, rightrun, leftrun2);//加速度とフォトインタラプタによる距離を出力
+ tt=0;
+ }
+
if(100 < distance && distance < 500) //障害物発見❕
{
@@ -260,7 +286,7 @@
printf("mortor stop\r\n");
motor1.speed(msj1); //機体を時計回りに９０度回転
- motor2.speed(-msj2);
+ motorForward();
wait(0.77);
printf("mortor rotation\r\n");
@@ -270,7 +296,7 @@
printf("mortor stop\r\n");
motor1.speed(ms1); //直進
- motor2.speed(ms2);
+ motorForward();
wait(2);
printf("mortor straight\r\n");
@@ -280,7 +306,7 @@
printf("mortor stop\r\n");
motor1.speed(-msj1); //機体を反時計回りに９０度回転
- motor2.speed(msj2);
+ motorForward();
wait(0.77);
printf("mortor rotation\r\n");
@@ -290,11 +316,11 @@
printf("mortor stop\r\n");
motor1.speed(ms1); //直進
- motor2.speed(ms2);
+ motorForward();
int t=0;
- for(t=0;t<100;t++)
+ for(t=0;t<50;t++)
{
printf("%d\r\n", test.read());
printf("%d\r\n", test2.read());
@@ -349,9 +375,14 @@
oldSpeed = speed;
//printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
- printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示
+ printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
+
+ if (run >= 4396){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+ break; //つまりゴールについたらこのループからぬける
+ }
wait(0.01);
+
}
@@ -364,7 +395,7 @@
printf("mortor stop\r\n");
motor1.speed(-msj1); //機体を反時計回りに９０度回転
- motor2.speed(msj2);
+ motorForward();
wait(1);
printf("mortor rotation\r\n");
@@ -374,7 +405,7 @@
printf("mortor stop\r\n");
motor1.speed(ms1); //直進
- motor2.speed(ms2);
+ motorForward();
wait(2);
printf("mortor straight\r\n");
@@ -384,7 +415,7 @@
printf("mortor stop\r\n");
motor1.speed(msj1); //機体を時計回りに９０度回転
- motor2.speed(-msj2);
+ motorForward();
wait(1);
printf("mortor rotation\r\n");
@@ -401,7 +432,9 @@
fprintf(fp, "last accel.photo\r\n");
- fprintf(fp,"(%lf, %lf)\r\n", difference, rightrun);//最後の加速度とフォトインタラプタによる距離を出力
+ fprintf(fp,"(%lf, %lf)\r\n", difference, run);//最後の加速度とフォトインタラプタによる距離を出力
+ fprintf(fp, "last right.left\r\n");
+ fprintf(fp,"(%lf, %lf)\r\n", rightrun, leftrun2);
fclose(fp);
}
@@ -414,8 +447,23 @@
c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
return c;
}
+void motorForward() {
+ motorStop();
+ motorDir1 = 1;
+ motorDir2 = 0;
+}
+
+void motorReverse() {
+ motorStop();
+ motorDir1 = 0;
+ motorDir2 = 1;
+}
+
+void motorStop() {
+ motorDir1 = 0;
+ motorDir2 = 0;
+}
-