cansat-d_2018
（多分）全部+フライトピン+新しい加速度
Dependencies: mbed
Diff: main.cpp
- Revision:
- 1:10af8aaa5b40
- Parent:
- 0:a01fda36fde8
- Child:
- 2:37d831f82840
--- a/main.cpp Wed Oct 10 14:10:51 2018 +0000
+++ b/main.cpp Mon Oct 15 10:38:06 2018 +0000
@@ -3,10 +3,16 @@
#include "ultrasonic.h"
#include "motordriver.h"
-Serial pc(USBTX, USBRX);
+DigitalOut FET(p21); //FET
+
+InterruptIn button1(p15); //フォトインタラプタ
+DigitalIn test(p15); //ここでピン15からの電圧の値、つまりフォトインタラプタが何か遮るものを検知すればハイの1を返して、
+ //何もないつまりスリットの部分ではローの0を返す。それを変数testに代入している
+
+Serial pc(USBTX, USBRX); //GPS
GPS gps(p28, p27);
-Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake
+Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake モーター
Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake
void dist(int distance)
@@ -15,43 +21,32 @@
printf("Distance changed to %dmm\r\n", distance);
}
-ultrasonic mu(p11, p12, .1, 1, &dist); //Set the trigger pin to D8 and the echo pin to D9
+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
-LocalFileSystem local("local"); // Create the local filesystem under the name "local"
+LocalFileSystem local("local"); // Create the local filesystem under the name "local" データ保存
int main() {
- /*FILE *fp = fopen("/local/gps.txt", "w"); // Open "out.txt" on the local file system for writing
- fprintf(fp, "GPS Start\r\n");
- int n;
- for(n=0;n<45;n++) //GPSの取得を45回行う
- {
- if(gps.getgps()) //現在地取得
- fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
-
- else
- fprintf(fp,"No data\r\n");//データ取得に失敗した場合
-
- wait(1);
- }
- fprintf(fp,"GPS finish\r\n");
- fclose(fp);*/
-
-
+
+ FET = 0; //FET、ニクロム線切断
+ wait(60);
+ FET = 1;
+ wait(30);
+ FET = 0;
+
motor1.stop(0);
motor2.stop(0);
FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
- fprintf(fp, "\r\n\GPS Start\r\n");
+ fprintf(fp, "GPS Start\r\n");
int n;
- for(n=0;n<45;n++) //GPSの取得を45回行う
+ for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半)
{
- printf("%d\n",n);
if(gps.getgps()) //現在地取得
fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
@@ -60,78 +55,123 @@
fprintf(fp,"No data\r\n");//データ取得に失敗した場合
wait(1);
+
+ printf("%d\r\n",n); //今何回目かを出力（本番ではいらない）
+
}
fprintf(fp,"GPS finish\r\n");
- fclose(fp); //GPSの測定終了
+ fclose(fp); //GPSの測定終了
mu.startUpdates();//start mesuring the distance(超音波センサー)
int distance;
while(1)
{
+ int flag; //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
+ //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
+ //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
+ float run; //変数runをフロートで型づけする
+
+ printf("%d\r\n", test.read());
+ if (test.read() == 1 and flag == 0){ //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
+ flag = 1; //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
+ }
+ else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
+ flag = 0; //まずこれでスイッチを0にして入れる。
+ //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
+ run += 52.5; //総距離runに52.5を加算する
+ }
+ /* if (run > 250){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+ break; //つまりゴールについたらこのループからぬける
+ }*/
- motor1.speed(0.5); //通常走行
+ motor1.speed(0.5); //通常走行
motor2.speed(0.5);
- //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.
+ //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.
distance=mu.getCurrentDistance();
+ wait(0.1);
-
printf("%d\r\n",distance);
- if(100<distance && distance < 500)
+ if(100<distance && distance < 500) //障害物発見❕
{
printf("if success!\r\n");
float ms1,ms2,msj1,msj2;
- ms1=0.5; //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず0.5にしておく
- ms2=0.5;
+ ms1=1.0; //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ
+ ms2=1.0;
- msj1=0.5; //回転の時
- msj2=0.5;
+ msj1=1.0; //回転の時
+ msj2=1.0;
motor1.stop(0);
motor2.stop(0);
- wait(1);
+ wait(2);
printf("mortor stop\r\n");
motor1.speed(msj1); //機体を時計回りに９０度回転
motor2.speed(-msj2);
- wait(1);
+ wait(2);
printf("mortor rotation\r\n");
motor1.speed(ms1); //直進
motor2.speed(ms2);
- wait(1);
+ wait(2);
printf("mortor straight\r\n");
motor1.speed(-msj1); //機体を反時計回りに９０度回転
motor2.speed(msj2);
- wait(1);
+ wait(2);
printf("mortor rotation\r\n");
motor1.speed(ms1); //直進
motor2.speed(ms2);
- wait(1);
+
+ printf("%d\r\n", test.read());
+ if (test.read() == 1 and flag == 0)
+ {
+ flag = 1;
+ }
+ else if (test.read() == 0 and flag == 1)
+ {
+ flag=0;
+ }
+
+ wait(2);
printf("mortor straight\r\n");
motor1.speed(-msj1); //機体を反時計回りに９０度回転
motor2.speed(msj2);
- wait(1);
+ wait(2);
printf("mortor rotation\r\n");
motor1.speed(ms1); //直進
motor2.speed(ms2);
- wait(1);
+ wait(2);
printf("mortor straight\r\n");
motor1.speed(msj1); //機体を時計回りに９０度回転
motor2.speed(-msj2);
- wait(1);
+ wait(2);
printf("mortor rotation\r\n");
}
}
}
+
+
+
+ float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
+ {
+ float c;
+ c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
+ return c;
+ }
+
+
+
+
+