Revision 4:1fdffa2e6312, committed 2017-08-22

Comitter:

Nike3221

Date:

Tue Aug 22 16:16:34 2017 +0000

Parent:

3:8d904225adfd

Commit message:

tane

Changed in this revision

--- a/main.cpp	Tue Feb 28 15:54:53 2017 +0000
+++ b/main.cpp	Tue Aug 22 16:16:34 2017 +0000
@@ -19,28 +19,30 @@
 //MAG3110 mag(p9,p10);
 MAG3110 mag(p28,p27);
 Serial gps(p13,p14);
+//Serial gps(p9,p10);
 
-DigitalIn local_on(p17);
-//DigitalIn local_del(p19);
+DigitalOut bin(p17);
+DigitalIn local_del(p20);
 //DigitalIn cds(p29);
 Timer ActiveTime; //タイマー
-//DigitalOut trg(p29);
-//DigitalOut sig(p28);
-//InterruptIn distance(p27);
+DigitalOut trg(p26);
+//DigitalOut sig(p25);
+InterruptIn distance(p25);
 
 DigitalOut bit1(LED4);
 DigitalOut bit2(LED3);
 DigitalOut bit3(LED2);
 DigitalOut bit4(LED1);
 //DigitalOut bit5(p16);
-DigitalIn local_on2(p16);
+DigitalOut ain(p16);
 DigitalOut stby(p11);
 DigitalOut motor_io(p12);
-PwmOut servo(p24);
-PwmOut motor_r(p21);
+PwmOut servo(p21);
+PwmOut motor_r(p23);
 PwmOut motor_l(p22);
 DigitalOut muda(p15);
-InterruptIn cds(p29);
+InterruptIn cds(p18);
+DigitalOut cds_out(p19);
 //DigitalIn cds(p29);
 
 char gps_s[256];
@@ -54,8 +56,8 @@
 float count_offset,count_gps;
 int a,b,gp,gga,rmc,gll,flag,count2,count_move,quadrant;
 float x_target,y_target,d_target,sita,angle_caly,angle_calx,direction_target;
-float longitude_target = 36.115986;//36.11707
-float latitude_target = 139.456207;//139.4720
+float longitude_target = 30.375776;//36.11707
+float latitude_target = 130.960419;//139.4720
 float utc_time,latitude1,latitude2,longitude1,longitude2,speed,course,utc_date,gps_magnetic;
 char pos_status;
 float direction,x_dat,y_dat,longitudegosa,latitudegosa;
@@ -64,14 +66,16 @@
 
 void init()
 {
-    muda=0;
+    ain = 1;
+    bin = 1;
+    cds_out = 1;
+    muda=1;
     cds_on = 0;
     count_move = 0;
     stby = 1;
     motor_io = 1;
     motor_r.period(0.02);
     motor_l.period(0.02);
-    servo.period(0.02);
     x_max = -1000.0;//x,yの最大値を初期化
     y_max = -1000.0;
     x_min = 1000.0;
@@ -79,7 +83,7 @@
 
 }
 
