kyunsat / Mbed 2 deprecated MAG3110test

2/25/2017 LPC1768 ver

Dependencies:   MotionSensor SDFileSystem mbed

Fork of TanecCon by hswell and nike

main.cpp@1:2992de38cf3e, 2017-02-23 (annotated)

Committer:
Nike3221
Date:
Thu Feb 23 00:24:24 2017 +0000
Revision:
1:2992de38cf3e
Parent:
0:029ef267b1bc
Child:
2:6f9881435f17
cansat

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nike3221 0:029ef267b1bc 1 /*
Nike3221 0:029ef267b1bc 2 * MAG3110 Sensor Library for mbed
Nike3221 0:029ef267b1bc 3 * TODO: Add proper header
Nike3221 0:029ef267b1bc 4 */
Nike3221 0:029ef267b1bc 5
Nike3221 0:029ef267b1bc 6
Nike3221 0:029ef267b1bc 7 #include "mbed.h"
Nike3221 0:029ef267b1bc 8 #include "MotionSensor.h"
Nike3221 0:029ef267b1bc 9 #include "MAG3110.h"
Nike3221 0:029ef267b1bc 10 #include "math.h"
Nike3221 0:029ef267b1bc 11 //#include "SDFileSystem.h"
Nike3221 0:029ef267b1bc 12
Nike3221 0:029ef267b1bc 13 //SDFileSystem sd(p5, p6, p7, p8, "sd");
Nike3221 1:2992de38cf3e 14 Serial pc(USBTX,USBRX);
Nike3221 1:2992de38cf3e 15 //Serial pc(p9,p10);
Nike3221 0:029ef267b1bc 16
Nike3221 1:2992de38cf3e 17 int16_t con_x,con_y,con_z;
Nike3221 0:029ef267b1bc 18 LocalFileSystem local("local");
Nike3221 1:2992de38cf3e 19 //MAG3110 mag(p9,p10);
Nike3221 0:029ef267b1bc 20 MAG3110 mag(p28,p27);
Nike3221 0:029ef267b1bc 21 Serial gps(p13,p14);
Nike3221 0:029ef267b1bc 22
Nike3221 1:2992de38cf3e 23 DigitalIn local_on(p17);
Nike3221 1:2992de38cf3e 24 //DigitalIn local_del(p19);
Nike3221 1:2992de38cf3e 25 //DigitalIn cds(p29);
Nike3221 1:2992de38cf3e 26 Timer ActiveTime; //タイマー
Nike3221 1:2992de38cf3e 27 //DigitalOut trg(p29);
Nike3221 1:2992de38cf3e 28 //DigitalOut sig(p28);
Nike3221 1:2992de38cf3e 29 //InterruptIn distance(p27);
Nike3221 0:029ef267b1bc 30
Nike3221 0:029ef267b1bc 31 DigitalOut bit1(LED4);
Nike3221 0:029ef267b1bc 32 DigitalOut bit2(LED3);
Nike3221 0:029ef267b1bc 33 DigitalOut bit3(LED2);
Nike3221 0:029ef267b1bc 34 DigitalOut bit4(LED1);
Nike3221 1:2992de38cf3e 35 DigitalOut bit5(p16);
Nike3221 1:2992de38cf3e 36 DigitalOut stby(p11);
Nike3221 1:2992de38cf3e 37 DigitalOut motor_io(p12);
Nike3221 1:2992de38cf3e 38 PwmOut servo(p24);
Nike3221 1:2992de38cf3e 39 PwmOut motor_r(p21);
Nike3221 1:2992de38cf3e 40 PwmOut motor_l(p22);
Nike3221 1:2992de38cf3e 41 DigitalOut muda(p15);
Nike3221 1:2992de38cf3e 42 InterruptIn cds(p29);
Nike3221 1:2992de38cf3e 43 //DigitalIn cds(p29);
Nike3221 0:029ef267b1bc 44
Nike3221 1:2992de38cf3e 45 char gps_s[256];
Nike3221 1:2992de38cf3e 46 char gps_c[256];
Nike3221 1:2992de38cf3e 47 float palse=0.02;
Nike3221 1:2992de38cf3e 48 float servo_palse=0.002;
Nike3221 1:2992de38cf3e 49 float motor_palse=0.01;
Nike3221 1:2992de38cf3e 50 float sig_time;
Nike3221 0:029ef267b1bc 51 int offset_x,offset_y;
Nike3221 1:2992de38cf3e 52 float cds_on,x_max,y_max,x_min,y_min;
Nike3221 1:2992de38cf3e 53 float count_offset,count_gps;
Nike3221 1:2992de38cf3e 54 int a,b,gp,gga,rmc,gll,flag,count2,count_move,quadrant;
Nike3221 1:2992de38cf3e 55 float x_target,y_target,d_target,sita,angle_caly,angle_calx,direction_target;
Nike3221 0:029ef267b1bc 56 float longitude_target = 36.11707;
Nike3221 0:029ef267b1bc 57 float latitude_target = 139.4720;
Nike3221 1:2992de38cf3e 58 float utc_time,latitude1,latitude2,longitude1,longitude2,speed,course,utc_date,gps_magnetic;
Nike3221 0:029ef267b1bc 59 char pos_status;
Nike3221 1:2992de38cf3e 60 float direction,x_dat,y_dat,longitudegosa,latitudegosa;
Nike3221 0:029ef267b1bc 61 char angle_c;
Nike3221 0:029ef267b1bc 62
Nike3221 0:029ef267b1bc 63
Nike3221 1:2992de38cf3e 64 void init()
Nike3221 1:2992de38cf3e 65 {
Nike3221 1:2992de38cf3e 66 muda=0;
Nike3221 1:2992de38cf3e 67 cds_on = 0;
Nike3221 1:2992de38cf3e 68 count_move = 0;
Nike3221 1:2992de38cf3e 69 stby = 1;
Nike3221 1:2992de38cf3e 70 motor_io = 1;
Nike3221 1:2992de38cf3e 71 motor_r.period(0.02);
Nike3221 1:2992de38cf3e 72 motor_l.period(0.02);
Nike3221 1:2992de38cf3e 73 servo.period(0.02);
Nike3221 1:2992de38cf3e 74 x_max = -1000.0;//x,yの最大値を初期化
Nike3221 1:2992de38cf3e 75 y_max = -1000.0;
Nike3221 1:2992de38cf3e 76 x_min = 1000.0;
Nike3221 1:2992de38cf3e 77 y_min = 1000.0;
Nike3221 1:2992de38cf3e 78
Nike3221 1:2992de38cf3e 79 }
Nike3221 1:2992de38cf3e 80
Nike3221 1:2992de38cf3e 81 void cal_l()
Nike3221 1:2992de38cf3e 82 {
Nike3221 1:2992de38cf3e 83 latitude1 = latitude1/100;
Nike3221 1:2992de38cf3e 84 latitude2 = (fmod(latitude1,1)/60)*100;
Nike3221 1:2992de38cf3e 85 latitude1 = floor(latitude1);
Nike3221 1:2992de38cf3e 86 latitude1 = latitude1+latitude2;
Nike3221 1:2992de38cf3e 87
Nike3221 1:2992de38cf3e 88 longitude1 = longitude1/100;
Nike3221 1:2992de38cf3e 89 longitude2 = fmod(longitude1,1)/60*100;
Nike3221 1:2992de38cf3e 90 longitude1 = floor(longitude1);
Nike3221 1:2992de38cf3e 91 longitude1 = longitude1+longitude2;
Nike3221 1:2992de38cf3e 92
Nike3221 1:2992de38cf3e 93 }
Nike3221 1:2992de38cf3e 94
Nike3221 1:2992de38cf3e 95 void gps_read()
Nike3221 1:2992de38cf3e 96 {
Nike3221 1:2992de38cf3e 97 __disable_irq();//割り込み禁止
Nike3221 0:029ef267b1bc 98
Nike3221 1:2992de38cf3e 99 bit4=1;
Nike3221 1:2992de38cf3e 100 pc.printf("gps_read\n");
Nike3221 1:2992de38cf3e 101 for(count2=0; count2 <= 250 ; count2++) {
Nike3221 1:2992de38cf3e 102 gps_s[count2] = gps.getc();
Nike3221 1:2992de38cf3e 103 pc.printf("%c",gps_s[count2]);
Nike3221 1:2992de38cf3e 104 }
Nike3221 1:2992de38cf3e 105 bit4=0;
Nike3221 1:2992de38cf3e 106 if(count_move==0 || count_move==3)
Nike3221 1:2992de38cf3e 107 {__enable_irq();}//割り込み許可
Nike3221 1:2992de38cf3e 108 }
Nike3221 1:2992de38cf3e 109
Nike3221 1:2992de38cf3e 110 int gps_save()
Nike3221 1:2992de38cf3e 111 {
Nike3221 1:2992de38cf3e 112 a=0;
Nike3221 1:2992de38cf3e 113 gp = 2;
Nike3221 1:2992de38cf3e 114 count_gps = 0;
Nike3221 1:2992de38cf3e 115
Nike3221 1:2992de38cf3e 116 pc.printf("\ngps_save\n");
Nike3221 1:2992de38cf3e 117
Nike3221 1:2992de38cf3e 118 while(a <= 250 && count_gps < 5) {
Nike3221 1:2992de38cf3e 119
Nike3221 1:2992de38cf3e 120 for(a=0,b=0,flag=0;flag < gp && flag < 5 && gp < 5 && a < 200; a++,b++) {
Nike3221 1:2992de38cf3e 121
Nike3221 1:2992de38cf3e 122 bit3=1;
Nike3221 1:2992de38cf3e 123 gps_c[a] = gps_s[b];
Nike3221 1:2992de38cf3e 124 pc.printf("%d %c \n",a,gps_c[a]);
Nike3221 1:2992de38cf3e 125
Nike3221 1:2992de38cf3e 126 if(gps_c[a] == '$') {
Nike3221 1:2992de38cf3e 127 gps_c[0] = '$';
Nike3221 1:2992de38cf3e 128 a=0;
Nike3221 1:2992de38cf3e 129 flag++;
Nike3221 1:2992de38cf3e 130 pc.printf("%d %c flag:%d gp:%d\n",a,gps_c[a],flag,gp);
Nike3221 1:2992de38cf3e 131 }
Nike3221 1:2992de38cf3e 132
Nike3221 1:2992de38cf3e 133 else if(gps_c[a-1] == '\r' && gps_c[a] == '\n' && flag == gp)
Nike3221 1:2992de38cf3e 134 {
Nike3221 1:2992de38cf3e 135 gps_c[a+1]='\0';
Nike3221 1:2992de38cf3e 136 flag = 10;
Nike3221 1:2992de38cf3e 137 }
Nike3221 1:2992de38cf3e 138 else if(a > 256)//aが256を超えたとき、ループを強制脱出
Nike3221 1:2992de38cf3e 139 {
Nike3221 1:2992de38cf3e 140 count_gps = 5;
Nike3221 1:2992de38cf3e 141 flag = 10;
Nike3221 1:2992de38cf3e 142 }
Nike3221 1:2992de38cf3e 143
Nike3221 1:2992de38cf3e 144 }
Nike3221 1:2992de38cf3e 145
Nike3221 1:2992de38cf3e 146 bit3=0;
Nike3221 1:2992de38cf3e 147 bit2=1;
Nike3221 1:2992de38cf3e 148 wait(0.5);
Nike3221 1:2992de38cf3e 149
Nike3221 1:2992de38cf3e 150 if(memcmp(gps_c,"$GPRMC",6) == 0) {
Nike3221 1:2992de38cf3e 151 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);
Nike3221 1:2992de38cf3e 152 //latitude = strncpy(latitude1,latitude1,2);
Nike3221 1:2992de38cf3e 153 cal_l();
Nike3221 1:2992de38cf3e 154 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);
Nike3221 1:2992de38cf3e 155 gp++;
Nike3221 1:2992de38cf3e 156
Nike3221 1:2992de38cf3e 157 if(latitude1!=0) {
Nike3221 1:2992de38cf3e 158 bit1=1;
Nike3221 1:2992de38cf3e 159 }
Nike3221 1:2992de38cf3e 160 //mkdir("/sd/gps", 0777);
Nike3221 1:2992de38cf3e 161
Nike3221 1:2992de38cf3e 162 if(local_on == 1) {
Nike3221 1:2992de38cf3e 163 FILE *fp = fopen("/local/gps.txt", "a");
Nike3221 1:2992de38cf3e 164 if(fp == NULL) {
Nike3221 1:2992de38cf3e 165 error("Could not open file for write\n");
Nike3221 1:2992de38cf3e 166 }
Nike3221 1:2992de38cf3e 167
Nike3221 1:2992de38cf3e 168 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);
Nike3221 1:2992de38cf3e 169 fclose(fp);
Nike3221 1:2992de38cf3e 170 }
Nike3221 1:2992de38cf3e 171 bit1=0;
Nike3221 1:2992de38cf3e 172 count_gps++;
Nike3221 1:2992de38cf3e 173 pc.printf("count_gps:%.1f\n",count_gps);
Nike3221 1:2992de38cf3e 174 }
Nike3221 1:2992de38cf3e 175
Nike3221 1:2992de38cf3e 176 else if(memcmp(gps_c,"$GPGGA",6) == 0) {
Nike3221 1:2992de38cf3e 177 sscanf(gps_c,"$GPGGA,%f,%f,N,%f,E",&utc_time,&latitude1,&longitude1);
Nike3221 1:2992de38cf3e 178 cal_l();
Nike3221 1:2992de38cf3e 179 pc.printf("$GPGGA,time[%f],%f,N,%f,E \n",utc_time,latitude1,longitude1);
Nike3221 1:2992de38cf3e 180 gp++;
Nike3221 1:2992de38cf3e 181
Nike3221 1:2992de38cf3e 182 if(latitude1!=0) {
Nike3221 1:2992de38cf3e 183 bit1=1;
Nike3221 1:2992de38cf3e 184 }
Nike3221 1:2992de38cf3e 185 // mkdir("/sd/gps", 0777);
Nike3221 1:2992de38cf3e 186 if(local_on == 1) {
Nike3221 1:2992de38cf3e 187 FILE *fp = fopen("/local/gps.txt", "a");
Nike3221 1:2992de38cf3e 188 if(fp == NULL) {
Nike3221 1:2992de38cf3e 189 error("Could not open file for write\n");
Nike3221 1:2992de38cf3e 190 }
Nike3221 1:2992de38cf3e 191
Nike3221 1:2992de38cf3e 192 fprintf(fp,"$GPGGA,%f,%f,N,%f,E \n",utc_time,latitude1,longitude1);
Nike3221 1:2992de38cf3e 193 fclose(fp);
Nike3221 1:2992de38cf3e 194 }
Nike3221 1:2992de38cf3e 195 bit1=0;
Nike3221 1:2992de38cf3e 196 count_gps++;
Nike3221 1:2992de38cf3e 197 pc.printf("count_gps:%.1f\n",count_gps);
Nike3221 1:2992de38cf3e 198 }
Nike3221 1:2992de38cf3e 199
Nike3221 1:2992de38cf3e 200 else if(memcmp(gps_c,"$GPGLL",6) == 0) {
Nike3221 1:2992de38cf3e 201 sscanf(gps_c,"$GPGLL,%f,N,%f,E \n",&latitude1,&longitude1);
Nike3221 1:2992de38cf3e 202 cal_l();
Nike3221 1:2992de38cf3e 203 pc.printf("$GPGLL,%f,N,%f,E \n",latitude1,longitude1);
Nike3221 1:2992de38cf3e 204 gp++;
Nike3221 1:2992de38cf3e 205
Nike3221 1:2992de38cf3e 206 if(latitude1!=0) {
Nike3221 1:2992de38cf3e 207 bit1=1;
Nike3221 1:2992de38cf3e 208 }
Nike3221 1:2992de38cf3e 209 //mkdir("/sd/gps", 0777);
Nike3221 1:2992de38cf3e 210
Nike3221 1:2992de38cf3e 211 if(local_on == 1) {
Nike3221 1:2992de38cf3e 212 FILE *fp = fopen("/local/gps.txt", "a");
Nike3221 1:2992de38cf3e 213 if(fp == NULL) {
Nike3221 1:2992de38cf3e 214 error("Could not open file for write\n");
Nike3221 1:2992de38cf3e 215 }
Nike3221 1:2992de38cf3e 216
Nike3221 1:2992de38cf3e 217 fprintf(fp,"$GPGLL,%f,N,%f,E \n",latitude1,longitude1);
Nike3221 1:2992de38cf3e 218 fclose(fp);
Nike3221 1:2992de38cf3e 219 }
Nike3221 1:2992de38cf3e 220 bit1=0;
Nike3221 1:2992de38cf3e 221 count_gps++;
Nike3221 1:2992de38cf3e 222 pc.printf("count_gps:%.1f\n",count_gps);
Nike3221 1:2992de38cf3e 223 }
Nike3221 1:2992de38cf3e 224
Nike3221 1:2992de38cf3e 225 else
Nike3221 1:2992de38cf3e 226 {
Nike3221 1:2992de38cf3e 227 count_gps++;
Nike3221 1:2992de38cf3e 228 gp++;
Nike3221 1:2992de38cf3e 229 pc.printf("gps_c is not match count_gps:%.1f\n",count_gps);
Nike3221 1:2992de38cf3e 230 }
Nike3221 1:2992de38cf3e 231 bit2=0;
Nike3221 1:2992de38cf3e 232 }
Nike3221 1:2992de38cf3e 233
Nike3221 1:2992de38cf3e 234
Nike3221 1:2992de38cf3e 235 return 0;
Nike3221 1:2992de38cf3e 236 }
Nike3221 1:2992de38cf3e 237
Nike3221 1:2992de38cf3e 238 int offset(float x_rd,float y_rd)
Nike3221 1:2992de38cf3e 239 {
Nike3221 1:2992de38cf3e 240
Nike3221 1:2992de38cf3e 241 if(x_max == 0 || y_max == 0 || x_min == 0 || y_min == 0) {
Nike3221 1:2992de38cf3e 242 x_max = -1000.0;//x,yの最大値を初期化 +-32500
Nike3221 1:2992de38cf3e 243 y_max = -1000.0;
Nike3221 1:2992de38cf3e 244 x_min = 1000.0;
Nike3221 1:2992de38cf3e 245 y_min = 1000.0;
Nike3221 1:2992de38cf3e 246 }//最大値、最小値に0が入った時初期化
Nike3221 1:2992de38cf3e 247
Nike3221 1:2992de38cf3e 248 if(x_max<x_rd && x_rd != 0) {
Nike3221 1:2992de38cf3e 249 x_max=x_rd;
Nike3221 1:2992de38cf3e 250 }
Nike3221 1:2992de38cf3e 251
Nike3221 1:2992de38cf3e 252 if(y_max<y_rd && y_rd != 0) {
Nike3221 1:2992de38cf3e 253 y_max=y_rd;
Nike3221 1:2992de38cf3e 254 }
Nike3221 1:2992de38cf3e 255
Nike3221 1:2992de38cf3e 256 if(x_min>x_rd && x_rd != 0) {
Nike3221 1:2992de38cf3e 257 x_min=x_rd;
Nike3221 1:2992de38cf3e 258 }
Nike3221 1:2992de38cf3e 259
Nike3221 1:2992de38cf3e 260 if(y_min>y_rd && y_rd != 0) {
Nike3221 1:2992de38cf3e 261 y_min=y_rd;
Nike3221 1:2992de38cf3e 262 }
Nike3221 1:2992de38cf3e 263
Nike3221 1:2992de38cf3e 264 offset_x = ((abs(x_min)+abs(x_max))/2);
Nike3221 1:2992de38cf3e 265 offset_y = ((abs(y_min)+abs(y_max))/2);
Nike3221 1:2992de38cf3e 266
Nike3221 1:2992de38cf3e 267 x_dat = x_rd + offset_x;
Nike3221 1:2992de38cf3e 268 y_dat = y_rd - offset_y;
Nike3221 1:2992de38cf3e 269
Nike3221 1:2992de38cf3e 270 pc.printf("x: %d , y: %d , x_dat: %f , y_dat: %f , x_max: %.5f , x_min: %.5f , direction: %f \n",con_x, con_y, x_rd, y_rd, x_max, x_min, direction);
Nike3221 1:2992de38cf3e 271 return 0;
Nike3221 1:2992de38cf3e 272 }
Nike3221 1:2992de38cf3e 273
Nike3221 1:2992de38cf3e 274 void cal_gps()
Nike3221 1:2992de38cf3e 275 {
Nike3221 1:2992de38cf3e 276 if(latitude_target > latitude1 && longitude_target > longitude1)//第１象限
Nike3221 1:2992de38cf3e 277 {quadrant = 1;}
Nike3221 1:2992de38cf3e 278 if(latitude_target > latitude1 && longitude_target < longitude1)
Nike3221 1:2992de38cf3e 279 {quadrant = 2;}
Nike3221 1:2992de38cf3e 280 if(latitude_target < latitude1 && longitude_target < longitude1)
Nike3221 1:2992de38cf3e 281 {quadrant = 3;}
Nike3221 1:2992de38cf3e 282 if(latitude_target < latitude1 && longitude_target < longitude1)
Nike3221 1:2992de38cf3e 283 {quadrant = 4;}
Nike3221 1:2992de38cf3e 284
Nike3221 1:2992de38cf3e 285 y_target = (latitude_target-latitude1)*111319.49;
Nike3221 1:2992de38cf3e 286 x_target = (longitude_target-longitude1)*cos(latitude1*(3.1415926535/180))*111319.49;
Nike3221 1:2992de38cf3e 287 d_target = sqrt(pow(con_x,2.0)*pow(con_y,2.0));//目的地までの距離
Nike3221 1:2992de38cf3e 288
Nike3221 1:2992de38cf3e 289 angle_caly = cos(latitude_target)*sin(longitude_target-longitude1);
Nike3221 1:2992de38cf3e 290 angle_calx = cos(latitude1)*sin(latitude_target)-sin(latitude1)*cos(latitude_target)*cos(longitude_target-longitude1);
Nike3221 1:2992de38cf3e 291 sita = atan2(angle_caly,angle_calx);
Nike3221 1:2992de38cf3e 292 if(sita < 0)
Nike3221 1:2992de38cf3e 293 {sita = sita + 2*3.1415926535;}
Nike3221 1:2992de38cf3e 294 direction_target = sita*180/3.1415926535;
Nike3221 1:2992de38cf3e 295 }
Nike3221 1:2992de38cf3e 296
Nike3221 1:2992de38cf3e 297 void cal_con()
Nike3221 1:2992de38cf3e 298 {
Nike3221 1:2992de38cf3e 299
Nike3221 1:2992de38cf3e 300
Nike3221 1:2992de38cf3e 301 if(direction < 0)
Nike3221 1:2992de38cf3e 302 {direction = direction + 2*3.1415926535;}
Nike3221 1:2992de38cf3e 303 direction = (atan(y_dat/x_dat))*57.2958;//ラジアンに変換
Nike3221 1:2992de38cf3e 304 //direction = (atan(y_cal/x_cal))*57.2958;
Nike3221 1:2992de38cf3e 305
Nike3221 1:2992de38cf3e 306 if(0 < direction && 89 > direction){
Nike3221 1:2992de38cf3e 307 angle_c = 'N';}
Nike3221 1:2992de38cf3e 308
Nike3221 1:2992de38cf3e 309 else if(90 < direction && 179 > direction){
Nike3221 1:2992de38cf3e 310 angle_c = 'E';}
Nike3221 1:2992de38cf3e 311
Nike3221 1:2992de38cf3e 312 else if(180 < direction && 269 > direction || -180 < direction && -91 > direction){
Nike3221 1:2992de38cf3e 313 angle_c = 'S';}
Nike3221 1:2992de38cf3e 314
Nike3221 1:2992de38cf3e 315 else if(270 < direction && 359 > direction || -90 < direction && -1 > direction){
Nike3221 1:2992de38cf3e 316 angle_c = 'W';}
Nike3221 1:2992de38cf3e 317
Nike3221 1:2992de38cf3e 318 else{ angle_c = '?';}
Nike3221 1:2992de38cf3e 319
Nike3221 1:2992de38cf3e 320 /*if(north == 1) {
Nike3221 1:2992de38cf3e 321 angle_c = 'N';
Nike3221 1:2992de38cf3e 322 }
Nike3221 1:2992de38cf3e 323
Nike3221 1:2992de38cf3e 324 else if(east == 1) {
Nike3221 1:2992de38cf3e 325 angle_c = 'E';
Nike3221 1:2992de38cf3e 326 }
Nike3221 1:2992de38cf3e 327
Nike3221 1:2992de38cf3e 328 else if(south == 1) {
Nike3221 1:2992de38cf3e 329 angle_c = 'S';
Nike3221 1:2992de38cf3e 330 }
Nike3221 1:2992de38cf3e 331
Nike3221 1:2992de38cf3e 332 else if(west == 1) {
Nike3221 1:2992de38cf3e 333 angle_c = 'W';
Nike3221 1:2992de38cf3e 334 }
Nike3221 1:2992de38cf3e 335
Nike3221 1:2992de38cf3e 336 else {
Nike3221 1:2992de38cf3e 337 angle_c = '?';
Nike3221 1:2992de38cf3e 338 }*/
Nike3221 1:2992de38cf3e 339
Nike3221 1:2992de38cf3e 340 }
Nike3221 1:2992de38cf3e 341
Nike3221 1:2992de38cf3e 342 void trigger()
Nike3221 1:2992de38cf3e 343 {
Nike3221 1:2992de38cf3e 344 //trg = 1;
Nike3221 1:2992de38cf3e 345 bit2 = 1;
Nike3221 1:2992de38cf3e 346 wait_us(10);
Nike3221 1:2992de38cf3e 347 //trg = 0;
Nike3221 1:2992de38cf3e 348 bit2 = 0;
Nike3221 1:2992de38cf3e 349 }
Nike3221 1:2992de38cf3e 350
Nike3221 1:2992de38cf3e 351 void rise_echo()//立ち上がり割り込み
Nike3221 1:2992de38cf3e 352 {
Nike3221 1:2992de38cf3e 353 //sig=1;
Nike3221 1:2992de38cf3e 354 ActiveTime.start();
Nike3221 1:2992de38cf3e 355 __disable_irq();
Nike3221 1:2992de38cf3e 356 }
Nike3221 1:2992de38cf3e 357
Nike3221 1:2992de38cf3e 358 void fall_echo()//落ち込み割り込み
Nike3221 1:2992de38cf3e 359 {
Nike3221 1:2992de38cf3e 360 //sig=0;
Nike3221 1:2992de38cf3e 361 ActiveTime.stop();
Nike3221 1:2992de38cf3e 362 if(ActiveTime.read_us() > 0.00002){
Nike3221 1:2992de38cf3e 363 sig_time = ActiveTime.read_us()/6.169;}
Nike3221 1:2992de38cf3e 364 ActiveTime.reset();
Nike3221 1:2992de38cf3e 365
Nike3221 1:2992de38cf3e 366 if(local_on == 1){
Nike3221 1:2992de38cf3e 367 bit1=1;
Nike3221 1:2992de38cf3e 368 FILE *fp = fopen("/local/ultra.txt","a");
Nike3221 1:2992de38cf3e 369 fprintf(fp,"%f\n",sig_time);
Nike3221 1:2992de38cf3e 370 fclose(fp);
Nike3221 1:2992de38cf3e 371 bit1=0;}
Nike3221 1:2992de38cf3e 372 }
Nike3221 1:2992de38cf3e 373
Nike3221 1:2992de38cf3e 374
Nike3221 1:2992de38cf3e 375 void first_sequence()
Nike3221 1:2992de38cf3e 376 {
Nike3221 1:2992de38cf3e 377 if(count_move==0)
Nike3221 1:2992de38cf3e 378 {count_move=1;}
Nike3221 1:2992de38cf3e 379 }
Nike3221 1:2992de38cf3e 380
Nike3221 1:2992de38cf3e 381 void move()
Nike3221 1:2992de38cf3e 382 {
Nike3221 1:2992de38cf3e 383 switch(count_move)
Nike3221 1:2992de38cf3e 384 {
Nike3221 1:2992de38cf3e 385 case 1:
Nike3221 1:2992de38cf3e 386 bit5 = 1;
Nike3221 1:2992de38cf3e 387 pc.printf("*************case1**************\n");
Nike3221 1:2992de38cf3e 388 motor_l.pulsewidth(0.001); //パルス幅
Nike3221 1:2992de38cf3e 389 motor_r.pulsewidth(0.001);
Nike3221 1:2992de38cf3e 390 wait(10);//落下時間
Nike3221 1:2992de38cf3e 391
Nike3221 1:2992de38cf3e 392 servo.pulsewidth(servo_palse);
Nike3221 1:2992de38cf3e 393
Nike3221 1:2992de38cf3e 394 /*if(direction <= direction_target+3 && direction >= direction_target-3)
Nike3221 1:2992de38cf3e 395 {count_move++;}*/
Nike3221 1:2992de38cf3e 396 count_move++;
Nike3221 1:2992de38cf3e 397 pc.printf("*************case1 end**************\n");
Nike3221 1:2992de38cf3e 398 break;
Nike3221 1:2992de38cf3e 399 case 2:
Nike3221 1:2992de38cf3e 400 pc.printf("*************case2**************\n");
Nike3221 1:2992de38cf3e 401 pc.printf("direction: %f direction_target: %f\n",direction,direction_target);
Nike3221 1:2992de38cf3e 402 longitudegosa = longitude_target-longitude1;
Nike3221 1:2992de38cf3e 403 latitudegosa = latitude_target-latitude1;
Nike3221 1:2992de38cf3e 404
Nike3221 1:2992de38cf3e 405 if(direction > direction_target+5.0)
Nike3221 1:2992de38cf3e 406 { while(direction > direction_target+5.0)
Nike3221 1:2992de38cf3e 407 {
Nike3221 1:2992de38cf3e 408 pc.printf("tarn clockwise \ndirection: %f direction_target: %f\n",direction,direction_target);
Nike3221 1:2992de38cf3e 409 mag.getX(&con_x);//コンパス読み込み
Nike3221 1:2992de38cf3e 410 mag.getY(&con_y);
Nike3221 1:2992de38cf3e 411 offset(con_x,con_y);//x,y オフセット計算
Nike3221 1:2992de38cf3e 412 cal_con();
Nike3221 1:2992de38cf3e 413 motor_l.pulsewidth(motor_palse/2);
Nike3221 1:2992de38cf3e 414 motor_r.pulsewidth(motor_palse/4);}}
Nike3221 1:2992de38cf3e 415
Nike3221 1:2992de38cf3e 416 else if(direction < direction_target-5.0)
Nike3221 1:2992de38cf3e 417 { while(direction < direction_target-5.0)
Nike3221 1:2992de38cf3e 418 {
Nike3221 1:2992de38cf3e 419 pc.printf("tarn anti clockwise \ndirection: %f direction_target: %f\n",direction,direction_target);
Nike3221 1:2992de38cf3e 420 mag.getX(&con_x);//コンパス読み込み
Nike3221 1:2992de38cf3e 421 mag.getY(&con_y);
Nike3221 1:2992de38cf3e 422 offset(con_x,con_y);//x,y オフセット計算
Nike3221 1:2992de38cf3e 423 cal_con();
Nike3221 1:2992de38cf3e 424 motor_r.pulsewidth(motor_palse/2);
Nike3221 1:2992de38cf3e 425 motor_l.pulsewidth(motor_palse/4);}}
Nike3221 1:2992de38cf3e 426
Nike3221 1:2992de38cf3e 427 else if(direction <= direction_target+5 && direction >= direction_target-5)
Nike3221 1:2992de38cf3e 428 {count_move++;}
Nike3221 1:2992de38cf3e 429
Nike3221 1:2992de38cf3e 430 if( abs(longitudegosa)<=50 && abs(latitudegosa)<=50 )
Nike3221 1:2992de38cf3e 431 { pc.printf("front \n");
Nike3221 1:2992de38cf3e 432 motor_l.pulsewidth(motor_palse); //パルス幅
Nike3221 1:2992de38cf3e 433 motor_r.pulsewidth(motor_palse);
Nike3221 1:2992de38cf3e 434 count_move++; }
Nike3221 1:2992de38cf3e 435 pc.printf("direction: %f direction_target: %f\n",direction,direction_target);
Nike3221 1:2992de38cf3e 436
Nike3221 1:2992de38cf3e 437 pc.printf("*************case2 end**************\n");
Nike3221 1:2992de38cf3e 438
Nike3221 1:2992de38cf3e 439 break;
Nike3221 1:2992de38cf3e 440
Nike3221 1:2992de38cf3e 441 case 3:
Nike3221 1:2992de38cf3e 442 pc.printf("*************case3**************\n");
Nike3221 1:2992de38cf3e 443 trigger();
Nike3221 1:2992de38cf3e 444 //distance.rise(rise_echo);
Nike3221 1:2992de38cf3e 445 //distance.fall(fall_echo);
Nike3221 1:2992de38cf3e 446
Nike3221 1:2992de38cf3e 447 pc.printf("%f\n",sig_time);
Nike3221 1:2992de38cf3e 448
Nike3221 1:2992de38cf3e 449 if(sig_time <= 150.0)//150mm
Nike3221 1:2992de38cf3e 450 (count_move++);
Nike3221 1:2992de38cf3e 451 else
Nike3221 1:2992de38cf3e 452 {
Nike3221 1:2992de38cf3e 453 motor_l.pulsewidth(motor_palse/2); //パルス幅
Nike3221 1:2992de38cf3e 454 motor_r.pulsewidth(motor_palse/2);
Nike3221 1:2992de38cf3e 455 }
Nike3221 1:2992de38cf3e 456
Nike3221 1:2992de38cf3e 457 break;
Nike3221 1:2992de38cf3e 458
Nike3221 1:2992de38cf3e 459 default:
Nike3221 1:2992de38cf3e 460 motor_l.pulsewidth(0.0);
Nike3221 1:2992de38cf3e 461 motor_r.pulsewidth(0.0);
Nike3221 1:2992de38cf3e 462 bit5 = 0;
Nike3221 1:2992de38cf3e 463 break;
Nike3221 1:2992de38cf3e 464
Nike3221 1:2992de38cf3e 465 }
Nike3221 0:029ef267b1bc 466
Nike3221 0:029ef267b1bc 467 }
Nike3221 0:029ef267b1bc 468
Nike3221 0:029ef267b1bc 469
Nike3221 1:2992de38cf3e 470 void con_save()
Nike3221 1:2992de38cf3e 471 {
Nike3221 0:029ef267b1bc 472
Nike3221 0:029ef267b1bc 473 bit1=1;//led4を1にしてデータを保存
Nike3221 0:029ef267b1bc 474 FILE *fp = fopen("/local/commpas_new.txt","a");
Nike3221 1:2992de38cf3e 475 fprintf(fp,"%c,%f,%f,%f,%f\n",angle_c,d_target,x_dat,y_dat,direction);
Nike3221 1:2992de38cf3e 476 //fprintf(fp,"%d,%d,%d,%d,%d\n",x,y,z,offset_x,offset_y);
Nike3221 1:2992de38cf3e 477 fclose(fp);
Nike3221 1:2992de38cf3e 478 bit1=0;
Nike3221 0:029ef267b1bc 479 }
Nike3221 0:029ef267b1bc 480
Nike3221 1:2992de38cf3e 481 void dele()
Nike3221 1:2992de38cf3e 482 {
Nike3221 1:2992de38cf3e 483
Nike3221 0:029ef267b1bc 484 bit2=1;//led3を1
Nike3221 1:2992de38cf3e 485
Nike3221 0:029ef267b1bc 486 remove("/local/commpas_new.txt");
Nike3221 0:029ef267b1bc 487 remove("/local/gps.txt");
Nike3221 1:2992de38cf3e 488
Nike3221 0:029ef267b1bc 489 bit2=0;
Nike3221 0:029ef267b1bc 490 }
Nike3221 0:029ef267b1bc 491
Nike3221 1:2992de38cf3e 492 int main()
Nike3221 1:2992de38cf3e 493 {
Nike3221 1:2992de38cf3e 494 bit1 = 1;
Nike3221 1:2992de38cf3e 495 pc.printf("start\n");
Nike3221 1:2992de38cf3e 496 //__disable_irq();//割り込み禁止
Nike3221 1:2992de38cf3e 497 init(); //初期化
Nike3221 1:2992de38cf3e 498 //cds.rise(first_sequence);//cds立ち上がり割り込み
Nike3221 1:2992de38cf3e 499 cds.fall(&first_sequence);//cds落ち込み割り込み
Nike3221 1:2992de38cf3e 500 mag.sampleRate(0x40);
Nike3221 0:029ef267b1bc 501 mag.enable();//コンパス有効化
Nike3221 1:2992de38cf3e 502 mag.getX(&con_x);//コンパス読み込み
Nike3221 1:2992de38cf3e 503 mag.getY(&con_y);
Nike3221 1:2992de38cf3e 504 mag.getZ(&con_z);
Nike3221 1:2992de38cf3e 505 for(count_offset=0; count_offset < 100; count_offset++)//300個分のサンプ取得
Nike3221 1:2992de38cf3e 506 {offset(con_x,con_y);}
Nike3221 1:2992de38cf3e 507 bit1 = 0;
Nike3221 0:029ef267b1bc 508
Nike3221 1:2992de38cf3e 509 while(1) {
Nike3221 1:2992de38cf3e 510
Nike3221 1:2992de38cf3e 511 __enable_irq();//割り込み許可
Nike3221 1:2992de38cf3e 512 mag.getX(&con_x);//コンパス読み込み
Nike3221 1:2992de38cf3e 513 mag.getY(&con_y);
Nike3221 1:2992de38cf3e 514 offset(con_x,con_y);//x,y オフセット計算
Nike3221 1:2992de38cf3e 515 cal_con();
Nike3221 1:2992de38cf3e 516
Nike3221 1:2992de38cf3e 517 gps_read();
Nike3221 1:2992de38cf3e 518 gps_save();
Nike3221 1:2992de38cf3e 519 cal_gps();
Nike3221 1:2992de38cf3e 520
Nike3221 1:2992de38cf3e 521 /*if(cds == 0)
Nike3221 1:2992de38cf3e 522 {first_sequence();
Nike3221 1:2992de38cf3e 523 bit5=1;}*/
Nike3221 1:2992de38cf3e 524
Nike3221 1:2992de38cf3e 525 move();
Nike3221 1:2992de38cf3e 526
Nike3221 1:2992de38cf3e 527 if(local_on == 1)
Nike3221 1:2992de38cf3e 528 { con_save(); } //pin20がhighの時、データ保存
Nike3221 1:2992de38cf3e 529 /*if(local_del == 1)
Nike3221 1:2992de38cf3e 530 { dele(); }*/ //pin19がhighの時、保存したデータを消去
Nike3221 1:2992de38cf3e 531
Nike3221 1:2992de38cf3e 532 }
Nike3221 0:029ef267b1bc 533 }