cansat-d_2018
otamesi
Dependencies: mbed
Diff: main.cpp
- Revision:
- 38:d3b7fda44f72
- Parent:
- 37:24866a13b959
- Child:
- 39:a0c63ae63c99
--- a/main.cpp Sat Dec 15 08:10:40 2018 +0000
+++ b/main.cpp Sat Dec 15 15:54:39 2018 +0000
@@ -6,8 +6,8 @@
#include "MPU6050.h"
-PwmOut motorSpeed1(p26);
-PwmOut motorSpeed2(p25);
+PwmOut motorSpeedR(p26);
+PwmOut motorSpeedL(p25);
DigitalOut motor1Dir1(p19);
DigitalOut motor1Dir2(p20);
DigitalOut motor2Dir1(p16);
@@ -18,7 +18,7 @@
GPS gps(p28, p27); //GPS
HMC5883L compass(p9, p10); //地磁気センサー
-MPU6050 mpu(p9, p10); //(SDA,SCL)のピンをアサインしてね☆ 加速度センサー
+MPU6050 mpu(p9, p10); //(SDA,SCL) 加速度センサー*/
DigitalIn flight(p23); //フライトピン
DigitalOut SW(p22); //トリガー用
@@ -31,8 +31,8 @@
DigitalIn test2(p18);
-Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake モーター
-Motor motor2(p22, p19, p20, 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)
{
@@ -44,13 +44,13 @@
//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);
+void motorForwardR(void);
+void motorStopR(void);
+void motorReverseR(void);
-void motorForward2(void);
-void motorStop2(void);
-void motorReverse2(void);
+void motorForwardL(void);
+void motorStopL(void);
+void motorReverseL(void);
LocalFileSystem local("local"); // Create the local filesystem under the name "local" データ保存
@@ -59,19 +59,22 @@
int main() {
printf("cansat start\r\n");
-
+ /* flight==1;
FET = 0;
SW = 1; //p23をhigh(3.3V)にする。
- /* while(1) {
+ while(1) {
if(flight==1) {
wait(10);
+ printf("mada\r\n");
}
else{
if(flight==1) {
wait(10);
+ printf("madamada\r\n");
}
else{
+ printf("yattar\r\n");
FET = 0; //FET、ニクロム線切断
wait(20);
FET = 1;
@@ -81,22 +84,22 @@
FET = 1;
wait(12);
FET = 0;
- SW = 0; //p23をlow(0V)にする。
+ SW = 0; //p23をlow(0V)にする。
break;
}
}
- }
- */
- motor1.stop(0);
- motor2.stop(0);
+ }*/
- wait(20); //確認用
+ motorStopR();
+ motorStopL();
+
+ wait(1); //確認用//デフォ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分半)
{
@@ -115,15 +118,15 @@
fprintf(fp,"GPS finish\r\n");
// fclose(fp); // GPSの測定終了 */
- motorSpeed1.period_ms(4); //モーター調節
- motorSpeed1 = 0.955;
- motorSpeed2.period_ms(4); //モーター調節
- motorSpeed2 = 0.955;
+ motorSpeedR.period_ms(4); //モーター調節
+ motorSpeedR = 0.955;
+ motorSpeedL.period_ms(4); //モーター調節
+ motorSpeedL = 0.755;
compass.init(); //地磁気センサー動作
int i;
-for(i=0;i<20;i++) //地磁気測定
+for(i=0;i<20;i++) //地磁気測定//デフォ20
{
pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記
fprintf(fp,"raw=\r\n");
@@ -134,54 +137,64 @@
mc1=1.0;
mc2=1.0;
//地磁気センサのキャリブレーション
- motorForward2(); //車体を時計回りに３秒回転
- motorReverse();
+ motorForwardL(); //車体を時計回りに３秒回転
+ motorReverseR();
wait(1.6);
- motorStop();
- motorStop2();
+ motorStopR();
+ motorStopL();
wait(1);
- motorReverse2(); //車体を反時計回りに３秒回
- motorForward();
+ motorReverseL(); //車体を反時計回りに３秒回
+ motorForwardR();
wait(1.6);
- motorStop();
- motorStop2();
+ motorStopR();
+ motorStopL();
wait(1);
printf("compass carriblation\r\n"); //キャリブレーション終了
-float mcn1,mcn2;
-double heading;
- mcn1=1.0;
- mcn2=1.0;
-compass.init();
-heading=compass.getHeadingXYDeg();
+//float mcn1,mcn2;
+double heading,w;
+ // mcn1=1.0;
+ // mcn2=1.0;
+
+heading=compass.getHeadingXYDeg();//headingに地磁気の値を格納する
if(90<heading<267.5){
- motorForward2();//右回転
- motorReverse();
- wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
- motorStop();
- motorStop2();
+ printf("right\r\n");
+ motorForwardL();//右回転
+ motorReverseR();
+ w=(270-heading)*0.004448;
+ wait(w); //角度のずれ*1度回転するのにかかる時間
+ motorStopR();
+ motorStopL();
wait(1);
-}else if(0<=heading<=90.0){
- motorReverse2();//左回転
- motorForward();
- wait((heading+90.0)*0.004448);
- motorStop();
- motorStop2();
+}
+else if(0<=heading<=90.0){
+ printf("left\r\n");
+ motorReverseL();//左回転
+ motorForwardR();
+ w=(heading+90.0)*0.004448;
+ wait(w);
+ motorStopR();
+ motorStopL();
wait(1);
-}else if(272.5<heading<360){
- motorReverse2();//左回転
- motorForward();
- wait((heading-270)*0.004448);
- motorStop();
- motorStop2();
+}
+else if(272.5<heading<360){
+ printf("left\r\n");
+ motorReverseL();//左回転
+ motorForwardR();
+ w=(heading-270)*0.004448;
+ wait(w);
+ motorStopR();
+ motorStopL();
wait(1);
-}else{
+}
+else{
wait(5);
}
printf("searchN\r\n"); //機体が北を向く
+wait(2);
mu.startUpdates();//start mesuring the distance(超音波センサー)
int distance;
@@ -200,7 +213,7 @@
float run=0;
fprintf(fp, "x,y,z,lefttrun2,rightrun\r\n");
-
+ int k=0;//中間地点角度判定プログラムを１回しか通さないためのフラグ
while(1)
{
distance=mu.getCurrentDistance();
@@ -211,7 +224,7 @@
int y = accel[1]+60;//y軸方向の加速度
int z = accel[2]+1110 ;//z軸方向の加速度
- printf("x=%f,y=%f,z=%f\r\n",x,y,z); //加速度の表示
+ printf("x=%d,y=%d,z=%d\r\n",x,y,z); //加速度の表示
printf("%d\r\n", test.read()); //フォトインタラプタ
printf("%d\r\n", test2.read());
@@ -237,37 +250,45 @@
printf("%f", leftrun2);
printf("\t%f\r\n", rightrun);
run=culculate_distance_3(rightrun,leftrun2);
- if (5000<run && run<5050){ //半分くらい進んだら方位を計測し直す
- motor1.stop(0);
- motor2.stop(0);
+ if(5000< run && run<5050 && k!=1){//中間地点で北を向きなおす
+ motorStopR();
+ motorStopL();
wait(1);
- if(90<heading<267.5){
- motorForward2();//右回転
- motorReverse();
- wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
- motor1.stop(0);
- motor2.stop(0);
+ heading=compass.getHeadingXYDeg();//コンパスの値をもう一度headingに格納
+ wait(2);
+ k=1;
+
+if(90<heading<267.5){
+ motorForwardL();//右回転
+ motorReverseR();
+ w=(270-heading)*0.004448;
+ wait(w); //角度のずれ*1度回転するのにかかる時間
+ motorStopR();
+ motorStopL();
wait(1);
}else if(0<=heading<=90.0){
- motorReverse2();//左回転
- motorForward();
- wait((heading+90.0)*0.004448);
- motor1.stop(0);
- motor2.stop(0);
+ motorReverseL();//左回転
+ motorForwardR();
+ w=(heading+90.0)*0.004448;
+ wait(w);
+ motorStopR();
+ motorStopL();
wait(1);
}else if(272.5<heading<360){
- motorReverse2();//左回転
- motorForward();
- wait((heading-270)*0.004448);
- motor1.stop(0);
- motor2.stop(0);
+ motorReverseL();//左回転
+ motorForwardR();
+ w=(heading-270)*0.004448;
+ wait(w);
+ motorStopR();
+ motorStopL();
wait(1);
}else{
wait(5);
+
}
printf("searchN\r\n"); //機体が北を向く
}
- else if (run >= 10000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+ else if(run >= 10000){//もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
break; //つまりゴールについたらこのループからぬける
}
@@ -276,8 +297,8 @@
break;
} */
- motorForward2(); //通常走行
- motorForward();
+ motorForwardL(); //通常走行
+ motorForwardR();
//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.
@@ -287,7 +308,7 @@
fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z, leftrun2, rightrun);//加速度とフォトインタラプタによる距離を出力
- if(450 < distance && distance < 550) //障害物発見❕
+ if(450 < distance && distance < 550) //障害物発見
{
printf("if success!\r\n");
@@ -299,43 +320,43 @@
msj1=1.0; //回転の時
msj2=1.0;
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(2);
printf("mortor stop\r\n");
- motorForward2(); //機体を時計回りに９０度回転
- motorReverse();
+ motorForwardL(); //機体を時計回りに９０度回転
+ motorReverseR();
wait(0.5);
printf("mortor rotation\r\n");
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(2);
printf("mortor stop\r\n");
- motorForward2(); //直進
- motorForward();
+ motorForwardL(); //直進
+ motorForwardR();
wait(2);
printf("mortor straight\r\n");
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(2);
printf("mortor stop\r\n");
- motorReverse2(); //機体を反時計回りに９０度回転
- motorForward();
+ motorReverseL(); //機体を反時計回りに９０度回転
+ motorForwardR();
wait(0.5);
printf("mortor rotation\r\n");
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(2);
printf("mortor stop\r\n");
- motorForward2(); //直進
- motorForward();
+ motorForwardL(); //直進
+ motorForwardR();
int t=0;
@@ -368,37 +389,9 @@
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; */
- fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z, leftrun2, rightrun);//加速度とフォトインタラプタによる距離を出力
-
- /* 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;
+ fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z, leftrun2, rightrun);//加速度とフォトインタラプタによる距離を出力
- // 速度計算(加速度を台形積分する)
- 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）と変位を表示*/
-
- if (run >= 10000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
+ if (run >= 10000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
break; //つまりゴールについたらこのループからぬける
}
@@ -410,61 +403,61 @@
printf("mortor straight\r\n");
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(2);
printf("mortor stop\r\n");
- motorReverse2(); //機体を反時計回りに９０度回転
- motorForward();
+ motorReverseL(); //機体を反時計回りに９０度回転
+ motorForwardR();
wait(0.5);
printf("mortor rotation\r\n");
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(2);
printf("mortor stop\r\n");
- motorForward2(); //直進
- motorForward();
+ motorForwardL(); //直進
+ motorForwardR();
wait(2);
printf("mortor straight\r\n");
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(2);
printf("mortor stop\r\n");
- motorForward2(); //機体を時計回りに９０度回転
- motorReverse();
+ motorForwardL(); //機体を時計回りに９０度回転
+ motorReverseR();
wait(0.5);
printf("mortor rotation\r\n");
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(2);
printf("mortor stop\r\n");
if(90<heading<267.5){
- motorForward2();//右回転
- motorReverse();
+ motorForwardL();//右回転
+ motorReverseR();
wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(1);
}else if(0<=heading<=90.0){
- motorReverse2();//左回転
- motorForward();
+ motorReverseL();//左回転
+ motorForwardR();
wait((heading+90.0)*0.004448);
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(1);
}else if(272.5<heading<360){
- motorReverse2();//左回転
- motorForward();
+ motorReverseL();//左回転
+ motorForwardR();
wait((heading-270)*0.004448);
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
wait(1);
}else{
wait(5);
@@ -473,8 +466,8 @@
}
}
- motor1.stop(0);
- motor2.stop(0);
+ motorStopR();
+ motorStopL();
fprintf(fp, "last photo\r\n");
@@ -488,42 +481,42 @@
- float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
+ float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
{
float c;
c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
return c;
}
-void motorForward() {
- motorStop();
+void motorForwardR() {
+ motorStopR();
motor1Dir1 = 1;
motor1Dir2 = 0;
}
-void motorReverse() {
- motorStop();
+void motorReverseR() {
+ motorStopR();
motor1Dir1 = 0;
motor1Dir2 = 1;
}
-void motorStop() {
+void motorStopR() {
motor1Dir1 = 0;
motor1Dir2 = 0;
}
-void motorForward2() {
- motorStop();
+void motorForwardL() {
+ motorStopR();
motor2Dir1 = 1;
motor2Dir2 = 0;
}
-void motorReverse2() {
- motorStop();
+void motorReverseL() {
+ motorStopR();
motor2Dir1 = 0;
motor2Dir2 = 1;
}
-void motorStop2() {
+void motorStopL() {
motor2Dir1 = 0;
motor2Dir2 = 0;
}