-void cal_l()
+void cal_ll()
 {
     latitude1 = latitude1/100;
     latitude2 = (fmod(latitude1,1)/60)*100;
@@ -97,13 +101,11 @@
 {
     __disable_irq();//割り込み禁止
     
-    bit4=1;
     pc.printf("gps_read\n");
-    for(count2=0; count2 < 200 ; count2++) {
+    for(count2=0; count2 < 300 ; count2++) {
         gps_s[count2] = gps.getc();
         pc.printf("%c",gps_s[count2]);
     }
-    bit4=0;
     if(count_move==0 || count_move==3)
      {__enable_irq();}//割り込み許可
 }
@@ -118,9 +120,8 @@
 
     while(a <= 250 && count_gps < 4) {
 
-        for(a=0,b=0,flag=0;  flag <= gp && flag < 4 && gp < 4 && a < 200;  a++,b++)//"lag<4" "gp<4" "a<200"は無限ループ対策
+        for(a=0,b=0,flag=0;  flag <= gp && flag < 4 && gp < 4 && a < 300;  a++,b++)//"lag<4" "gp<4" "a<200"は無限ループ対策
          {
-            bit3=1;
             gps_c[a] = gps_s[b];
             pc.printf("%d %c \n",a,gps_c[a]);
 
@@ -135,26 +136,16 @@
             else if(gps_c[a-1] == '\r' && gps_c[a] == '\n' && flag == gp)
               {
                 gps_c[a+1]='\0';
-                flag = 10;
+                flag = 10;//forから抜ける
               }
-              
-              
-            else if(a > 256)//aが256を超えたとき、ループを強制脱出(whileから)
-              {   
-                count_gps = 5;
-                flag = 10;
-              }
-
         }
 
-        bit3=0;
-        bit2=1;
         wait(0.5);
 
         if(memcmp(gps_c,"$GPRMC",6) == 0) {
             sscanf(gps_c,"$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W",&utc_time,&pos_status,&latitude1,&longitude1,&speed,&course,&utc_date,&gps_magnetic);
             //latitude = strncpy(latitude1,latitude1,2);
-            cal_l();
+            cal_ll();
             pc.printf("$GPRMC,time[%f],%c,%f,N,%f,E,%f,%f,%f,%f,W \n",utc_time,pos_status,latitude1,longitude1,speed,course,utc_date,gps_magnetic);
             gp++;
 
@@ -162,16 +153,13 @@
                 bit1=1;
             }
             //mkdir("/sd/gps", 0777);
-
-            if(local_on == 1) {
                 FILE *fp = fopen("/local/gps.txt", "a");
-                if(fp == NULL) {
-                    error("Could not open file for write\n");
-                }
+                if(fp == NULL) {}
+                   //error("Could not open file for write\n");
 
                 fprintf(fp,"$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W \n",utc_time,pos_status,latitude1,longitude1,speed,course,utc_date,gps_magnetic);
                 fclose(fp);
-            }
+            
             bit1=0;
             count_gps++;
             pc.printf("count_gps:%.1f\n",count_gps);
@@ -179,7 +167,7 @@
 
         else if(memcmp(gps_c,"$GPGGA",6) == 0) {
             sscanf(gps_c,"$GPGGA,%f,%f,N,%f,E",&utc_time,&latitude1,&longitude1);
-            cal_l();
+            cal_ll();
             pc.printf("$GPGGA,time[%f],%f,N,%f,E \n",utc_time,latitude1,longitude1);
             gp++;
 
@@ -187,15 +175,14 @@
                 bit1=1;
             }
             // mkdir("/sd/gps", 0777);
-            if(local_on == 1) {
                 FILE *fp = fopen("/local/gps.txt", "a");
                 if(fp == NULL) {
-                    error("Could not open file for write\n");
+                   //error("Could not open file for write\n");
                 }
 
                 fprintf(fp,"$GPGGA,%f,%f,N,%f,E \n",utc_time,latitude1,longitude1);
                 fclose(fp);
-            }
+            
             bit1=0;
             count_gps++;
             pc.printf("count_gps:%.1f\n",count_gps);
@@ -203,7 +190,7 @@
 
         else if(memcmp(gps_c,"$GPGLL",6) == 0) {
             sscanf(gps_c,"$GPGLL,%f,N,%f,E \n",&latitude1,&longitude1);
-            cal_l();
+            cal_ll();
             pc.printf("$GPGLL,%f,N,%f,E \n",latitude1,longitude1);
             gp++;
 
@@ -212,15 +199,15 @@
             }
             //mkdir("/sd/gps", 0777);
 
-            if(local_on == 1) {
+        
                 FILE *fp = fopen("/local/gps.txt", "a");
                 if(fp == NULL) {
-                    error("Could not open file for write\n");
+                   //error("Could not open file for write\n");
                 }
 
                 fprintf(fp,"$GPGLL,%f,N,%f,E \n",latitude1,longitude1);
                 fclose(fp);
-            }
+            
             bit1=0;
             count_gps++;
             pc.printf("count_gps:%.1f\n",count_gps);
@@ -232,11 +219,8 @@
          gp++;
          pc.printf("gps_c is not match count_gps:%.1f\n",count_gps);
         }
-        bit2=0;
     }
 
-
-    return 0;
 }
 
 int offset(float x_rd,float y_rd)
@@ -328,16 +312,16 @@
     direction = direction*57.29578; //ラジアンに変換
     //direction = (atan(y_cal/x_cal))*57.29578;
 
