cansat-d_2018
otamesi
Dependencies: mbed
Diff: main.cpp
- Revision:
- 35:40df2f91cea9
- Parent:
- 34:0e8f95f07612
- Child:
- 36:c2b4071970de
--- a/main.cpp Fri Dec 14 06:53:50 2018 +0000
+++ b/main.cpp Fri Dec 14 14:57:28 2018 +0000
@@ -6,9 +6,12 @@
#include "MPU6050.h"
-PwmOut motorSpeed(p26);
-DigitalOut motorDir1(p19);
-DigitalOut motorDir2(p20);
+PwmOut motorSpeed1(p26);
+PwmOut motorSpeed2(p25);
+DigitalOut motor1Dir1(p19);
+DigitalOut motor1Dir2(p20);
+DigitalOut motor2Dir1(p16);
+DigitalOut motor2Dir2(p17);
Serial pc(USBTX, USBRX); //地磁気センサー,GPS
@@ -45,9 +48,11 @@
void motorStop(void);
void motorReverse(void);
-
+void motorForward2(void);
+void motorStop2(void);
+void motorReverse2(void);
-LocalFileSystem local("local"); // Create the local filesystem under the name "local" データ保存
+LocalFileSystem local("local"); // Create the local filesystem under the name "local" データ保存
float culculate_distance_3(float a,float b);
@@ -67,7 +72,7 @@
wait(10);
}
else{
- /* FET = 0; //FET、ニクロム線切断
+ FET = 0; //FET、ニクロム線切断
wait(20);
FET = 1;
wait(12);
@@ -75,7 +80,7 @@
wait(10);
FET = 1;
wait(12);
- FET = 0; */
+ FET = 0;
SW = 0; //p23をlow(0V)にする。
break;
}
@@ -87,9 +92,9 @@
wait(20); //確認用
-// FILE *fp = fopen("/local/gps,foto.txt", "w"); // Open "gps.txt" on the local file system for writing
+// FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
- printf("GPS start\r\n");
+ 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");
int n;
@@ -108,11 +113,12 @@
}
fprintf(fp,"GPS finish\r\n");
- // fclose(fp); // GPSの測定終了 */
+ // fclose(fp); // GPSの測定終了 */
- motorSpeed.period_ms(4); //モーター調節
- motorSpeed = 0.955;
-
+ motorSpeed1.period_ms(4); //モーター調節
+ motorSpeed1 = 0.955;
+ motorSpeed2.period_ms(4); //モーター調節
+ motorSpeed2 = 0.955;
compass.init(); //地磁気センサー動作
@@ -128,7 +134,7 @@
mc1=1.0;
mc2=1.0;
//地磁気センサのキャリブレーション
- motor1.speed(mc1); //車体を時計回りに３秒回転
+ motorForward2(); //車体を時計回りに３秒回転
motorReverse();
wait(1.6);
@@ -136,7 +142,7 @@
motor2.stop(0);
wait(1);
- motor1.speed(-mc1); //車体を反時計回りに３秒回
+ motorReverse2(); //車体を反時計回りに３秒回
motorForward();
wait(1.6);
@@ -152,21 +158,21 @@
compass.init();
heading=compass.getHeadingXYDeg();
if(90<heading<267.5){
- motor1.speed(mcn1);//右回転
+ motorForward2();//右回転
motorReverse();
wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
motor1.stop(0);
motor2.stop(0);
wait(1);
}else if(0<=heading<=90.0){
- motor1.speed(-mcn1);//左回転
+ motorReverse2();//左回転
motorForward();
wait((heading+90.0)*0.004448);
motor1.stop(0);
motor2.stop(0);
wait(1);
}else if(272.5<heading<360){
- motor1.speed(-mcn1);//左回転
+ motorReverse2();//左回転
motorForward();
wait((heading-270)*0.004448);
motor1.stop(0);
@@ -187,19 +193,13 @@
float leftrun2;
rightrun=0.0;
leftrun2=0.0;
-
- /* float filterCoefficient = 0.9; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。
- float lowpassValue = 0;
- float highpassValue = 0;
- float speed = 0;//加速度時から算出した速度
- float oldSpeed = 0;//ひとつ前の速度
- float oldAccel = 0;//ひとつ前の加速度
- float difference=0;//変位
- float timespan=0.01;//時間差*/
+
int accel[3];//accelを3つの配列で定義。*/
int tt=0;
float run=0;
+ fprintf(fp, "x,y,z,lefttrun2,rightrun\r\n");
+
while(1)
{
@@ -210,35 +210,8 @@
int x = accel[0]-123;//x軸方向の加速度
int y = accel[1]+60;//y軸方向の加速度
int z = accel[2]+1110 ;//z軸方向の加速度
- /* float X = x*0.000597964111328125;
- float Y = y*0.000597964111328125;
- float Z = z*0.000597964111328125;
- /* double a = X*X+Y*Y+Z*Z-95.982071137936;
- if (a>0){
- a = sqrt(a);
- }
- if (a<0) {
- a = abs(a);
- a = -sqrt(a);
- }
- //printf("%lf %f %f %f\r\n",a,X,Y,Z);
- // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
- lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient);
- // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
- highpassValue = a - lowpassValue;
- // 速度計算(加速度を台形積分する)
- speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
- oldAccel = highpassValue;
-
- // 変位計算(速度を台形積分する)
- difference = ((speed + oldSpeed) * timespan) / 2 + difference;
- oldSpeed = speed;*/
-
- //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
- /* printf("speed %f diference %f\r\n",speed,difference);//速度（m）と変位を表示*/
-
- printf("x=%f,y=%f,z=%f\r\n",x,y,z);
+ printf("x=%f,y=%f,z=%f\r\n",x,y,z); //加速度の表示
printf("%d\r\n", test.read()); //フォトインタラプタ
printf("%d\r\n", test2.read());
@@ -261,29 +234,29 @@
leftrun2 += 56.5625;
printf("test2.read else\r\n");
}
- printf("%f", rightrun);
- printf("\t%f\r\n", leftrun2);
+ printf("%f", leftrun2);
+ printf("\t%f\r\n", rightrun);
run=culculate_distance_3(rightrun,leftrun2);
- if (15000<run && run<15050){ //半分くらい進んだら方位を計測し直す
+ if (5000<run && run<5050){ //半分くらい進んだら方位を計測し直す
motor1.stop(0);
motor2.stop(0);
wait(1);
if(90<heading<267.5){
- motor1.speed(mcn1);//右回転
+ motorForward2();//右回転
motorReverse();
wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
motor1.stop(0);
motor2.stop(0);
wait(1);
}else if(0<=heading<=90.0){
- motor1.speed(-mcn1);//左回転
+ motorReverse2();//左回転
motorForward();
wait((heading+90.0)*0.004448);
motor1.stop(0);
motor2.stop(0);
wait(1);
}else if(272.5<heading<360){
- motor1.speed(-mcn1);//左回転
+ motorReverse2();//左回転
motorForward();
wait((heading-270)*0.004448);
motor1.stop(0);
@@ -294,7 +267,7 @@
}
printf("searchN\r\n"); //機体が北を向く
}
- else if (run >= 30000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+ else if (run >= 10000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
break; //つまりゴールについたらこのループからぬける
}
@@ -303,7 +276,7 @@
break;
} */
- motor1.speed(0.5); //通常走行
+ motorForward2(); //通常走行
motorForward();
//Do something else here
// mu.checkDistance(); //call checkDistance() as much as possible, as this is where
@@ -311,14 +284,8 @@
wait(0.01);
- tt++;
- if(tt == 100)
- {
- fprintf(fp, "accel.rightrun.leftrun2\r\n");
- fprintf(fp,"(%d, %d, %d, %d,%d)\r\n", x, y, z, rightrun, leftrun2);//加速度とフォトインタラプタによる距離を出力
- tt=0;
- }
+ fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z, leftrun2, rightrun);//加速度とフォトインタラプタによる距離を出力
if(100 < distance && distance < 500) //障害物発見❕
{
@@ -337,7 +304,7 @@
wait(2);
printf("mortor stop\r\n");
- motor1.speed(msj1); //機体を時計回りに９０度回転
+ motorForward2(); //機体を時計回りに９０度回転
motorReverse();
wait(0.5);
printf("mortor rotation\r\n");
@@ -347,7 +314,7 @@
wait(2);
printf("mortor stop\r\n");
- motor1.speed(ms1); //直進
+ motorForward2(); //直進
motorForward();
wait(2);
printf("mortor straight\r\n");
@@ -357,7 +324,7 @@
wait(2);
printf("mortor stop\r\n");
- motor1.speed(-msj1); //機体を反時計回りに９０度回転
+ motorReverse2(); //機体を反時計回りに９０度回転
motorForward();
wait(0.5);
printf("mortor rotation\r\n");
@@ -367,7 +334,7 @@
wait(2);
printf("mortor stop\r\n");
- motor1.speed(ms1); //直進
+ motorForward2(); //直進
motorForward();
@@ -392,8 +359,8 @@
flag2 = 0;
leftrun2 += 56.5625;
}
- printf("%f", rightrun);
- printf("\t%f\r\n", leftrun2);
+ printf("%f", leftrun2);
+ printf("\t%f\r\n", rightrun);
@@ -404,9 +371,8 @@
/* float X = x*0.000597964111328125;
float Y = y*0.000597964111328125;
float Z = z*0.000597964111328125; */
- fprintf(fp, "accel.rightrun.leftrun2\r\n");
+ fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z, leftrun2, rightrun);//加速度とフォトインタラプタによる距離を出力
- fprintf(fp,"(%d, %d, %d, %d,%d)\r\n", x, y, z, rightrun, leftrun2);//加速度とフォトインタラプタによる距離を出力
/* double a = X*X+Y*Y+Z*Z-95.982071137936;
if (a>0){
a = sqrt(a);
@@ -430,9 +396,9 @@
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 >= 300000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+ if (run >= 10000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
break; //つまりゴールについたらこのループからぬける
}
@@ -449,7 +415,7 @@
wait(2);
printf("mortor stop\r\n");
- motor1.speed(-msj1); //機体を反時計回りに９０度回転
+ motorReverse2(); //機体を反時計回りに９０度回転
motorForward();
wait(0.5);
printf("mortor rotation\r\n");
@@ -459,7 +425,7 @@
wait(2);
printf("mortor stop\r\n");
- motor1.speed(ms1); //直進
+ motorForward2(); //直進
motorForward();
wait(2);
printf("mortor straight\r\n");
@@ -469,7 +435,7 @@
wait(2);
printf("mortor stop\r\n");
- motor1.speed(msj1); //機体を時計回りに９０度回転
+ motorForward2(); //機体を時計回りに９０度回転
motorReverse();
wait(0.5);
printf("mortor rotation\r\n");
@@ -480,21 +446,21 @@
printf("mortor stop\r\n");
if(90<heading<267.5){
- motor1.speed(mcn1);//右回転
+ motorForward2();//右回転
motorReverse();
wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
motor1.stop(0);
motor2.stop(0);
wait(1);
}else if(0<=heading<=90.0){
- motor1.speed(-mcn1);//左回転
+ motorReverse2();//左回転
motorForward();
wait((heading+90.0)*0.004448);
motor1.stop(0);
motor2.stop(0);
wait(1);
}else if(272.5<heading<360){
- motor1.speed(-mcn1);//左回転
+ motorReverse2();//左回転
motorForward();
wait((heading-270)*0.004448);
motor1.stop(0);
@@ -511,11 +477,12 @@
motor2.stop(0);
- fprintf(fp, "last accel.photo\r\n");
+ fprintf(fp, "last photo\r\n");
fprintf(fp,"(%lf)\r\n", run);//最後の加速度とフォトインタラプタによる距離を出力
- fprintf(fp, "last right.left\r\n");
- fprintf(fp,"(%lf, %lf)\r\n", rightrun, leftrun2);
+ fprintf(fp, "last left.right\r\n");
+ fprintf(fp,"(%lf, %lf)\r\n", leftrun2, rightrun);
fclose(fp);
+
}
@@ -529,19 +496,36 @@
}
void motorForward() {
motorStop();
- motorDir1 = 1;
- motorDir2 = 0;
+ motor1Dir1 = 1;
+ motor1Dir2 = 0;
}
void motorReverse() {
motorStop();
- motorDir1 = 0;
- motorDir2 = 1;
+ motor1Dir1 = 0;
+ motor1Dir2 = 1;
}
void motorStop() {
- motorDir1 = 0;
- motorDir2 = 0;
+ motor1Dir1 = 0;
+ motor1Dir2 = 0;
+}
+
+void motorForward2() {
+ motorStop();
+ motor2Dir1 = 1;
+ motor2Dir2 = 0;
+}
+
+void motorReverse2() {
+ motorStop();
+ motor2Dir1 = 0;
+ motor2Dir2 = 1;
+}
+
+void motorStop2() {
+ motor2Dir1 = 0;
+ motor2Dir2 = 0;
}