kyunsat
2/25/2017 LPC1768 ver
Dependencies: MotionSensor SDFileSystem mbed
Fork of
TanecCon
by 
hswell and nike
Diff: main.cpp
- Revision:
- 4:1fdffa2e6312
- Parent:
- 3:8d904225adfd
--- 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