-    if(0 < direction && 89 > direction){
+    if(316 < direction && 44 > direction){      //correction*********
         angle_c = 'N';}
 
-    else if(90 < direction && 179 > direction){
+    else if(225 < direction && 315 > direction){//correction*********
         angle_c = 'E';}
 
-    else if(180 < direction && 269 > direction || -180 < direction && -91 > direction){
+    else if(136 < direction && 224 > direction){//correction*********
         angle_c = 'S';}
 
-    else if(270 < direction && 359 > direction || -90 < direction && -1 > direction){
+    else if(45 < direction && 135 > direction){ //correction*********
         angle_c = 'W';}
 
     else{ angle_c = '?';}
@@ -346,10 +330,10 @@
 
 void trigger()
 {
-    //trg = 1;
+    trg = 1;
     bit2 = 1;
     wait_us(10);
-    //trg = 0;
+    trg = 0;
     bit2 = 0;
 }
 
@@ -367,13 +351,12 @@
     if(ActiveTime.read_us() > 0.00002){
                    sig_time = ActiveTime.read_us()/6.169;}
     ActiveTime.reset();
-    
-    if(local_on == 1){
+
         bit1=1;
         FILE *fp = fopen("/local/ultra.txt","a");
                    fprintf(fp,"%f\n",sig_time);
                    fclose(fp);
-        bit1=0;}
+        bit1=0;
 }
 
 
@@ -443,8 +426,8 @@
         case 1: 
                 
                 pc.printf("*************case1**************\n");
-                motor_l.pulsewidth(0.01);  //パルス幅
-                motor_r.pulsewidth(0.01);
+                motor_l.pulsewidth(0.018);  //パルス幅
+                motor_r.pulsewidth(0.018);
                 wait(10);//落下時間
                 
                 servo.pulsewidth(servo_palse);
@@ -525,6 +508,7 @@
                 
                 gps_read();
                 gps_save();
+                cal_ll();
                 cal_gps();
                 
                 mag.getX(&con_x);//コンパス読み込み
@@ -547,7 +531,7 @@
                   
                 break;        
         case 4:
-                pc.printf("*************case3**************\n");
+                pc.printf("*************case4**************\n");
                 
                 __enable_irq();
                 trigger();
@@ -556,14 +540,14 @@
                 
                 pc.printf("sig_time: %f\n",sig_time);
                 
-                if(sig_time <= 100.0)//100mm以下に近づいたとき停止
+                if(sig_time <= 100.0 && sig_time >= 50.0)//100mm以下に近づいたとき停止
                  { pc.printf("sig_time is 100mm or under: %f\n",sig_time);
                    count_move++;}
                 else
                  {
                   turn_sequence();
-                  motor_l.pulsewidth(motor_palse/2);  //パルス幅
-                  motor_r.pulsewidth(motor_palse/2);
+                  motor_l.pulsewidth(motor_palse*0.8);  //パルス幅
+                  motor_r.pulsewidth(motor_palse*0.8);
                  }
                  
                 break;
@@ -605,16 +589,29 @@
 {
     bit1 = 1;
     pc.printf("start\n");
+    
+    servo.period(0.02);
+    
+    servo.pulsewidth(servo_palse);//0.002
+    wait(5);
+    servo.pulsewidth(0.001);
     //__disable_irq();//割り込み禁止
     init(); //初期化
     //cds.rise(first_sequence);//cds立ち上がり割り込み
-    cds.fall(&first_sequence);//cds落ち込み割り込み
     mag.sampleRate(0x40);
     mag.enable();//コンパス有効化
     mag.getX(&con_x);//コンパス読み込み
     mag.getY(&con_y);
     mag.getZ(&con_z);
     
+    for(count_offset=0; count_offset <10; count_offset++ )
+     {
+         gps_read();
+         gps_save();
+         cal_ll();
+         cal_gps();
+     }
+    
     motor_r.pulsewidth(motor_palse*0.7);
     motor_l.pulsewidth(motor_palse*0.35);
     
@@ -625,17 +622,26 @@
       mag.getZ(&con_z);
       offset(con_x,con_y);
       cal_con();
-      
-      if(local_on2==1)
-        {
+
            FILE *fp = fopen("/local/first_con.txt","a");
                       fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
                       fclose(fp);
-        }
       }
+      
     motor_r.pulsewidth(motor_palse*0.0);
     motor_l.pulsewidth(motor_palse*0.0);
-    bit1 = 0;
+    
+    servo.pulsewidth(servo_palse);//0.002
+    wait(5);
+    servo.pulsewidth(0.001);
+    
+    bit1 = 1; bit2 = 1;
+    bit3 = 1; bit4 = 1;
+    pc.printf("cds wait\n");
+    wait(20);
+    bit1 = 0; bit2 = 0;
+    bit3 = 0; bit4 = 0;
+    cds.fall(&first_sequence);//cds落ち込み割り込み
     
     while(1) {
         
@@ -647,18 +653,38 @@
                 
                 gps_read();
                 gps_save();
+                cal_ll();
                 cal_gps();
                 
                 /*if(cds == 0)
                  {first_sequence();
                   bit5=1;}*/
 
-                  move();
+                move();
                 
-                if(local_on == 1)
-                 { con_save(); }  //pin20がhighの時、データ保存
+                con_save();   //pin20がhighの時、データ保存
                 /*if(local_del == 1)
                    { dele(); }*/  //pin19がhighの時、保存したデータを消去
                 
              }
-}
\ No newline at end of file
+}
+//         _01     40_
+//         _02     39_
+//         _03     38_
+// xxxxxxxx_04     37_
+//         _05     36_
+//         _06     35_
+//         _07     34_
+//         _08     33_
+//         _09     32_
+//         _10     31_
+//     stby_11     30_xxxxxxxxx
+//  motorio_12     29_
+//      gps_13     28_mag
+//      gps_14     27_mag
+//     muda_15     26_trg
+//      ain_16     25_distance
+//      bin_17     24_
+//      cds_18     23_motor_r
+//  cds_out_19     22_motor_l
+//      del_20     21_
\ No newline at end of file