kyunsat / Mbed 2 deprecated MAG3110test

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MotionSensor SDFileSystem mbed

Fork of TanecCon by hswell and nike

main.cpp@2:6f9881435f17, 2017-02-26 (annotated)

Committer:
Nike3221
Date:
Sun Feb 26 16:46:37 2017 +0000
Revision:
2:6f9881435f17
Parent:
1:2992de38cf3e
Child:
3:8d904225adfd
TaneCon

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 2:6f9881435f17 35 //DigitalOut bit5(p16);
Nike3221 2:6f9881435f17 36 DigitalIn local_on2(p16);
Nike3221 1:2992de38cf3e 37 DigitalOut stby(p11);
Nike3221 1:2992de38cf3e 38 DigitalOut motor_io(p12);
Nike3221 1:2992de38cf3e 39 PwmOut servo(p24);
Nike3221 1:2992de38cf3e 40 PwmOut motor_r(p21);
Nike3221 1:2992de38cf3e 41 PwmOut motor_l(p22);
Nike3221 1:2992de38cf3e 42 DigitalOut muda(p15);
Nike3221 1:2992de38cf3e 43 InterruptIn cds(p29);
Nike3221 1:2992de38cf3e 44 //DigitalIn cds(p29);
Nike3221 0:029ef267b1bc 45
Nike3221 1:2992de38cf3e 46 char gps_s[256];
Nike3221 1:2992de38cf3e 47 char gps_c[256];
Nike3221 1:2992de38cf3e 48 float palse=0.02;
Nike3221 1:2992de38cf3e 49 float servo_palse=0.002;
Nike3221 2:6f9881435f17 50 float motor_palse=0.02;
Nike3221 1:2992de38cf3e 51 float sig_time;
Nike3221 2:6f9881435f17 52 float offset_x,offset_y;
Nike3221 1:2992de38cf3e 53 float cds_on,x_max,y_max,x_min,y_min;
Nike3221 1:2992de38cf3e 54 float count_offset,count_gps;
Nike3221 1:2992de38cf3e 55 int a,b,gp,gga,rmc,gll,flag,count2,count_move,quadrant;
Nike3221 1:2992de38cf3e 56 float x_target,y_target,d_target,sita,angle_caly,angle_calx,direction_target;
Nike3221 2:6f9881435f17 57 float longitude_target = 36.065726;//36.11707
Nike3221 2:6f9881435f17 58 float latitude_target = 139.272254;//139.4720
Nike3221 1:2992de38cf3e 59 float utc_time,latitude1,latitude2,longitude1,longitude2,speed,course,utc_date,gps_magnetic;
Nike3221 0:029ef267b1bc 60 char pos_status;
Nike3221 1:2992de38cf3e 61 float direction,x_dat,y_dat,longitudegosa,latitudegosa;
Nike3221 0:029ef267b1bc 62 char angle_c;
Nike3221 0:029ef267b1bc 63
Nike3221 0:029ef267b1bc 64
Nike3221 1:2992de38cf3e 65 void init()
Nike3221 1:2992de38cf3e 66 {
Nike3221 1:2992de38cf3e 67 muda=0;
Nike3221 1:2992de38cf3e 68 cds_on = 0;
Nike3221 1:2992de38cf3e 69 count_move = 0;
Nike3221 1:2992de38cf3e 70 stby = 1;
Nike3221 1:2992de38cf3e 71 motor_io = 1;
Nike3221 1:2992de38cf3e 72 motor_r.period(0.02);
Nike3221 1:2992de38cf3e 73 motor_l.period(0.02);
Nike3221 1:2992de38cf3e 74 servo.period(0.02);
Nike3221 1:2992de38cf3e 75 x_max = -1000.0;//x,yの最大値を初期化
Nike3221 1:2992de38cf3e 76 y_max = -1000.0;
Nike3221 1:2992de38cf3e 77 x_min = 1000.0;
Nike3221 1:2992de38cf3e 78 y_min = 1000.0;
Nike3221 1:2992de38cf3e 79
Nike3221 1:2992de38cf3e 80 }
Nike3221 1:2992de38cf3e 81
Nike3221 1:2992de38cf3e 82 void cal_l()
Nike3221 1:2992de38cf3e 83 {
Nike3221 1:2992de38cf3e 84 latitude1 = latitude1/100;
Nike3221 1:2992de38cf3e 85 latitude2 = (fmod(latitude1,1)/60)*100;
Nike3221 1:2992de38cf3e 86 latitude1 = floor(latitude1);
Nike3221 1:2992de38cf3e 87 latitude1 = latitude1+latitude2;
Nike3221 1:2992de38cf3e 88
Nike3221 1:2992de38cf3e 89 longitude1 = longitude1/100;
Nike3221 1:2992de38cf3e 90 longitude2 = fmod(longitude1,1)/60*100;
Nike3221 1:2992de38cf3e 91 longitude1 = floor(longitude1);
Nike3221 1:2992de38cf3e 92 longitude1 = longitude1+longitude2;
Nike3221 1:2992de38cf3e 93
Nike3221 1:2992de38cf3e 94 }
Nike3221 1:2992de38cf3e 95
Nike3221 1:2992de38cf3e 96 void gps_read()
Nike3221 1:2992de38cf3e 97 {
Nike3221 1:2992de38cf3e 98 __disable_irq();//割り込み禁止
Nike3221 0:029ef267b1bc 99
Nike3221 1:2992de38cf3e 100 bit4=1;
Nike3221 1:2992de38cf3e 101 pc.printf("gps_read\n");
Nike3221 1:2992de38cf3e 102 for(count2=0; count2 <= 250 ; count2++) {
Nike3221 1:2992de38cf3e 103 gps_s[count2] = gps.getc();
Nike3221 1:2992de38cf3e 104 pc.printf("%c",gps_s[count2]);
Nike3221 1:2992de38cf3e 105 }
Nike3221 1:2992de38cf3e 106 bit4=0;
Nike3221 1:2992de38cf3e 107 if(count_move==0 || count_move==3)
Nike3221 1:2992de38cf3e 108 {__enable_irq();}//割り込み許可
Nike3221 1:2992de38cf3e 109 }
Nike3221 1:2992de38cf3e 110
Nike3221 1:2992de38cf3e 111 int gps_save()
Nike3221 1:2992de38cf3e 112 {
Nike3221 1:2992de38cf3e 113 a=0;
Nike3221 1:2992de38cf3e 114 gp = 2;
Nike3221 1:2992de38cf3e 115 count_gps = 0;
Nike3221 1:2992de38cf3e 116
Nike3221 1:2992de38cf3e 117 pc.printf("\ngps_save\n");
Nike3221 1:2992de38cf3e 118
Nike3221 1:2992de38cf3e 119 while(a <= 250 && count_gps < 5) {
Nike3221 1:2992de38cf3e 120
Nike3221 1:2992de38cf3e 121 for(a=0,b=0,flag=0;flag < gp && flag < 5 && gp < 5 && a < 200; a++,b++) {
Nike3221 1:2992de38cf3e 122
Nike3221 1:2992de38cf3e 123 bit3=1;
Nike3221 1:2992de38cf3e 124 gps_c[a] = gps_s[b];
Nike3221 1:2992de38cf3e 125 pc.printf("%d %c \n",a,gps_c[a]);
Nike3221 1:2992de38cf3e 126
Nike3221 1:2992de38cf3e 127 if(gps_c[a] == '$') {
Nike3221 1:2992de38cf3e 128 gps_c[0] = '$';
Nike3221 1:2992de38cf3e 129 a=0;
Nike3221 1:2992de38cf3e 130 flag++;
Nike3221 1:2992de38cf3e 131 pc.printf("%d %c flag:%d gp:%d\n",a,gps_c[a],flag,gp);
Nike3221 1:2992de38cf3e 132 }
Nike3221 1:2992de38cf3e 133
Nike3221 1:2992de38cf3e 134 else if(gps_c[a-1] == '\r' && gps_c[a] == '\n' && flag == gp)
Nike3221 1:2992de38cf3e 135 {
Nike3221 1:2992de38cf3e 136 gps_c[a+1]='\0';
Nike3221 1:2992de38cf3e 137 flag = 10;
Nike3221 1:2992de38cf3e 138 }
Nike3221 1:2992de38cf3e 139 else if(a > 256)//aが256を超えたとき、ループを強制脱出
Nike3221 1:2992de38cf3e 140 {
Nike3221 1:2992de38cf3e 141 count_gps = 5;
Nike3221 1:2992de38cf3e 142 flag = 10;
Nike3221 1:2992de38cf3e 143 }
Nike3221 1:2992de38cf3e 144
Nike3221 1:2992de38cf3e 145 }
Nike3221 1:2992de38cf3e 146
Nike3221 1:2992de38cf3e 147 bit3=0;
Nike3221 1:2992de38cf3e 148 bit2=1;
Nike3221 1:2992de38cf3e 149 wait(0.5);
Nike3221 1:2992de38cf3e 150
Nike3221 1:2992de38cf3e 151 if(memcmp(gps_c,"$GPRMC",6) == 0) {
Nike3221 1:2992de38cf3e 152 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 153 //latitude = strncpy(latitude1,latitude1,2);
Nike3221 1:2992de38cf3e 154 cal_l();
Nike3221 1:2992de38cf3e 155 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 156 gp++;
Nike3221 1:2992de38cf3e 157
Nike3221 1:2992de38cf3e 158 if(latitude1!=0) {
Nike3221 1:2992de38cf3e 159 bit1=1;
Nike3221 1:2992de38cf3e 160 }
Nike3221 1:2992de38cf3e 161 //mkdir("/sd/gps", 0777);
Nike3221 1:2992de38cf3e 162
Nike3221 1:2992de38cf3e 163 if(local_on == 1) {
Nike3221 1:2992de38cf3e 164 FILE *fp = fopen("/local/gps.txt", "a");
Nike3221 1:2992de38cf3e 165 if(fp == NULL) {
Nike3221 1:2992de38cf3e 166 error("Could not open file for write\n");
Nike3221 1:2992de38cf3e 167 }
Nike3221 1:2992de38cf3e 168
Nike3221 1:2992de38cf3e 169 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 170 fclose(fp);
Nike3221 1:2992de38cf3e 171 }
Nike3221 1:2992de38cf3e 172 bit1=0;
Nike3221 1:2992de38cf3e 173 count_gps++;
Nike3221 1:2992de38cf3e 174 pc.printf("count_gps:%.1f\n",count_gps);
Nike3221 1:2992de38cf3e 175 }
Nike3221 1:2992de38cf3e 176
Nike3221 1:2992de38cf3e 177 else if(memcmp(gps_c,"$GPGGA",6) == 0) {
Nike3221 1:2992de38cf3e 178 sscanf(gps_c,"$GPGGA,%f,%f,N,%f,E",&utc_time,&latitude1,&longitude1);
Nike3221 1:2992de38cf3e 179 cal_l();
Nike3221 1:2992de38cf3e 180 pc.printf("$GPGGA,time[%f],%f,N,%f,E \n",utc_time,latitude1,longitude1);
Nike3221 1:2992de38cf3e 181 gp++;
Nike3221 1:2992de38cf3e 182
Nike3221 1:2992de38cf3e 183 if(latitude1!=0) {
Nike3221 1:2992de38cf3e 184 bit1=1;
Nike3221 1:2992de38cf3e 185 }
Nike3221 1:2992de38cf3e 186 // mkdir("/sd/gps", 0777);
Nike3221 1:2992de38cf3e 187 if(local_on == 1) {
Nike3221 1:2992de38cf3e 188 FILE *fp = fopen("/local/gps.txt", "a");
Nike3221 1:2992de38cf3e 189 if(fp == NULL) {
Nike3221 1:2992de38cf3e 190 error("Could not open file for write\n");
Nike3221 1:2992de38cf3e 191 }
Nike3221 1:2992de38cf3e 192
Nike3221 1:2992de38cf3e 193 fprintf(fp,"$GPGGA,%f,%f,N,%f,E \n",utc_time,latitude1,longitude1);
Nike3221 1:2992de38cf3e 194 fclose(fp);
Nike3221 1:2992de38cf3e 195 }
Nike3221 1:2992de38cf3e 196 bit1=0;
Nike3221 1:2992de38cf3e 197 count_gps++;
Nike3221 1:2992de38cf3e 198 pc.printf("count_gps:%.1f\n",count_gps);
Nike3221 1:2992de38cf3e 199 }
Nike3221 1:2992de38cf3e 200
Nike3221 1:2992de38cf3e 201 else if(memcmp(gps_c,"$GPGLL",6) == 0) {
Nike3221 1:2992de38cf3e 202 sscanf(gps_c,"$GPGLL,%f,N,%f,E \n",&latitude1,&longitude1);
Nike3221 1:2992de38cf3e 203 cal_l();
Nike3221 1:2992de38cf3e 204 pc.printf("$GPGLL,%f,N,%f,E \n",latitude1,longitude1);
Nike3221 1:2992de38cf3e 205 gp++;
Nike3221 1:2992de38cf3e 206
Nike3221 1:2992de38cf3e 207 if(latitude1!=0) {
Nike3221 1:2992de38cf3e 208 bit1=1;
Nike3221 1:2992de38cf3e 209 }
Nike3221 1:2992de38cf3e 210 //mkdir("/sd/gps", 0777);
Nike3221 1:2992de38cf3e 211
Nike3221 1:2992de38cf3e 212 if(local_on == 1) {
Nike3221 1:2992de38cf3e 213 FILE *fp = fopen("/local/gps.txt", "a");
Nike3221 1:2992de38cf3e 214 if(fp == NULL) {
Nike3221 1:2992de38cf3e 215 error("Could not open file for write\n");
Nike3221 1:2992de38cf3e 216 }
Nike3221 1:2992de38cf3e 217
Nike3221 1:2992de38cf3e 218 fprintf(fp,"$GPGLL,%f,N,%f,E \n",latitude1,longitude1);
Nike3221 1:2992de38cf3e 219 fclose(fp);
Nike3221 1:2992de38cf3e 220 }
Nike3221 1:2992de38cf3e 221 bit1=0;
Nike3221 1:2992de38cf3e 222 count_gps++;
Nike3221 1:2992de38cf3e 223 pc.printf("count_gps:%.1f\n",count_gps);
Nike3221 1:2992de38cf3e 224 }
Nike3221 1:2992de38cf3e 225
Nike3221 1:2992de38cf3e 226 else
Nike3221 1:2992de38cf3e 227 {
Nike3221 1:2992de38cf3e 228 count_gps++;
Nike3221 1:2992de38cf3e 229 gp++;
Nike3221 1:2992de38cf3e 230 pc.printf("gps_c is not match count_gps:%.1f\n",count_gps);
Nike3221 1:2992de38cf3e 231 }
Nike3221 1:2992de38cf3e 232 bit2=0;
Nike3221 1:2992de38cf3e 233 }
Nike3221 1:2992de38cf3e 234
Nike3221 1:2992de38cf3e 235
Nike3221 1:2992de38cf3e 236 return 0;
Nike3221 1:2992de38cf3e 237 }
Nike3221 1:2992de38cf3e 238
Nike3221 1:2992de38cf3e 239 int offset(float x_rd,float y_rd)
Nike3221 1:2992de38cf3e 240 {
Nike3221 1:2992de38cf3e 241
Nike3221 1:2992de38cf3e 242 if(x_max == 0 || y_max == 0 || x_min == 0 || y_min == 0) {
Nike3221 1:2992de38cf3e 243 x_max = -1000.0;//x,yの最大値を初期化 +-32500
Nike3221 1:2992de38cf3e 244 y_max = -1000.0;
Nike3221 1:2992de38cf3e 245 x_min = 1000.0;
Nike3221 1:2992de38cf3e 246 y_min = 1000.0;
Nike3221 1:2992de38cf3e 247 }//最大値、最小値に0が入った時初期化
Nike3221 1:2992de38cf3e 248
Nike3221 1:2992de38cf3e 249 if(x_max<x_rd && x_rd != 0) {
Nike3221 1:2992de38cf3e 250 x_max=x_rd;
Nike3221 1:2992de38cf3e 251 }
Nike3221 1:2992de38cf3e 252
Nike3221 1:2992de38cf3e 253 if(y_max<y_rd && y_rd != 0) {
Nike3221 1:2992de38cf3e 254 y_max=y_rd;
Nike3221 1:2992de38cf3e 255 }
Nike3221 1:2992de38cf3e 256
Nike3221 1:2992de38cf3e 257 if(x_min>x_rd && x_rd != 0) {
Nike3221 1:2992de38cf3e 258 x_min=x_rd;
Nike3221 1:2992de38cf3e 259 }
Nike3221 1:2992de38cf3e 260
Nike3221 1:2992de38cf3e 261 if(y_min>y_rd && y_rd != 0) {
Nike3221 1:2992de38cf3e 262 y_min=y_rd;
Nike3221 1:2992de38cf3e 263 }
Nike3221 1:2992de38cf3e 264
Nike3221 2:6f9881435f17 265 offset_x = (x_min+x_max)/2;
Nike3221 2:6f9881435f17 266 offset_y = (y_min+y_max)/2;
Nike3221 1:2992de38cf3e 267
Nike3221 2:6f9881435f17 268 if(0 < offset_x)
Nike3221 2:6f9881435f17 269 { x_dat = con_x + offset_x*1.3; }
Nike3221 2:6f9881435f17 270 else if(0 > offset_x)
Nike3221 2:6f9881435f17 271 { x_dat = con_x - offset_x*1.3; }
Nike3221 2:6f9881435f17 272
Nike3221 2:6f9881435f17 273 if(0 < offset_y)
Nike3221 2:6f9881435f17 274 { y_dat = con_y + offset_y*1.3; }
Nike3221 2:6f9881435f17 275 else if(0 > offset_y)
Nike3221 2:6f9881435f17 276 { y_dat = con_y - offset_y*1.3; }
Nike3221 1:2992de38cf3e 277
Nike3221 1:2992de38cf3e 278 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 279 return 0;
Nike3221 1:2992de38cf3e 280 }
Nike3221 1:2992de38cf3e 281
Nike3221 1:2992de38cf3e 282 void cal_gps()
Nike3221 1:2992de38cf3e 283 {
Nike3221 1:2992de38cf3e 284 if(latitude_target > latitude1 && longitude_target > longitude1)//第１象限
Nike3221 1:2992de38cf3e 285 {quadrant = 1;}
Nike3221 1:2992de38cf3e 286 if(latitude_target > latitude1 && longitude_target < longitude1)
Nike3221 1:2992de38cf3e 287 {quadrant = 2;}
Nike3221 1:2992de38cf3e 288 if(latitude_target < latitude1 && longitude_target < longitude1)
Nike3221 1:2992de38cf3e 289 {quadrant = 3;}
Nike3221 1:2992de38cf3e 290 if(latitude_target < latitude1 && longitude_target < longitude1)
Nike3221 1:2992de38cf3e 291 {quadrant = 4;}
Nike3221 1:2992de38cf3e 292
Nike3221 1:2992de38cf3e 293 y_target = (latitude_target-latitude1)*111319.49;
Nike3221 1:2992de38cf3e 294 x_target = (longitude_target-longitude1)*cos(latitude1*(3.1415926535/180))*111319.49;
Nike3221 1:2992de38cf3e 295 d_target = sqrt(pow(con_x,2.0)*pow(con_y,2.0));//目的地までの距離
Nike3221 1:2992de38cf3e 296
Nike3221 1:2992de38cf3e 297 angle_caly = cos(latitude_target)*sin(longitude_target-longitude1);
Nike3221 2:6f9881435f17 298 angle_calx = cos(latitude1)*(sin(latitude_target)-sin(latitude1))*cos(latitude_target)*cos(longitude_target-longitude1);
Nike3221 1:2992de38cf3e 299 sita = atan2(angle_caly,angle_calx);
Nike3221 1:2992de38cf3e 300 if(sita < 0)
Nike3221 1:2992de38cf3e 301 {sita = sita + 2*3.1415926535;}
Nike3221 2:6f9881435f17 302 direction_target = sita*57.29578;
Nike3221 1:2992de38cf3e 303 }
Nike3221 1:2992de38cf3e 304
Nike3221 1:2992de38cf3e 305 void cal_con()
Nike3221 1:2992de38cf3e 306 {
Nike3221 1:2992de38cf3e 307
Nike3221 2:6f9881435f17 308 direction = atan2(y_dat,x_dat);
Nike3221 1:2992de38cf3e 309 if(direction < 0)
Nike3221 1:2992de38cf3e 310 {direction = direction + 2*3.1415926535;}
Nike3221 2:6f9881435f17 311 direction = direction*57.29578; //ラジアンに変換
Nike3221 2:6f9881435f17 312 //direction = (atan(y_cal/x_cal))*57.29578;
Nike3221 1:2992de38cf3e 313
Nike3221 1:2992de38cf3e 314 if(0 < direction && 89 > direction){
Nike3221 1:2992de38cf3e 315 angle_c = 'N';}
Nike3221 1:2992de38cf3e 316
Nike3221 1:2992de38cf3e 317 else if(90 < direction && 179 > direction){
Nike3221 1:2992de38cf3e 318 angle_c = 'E';}
Nike3221 1:2992de38cf3e 319
Nike3221 1:2992de38cf3e 320 else if(180 < direction && 269 > direction || -180 < direction && -91 > direction){
Nike3221 1:2992de38cf3e 321 angle_c = 'S';}
Nike3221 1:2992de38cf3e 322
Nike3221 1:2992de38cf3e 323 else if(270 < direction && 359 > direction || -90 < direction && -1 > direction){
Nike3221 1:2992de38cf3e 324 angle_c = 'W';}
Nike3221 1:2992de38cf3e 325
Nike3221 1:2992de38cf3e 326 else{ angle_c = '?';}
Nike3221 1:2992de38cf3e 327
Nike3221 1:2992de38cf3e 328 }
Nike3221 1:2992de38cf3e 329
Nike3221 1:2992de38cf3e 330 void trigger()
Nike3221 1:2992de38cf3e 331 {
Nike3221 1:2992de38cf3e 332 //trg = 1;
Nike3221 1:2992de38cf3e 333 bit2 = 1;
Nike3221 1:2992de38cf3e 334 wait_us(10);
Nike3221 1:2992de38cf3e 335 //trg = 0;
Nike3221 1:2992de38cf3e 336 bit2 = 0;
Nike3221 1:2992de38cf3e 337 }
Nike3221 1:2992de38cf3e 338
Nike3221 1:2992de38cf3e 339 void rise_echo()//立ち上がり割り込み
Nike3221 1:2992de38cf3e 340 {
Nike3221 1:2992de38cf3e 341 //sig=1;
Nike3221 1:2992de38cf3e 342 ActiveTime.start();
Nike3221 1:2992de38cf3e 343 __disable_irq();
Nike3221 1:2992de38cf3e 344 }
Nike3221 1:2992de38cf3e 345
Nike3221 1:2992de38cf3e 346 void fall_echo()//落ち込み割り込み
Nike3221 1:2992de38cf3e 347 {
Nike3221 1:2992de38cf3e 348 //sig=0;
Nike3221 1:2992de38cf3e 349 ActiveTime.stop();
Nike3221 1:2992de38cf3e 350 if(ActiveTime.read_us() > 0.00002){
Nike3221 1:2992de38cf3e 351 sig_time = ActiveTime.read_us()/6.169;}
Nike3221 1:2992de38cf3e 352 ActiveTime.reset();
Nike3221 1:2992de38cf3e 353
Nike3221 1:2992de38cf3e 354 if(local_on == 1){
Nike3221 1:2992de38cf3e 355 bit1=1;
Nike3221 1:2992de38cf3e 356 FILE *fp = fopen("/local/ultra.txt","a");
Nike3221 1:2992de38cf3e 357 fprintf(fp,"%f\n",sig_time);
Nike3221 1:2992de38cf3e 358 fclose(fp);
Nike3221 1:2992de38cf3e 359 bit1=0;}
Nike3221 1:2992de38cf3e 360 }
Nike3221 1:2992de38cf3e 361
Nike3221 1:2992de38cf3e 362
Nike3221 1:2992de38cf3e 363 void first_sequence()
Nike3221 1:2992de38cf3e 364 {
Nike3221 1:2992de38cf3e 365 if(count_move==0)
Nike3221 1:2992de38cf3e 366 {count_move=1;}
Nike3221 1:2992de38cf3e 367 }
Nike3221 1:2992de38cf3e 368
Nike3221 2:6f9881435f17 369 void turn_sequence()
Nike3221 2:6f9881435f17 370 {
Nike3221 2:6f9881435f17 371 __disable_irq();
Nike3221 2:6f9881435f17 372 pc.printf("************turn_sequence*************\n");
Nike3221 2:6f9881435f17 373 pc.printf("direction: %f direction_target: %f\n",direction,direction_target);
Nike3221 2:6f9881435f17 374 longitudegosa = longitude_target-longitude1;
Nike3221 2:6f9881435f17 375 latitudegosa = latitude_target-latitude1;
Nike3221 2:6f9881435f17 376
Nike3221 2:6f9881435f17 377 if(direction > direction_target+2.0)
Nike3221 2:6f9881435f17 378 { while(direction > direction_target+2.0)
Nike3221 2:6f9881435f17 379 {
Nike3221 2:6f9881435f17 380 pc.printf("turn clockwise \ndirection: %f direction_target: %f\n",direction,direction_target);
Nike3221 2:6f9881435f17 381
Nike3221 2:6f9881435f17 382 FILE *fp = fopen("/local/turn.txt","a");
Nike3221 2:6f9881435f17 383 fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
Nike3221 2:6f9881435f17 384 fclose(fp);
Nike3221 2:6f9881435f17 385
Nike3221 2:6f9881435f17 386 mag.getX(&con_x);//コンパス読み込み
Nike3221 2:6f9881435f17 387 mag.getY(&con_y);
Nike3221 2:6f9881435f17 388 offset(con_x,con_y);//x,y オフセット計算
Nike3221 2:6f9881435f17 389 cal_con();
Nike3221 2:6f9881435f17 390
Nike3221 2:6f9881435f17 391 gps_read();
Nike3221 2:6f9881435f17 392 gps_save();
Nike3221 2:6f9881435f17 393 cal_gps();
Nike3221 2:6f9881435f17 394 motor_l.pulsewidth(motor_palse/2);
Nike3221 2:6f9881435f17 395 motor_r.pulsewidth(motor_palse/4);
Nike3221 2:6f9881435f17 396 }}
Nike3221 2:6f9881435f17 397
Nike3221 2:6f9881435f17 398 else if(direction < direction_target-2.0)
Nike3221 2:6f9881435f17 399 { while(direction < direction_target-2.0)
Nike3221 2:6f9881435f17 400 {
Nike3221 2:6f9881435f17 401 pc.printf("turn anti clockwise \ndirection: %f direction_target: %f\n",direction,direction_target);
Nike3221 2:6f9881435f17 402
Nike3221 2:6f9881435f17 403 FILE *fp = fopen("/local/turn2.txt","a");
Nike3221 2:6f9881435f17 404 fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
Nike3221 2:6f9881435f17 405 fclose(fp);
Nike3221 2:6f9881435f17 406
Nike3221 2:6f9881435f17 407 mag.getX(&con_x);//コンパス読み込み
Nike3221 2:6f9881435f17 408 mag.getY(&con_y);
Nike3221 2:6f9881435f17 409 offset(con_x,con_y);//x,y オフセット計算
Nike3221 2:6f9881435f17 410 cal_con();
Nike3221 2:6f9881435f17 411
Nike3221 2:6f9881435f17 412 gps_read();
Nike3221 2:6f9881435f17 413 gps_save();
Nike3221 2:6f9881435f17 414 cal_gps();
Nike3221 2:6f9881435f17 415 motor_r.pulsewidth(motor_palse/2);
Nike3221 2:6f9881435f17 416 motor_l.pulsewidth(motor_palse/4);}}
Nike3221 2:6f9881435f17 417
Nike3221 2:6f9881435f17 418 else if(direction <= direction_target+2 && direction >= direction_target-2)
Nike3221 2:6f9881435f17 419 {
Nike3221 2:6f9881435f17 420 pc.printf("front \n");
Nike3221 2:6f9881435f17 421 motor_l.pulsewidth(motor_palse*0.8); //パルス幅
Nike3221 2:6f9881435f17 422 motor_r.pulsewidth(motor_palse*0.8);
Nike3221 2:6f9881435f17 423 }
Nike3221 2:6f9881435f17 424
Nike3221 2:6f9881435f17 425 pc.printf("direction: %f direction_target: %f\n",direction,direction_target);
Nike3221 2:6f9881435f17 426
Nike3221 2:6f9881435f17 427 pc.printf("************turn_sequence end*************\n");
Nike3221 2:6f9881435f17 428
Nike3221 2:6f9881435f17 429 }
Nike3221 2:6f9881435f17 430
Nike3221 1:2992de38cf3e 431 void move()
Nike3221 1:2992de38cf3e 432 {
Nike3221 1:2992de38cf3e 433 switch(count_move)
Nike3221 1:2992de38cf3e 434 {
Nike3221 1:2992de38cf3e 435 case 1:
Nike3221 2:6f9881435f17 436
Nike3221 1:2992de38cf3e 437 pc.printf("*************case1**************\n");
Nike3221 2:6f9881435f17 438 motor_l.pulsewidth(0.01); //パルス幅
Nike3221 2:6f9881435f17 439 motor_r.pulsewidth(0.01);
Nike3221 1:2992de38cf3e 440 wait(10);//落下時間
Nike3221 1:2992de38cf3e 441
Nike3221 1:2992de38cf3e 442 servo.pulsewidth(servo_palse);
Nike3221 1:2992de38cf3e 443
Nike3221 1:2992de38cf3e 444 /*if(direction <= direction_target+3 && direction >= direction_target-3)
Nike3221 1:2992de38cf3e 445 {count_move++;}*/
Nike3221 1:2992de38cf3e 446 count_move++;
Nike3221 1:2992de38cf3e 447 pc.printf("*************case1 end**************\n");
Nike3221 1:2992de38cf3e 448 break;
Nike3221 2:6f9881435f17 449
Nike3221 1:2992de38cf3e 450 case 2:
Nike3221 2:6f9881435f17 451 __disable_irq();
Nike3221 1:2992de38cf3e 452 pc.printf("*************case2**************\n");
Nike3221 1:2992de38cf3e 453 pc.printf("direction: %f direction_target: %f\n",direction,direction_target);
Nike3221 1:2992de38cf3e 454 longitudegosa = longitude_target-longitude1;
Nike3221 1:2992de38cf3e 455 latitudegosa = latitude_target-latitude1;
Nike3221 2:6f9881435f17 456
Nike3221 2:6f9881435f17 457 if(direction > direction_target+2.0)
Nike3221 2:6f9881435f17 458 { while(direction > direction_target+2.0)
Nike3221 1:2992de38cf3e 459 {
Nike3221 2:6f9881435f17 460 pc.printf("turn clockwise \ndirection: %f direction_target: %f\n",direction,direction_target);
Nike3221 2:6f9881435f17 461
Nike3221 2:6f9881435f17 462 FILE *fp = fopen("/local/turn.txt","a");
Nike3221 2:6f9881435f17 463 fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
Nike3221 2:6f9881435f17 464 fclose(fp);
Nike3221 2:6f9881435f17 465
Nike3221 1:2992de38cf3e 466 mag.getX(&con_x);//コンパス読み込み
Nike3221 1:2992de38cf3e 467 mag.getY(&con_y);
Nike3221 1:2992de38cf3e 468 offset(con_x,con_y);//x,y オフセット計算
Nike3221 1:2992de38cf3e 469 cal_con();
Nike3221 2:6f9881435f17 470
Nike3221 2:6f9881435f17 471 /*gps_read();
Nike3221 2:6f9881435f17 472 gps_save();
Nike3221 2:6f9881435f17 473 cal_gps();回転しすぎる*/
Nike3221 2:6f9881435f17 474 motor_l.pulsewidth(motor_palse*0.7);
Nike3221 2:6f9881435f17 475 motor_r.pulsewidth(motor_palse*0.35);}
Nike3221 2:6f9881435f17 476 motor_r.pulsewidth(motor_palse);
Nike3221 2:6f9881435f17 477 motor_l.pulsewidth(motor_palse);
Nike3221 2:6f9881435f17 478 }
Nike3221 1:2992de38cf3e 479
Nike3221 2:6f9881435f17 480 else if(direction < direction_target-2.0)
Nike3221 2:6f9881435f17 481 { while(direction < direction_target-2.0)
Nike3221 1:2992de38cf3e 482 {
Nike3221 2:6f9881435f17 483 pc.printf("turn anti clockwise \ndirection: %f direction_target: %f\n",direction,direction_target);
Nike3221 2:6f9881435f17 484
Nike3221 2:6f9881435f17 485 FILE *fp = fopen("/local/turn2.txt","a");
Nike3221 2:6f9881435f17 486 fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
Nike3221 2:6f9881435f17 487 fclose(fp);
Nike3221 2:6f9881435f17 488
Nike3221 1:2992de38cf3e 489 mag.getX(&con_x);//コンパス読み込み
Nike3221 1:2992de38cf3e 490 mag.getY(&con_y);
Nike3221 1:2992de38cf3e 491 offset(con_x,con_y);//x,y オフセット計算
Nike3221 1:2992de38cf3e 492 cal_con();
Nike3221 1:2992de38cf3e 493
Nike3221 2:6f9881435f17 494 /*gps_read();
Nike3221 2:6f9881435f17 495 gps_save();
Nike3221 2:6f9881435f17 496 cal_gps();回転しすぎる*/
Nike3221 2:6f9881435f17 497 motor_r.pulsewidth(motor_palse*0.7);
Nike3221 2:6f9881435f17 498 motor_l.pulsewidth(motor_palse*0.35);}
Nike3221 2:6f9881435f17 499 motor_r.pulsewidth(motor_palse);
Nike3221 2:6f9881435f17 500 motor_l.pulsewidth(motor_palse);
Nike3221 2:6f9881435f17 501 }
Nike3221 2:6f9881435f17 502
Nike3221 2:6f9881435f17 503 if(direction <= direction_target+2 && direction >= direction_target-2)
Nike3221 2:6f9881435f17 504 { pc.printf("front \n");
Nike3221 2:6f9881435f17 505 motor_l.pulsewidth(motor_palse); //パルス幅
Nike3221 2:6f9881435f17 506 motor_r.pulsewidth(motor_palse);
Nike3221 2:6f9881435f17 507 count_move++;}
Nike3221 1:2992de38cf3e 508
Nike3221 1:2992de38cf3e 509 pc.printf("direction: %f direction_target: %f\n",direction,direction_target);
Nike3221 1:2992de38cf3e 510
Nike3221 1:2992de38cf3e 511 pc.printf("*************case2 end**************\n");
Nike3221 1:2992de38cf3e 512
Nike3221 1:2992de38cf3e 513 break;
Nike3221 2:6f9881435f17 514 case 3:
Nike3221 2:6f9881435f17 515 motor_l.pulsewidth(motor_palse*0.4); //パルス幅
Nike3221 2:6f9881435f17 516 motor_r.pulsewidth(motor_palse*0.4);
Nike3221 1:2992de38cf3e 517
Nike3221 2:6f9881435f17 518 if(direction >= direction_target+2 && direction <= direction_target-2)
Nike3221 2:6f9881435f17 519 { pc.printf("trun \n");
Nike3221 2:6f9881435f17 520 turn_sequence();}
Nike3221 2:6f9881435f17 521
Nike3221 2:6f9881435f17 522 gps_read();
Nike3221 2:6f9881435f17 523 gps_save();
Nike3221 2:6f9881435f17 524 cal_gps();
Nike3221 2:6f9881435f17 525
Nike3221 2:6f9881435f17 526 longitudegosa = longitude_target-longitude1;
Nike3221 2:6f9881435f17 527 latitudegosa = latitude_target-latitude1;
Nike3221 2:6f9881435f17 528
Nike3221 2:6f9881435f17 529 motor_l.pulsewidth(motor_palse*0.8); //パルス幅
Nike3221 2:6f9881435f17 530 motor_r.pulsewidth(motor_palse*0.8);
Nike3221 2:6f9881435f17 531
Nike3221 2:6f9881435f17 532 turn_sequence();
Nike3221 2:6f9881435f17 533
Nike3221 2:6f9881435f17 534 if( abs(longitudegosa)<=0.1 && abs(latitudegosa)<=0.1 )
Nike3221 2:6f9881435f17 535 { pc.printf("front \n");
Nike3221 2:6f9881435f17 536 count_move++; }
Nike3221 2:6f9881435f17 537
Nike3221 2:6f9881435f17 538 break;
Nike3221 2:6f9881435f17 539 case 4:
Nike3221 1:2992de38cf3e 540 pc.printf("*************case3**************\n");
Nike3221 2:6f9881435f17 541
Nike3221 2:6f9881435f17 542 __enable_irq();
Nike3221 1:2992de38cf3e 543 trigger();
Nike3221 1:2992de38cf3e 544 //distance.rise(rise_echo);
Nike3221 1:2992de38cf3e 545 //distance.fall(fall_echo);
Nike3221 1:2992de38cf3e 546
Nike3221 2:6f9881435f17 547 pc.printf("sig_time: %f\n",sig_time);
Nike3221 1:2992de38cf3e 548
Nike3221 2:6f9881435f17 549 if(sig_time <= 100.0)//100mm以下に近づいたとき停止
Nike3221 2:6f9881435f17 550 { pc.printf("sig_time is 100mm or under: %f\n",sig_time);
Nike3221 2:6f9881435f17 551 count_move++;}
Nike3221 1:2992de38cf3e 552 else
Nike3221 1:2992de38cf3e 553 {
Nike3221 1:2992de38cf3e 554 motor_l.pulsewidth(motor_palse/2); //パルス幅
Nike3221 1:2992de38cf3e 555 motor_r.pulsewidth(motor_palse/2);
Nike3221 1:2992de38cf3e 556 }
Nike3221 1:2992de38cf3e 557
Nike3221 1:2992de38cf3e 558 break;
Nike3221 1:2992de38cf3e 559
Nike3221 1:2992de38cf3e 560 default:
Nike3221 1:2992de38cf3e 561 motor_l.pulsewidth(0.0);
Nike3221 1:2992de38cf3e 562 motor_r.pulsewidth(0.0);
Nike3221 2:6f9881435f17 563
Nike3221 1:2992de38cf3e 564 break;
Nike3221 1:2992de38cf3e 565
Nike3221 1:2992de38cf3e 566 }
Nike3221 0:029ef267b1bc 567
Nike3221 0:029ef267b1bc 568 }
Nike3221 0:029ef267b1bc 569
Nike3221 0:029ef267b1bc 570
Nike3221 1:2992de38cf3e 571 void con_save()
Nike3221 1:2992de38cf3e 572 {
Nike3221 0:029ef267b1bc 573
Nike3221 0:029ef267b1bc 574 bit1=1;//led4を1にしてデータを保存
Nike3221 0:029ef267b1bc 575 FILE *fp = fopen("/local/commpas_new.txt","a");
Nike3221 1:2992de38cf3e 576 fprintf(fp,"%c,%f,%f,%f,%f\n",angle_c,d_target,x_dat,y_dat,direction);
Nike3221 1:2992de38cf3e 577 //fprintf(fp,"%d,%d,%d,%d,%d\n",x,y,z,offset_x,offset_y);
Nike3221 1:2992de38cf3e 578 fclose(fp);
Nike3221 1:2992de38cf3e 579 bit1=0;
Nike3221 0:029ef267b1bc 580 }
Nike3221 0:029ef267b1bc 581
Nike3221 1:2992de38cf3e 582 void dele()
Nike3221 1:2992de38cf3e 583 {
Nike3221 1:2992de38cf3e 584
Nike3221 0:029ef267b1bc 585 bit2=1;//led3を1
Nike3221 1:2992de38cf3e 586
Nike3221 0:029ef267b1bc 587 remove("/local/commpas_new.txt");
Nike3221 0:029ef267b1bc 588 remove("/local/gps.txt");
Nike3221 1:2992de38cf3e 589
Nike3221 0:029ef267b1bc 590 bit2=0;
Nike3221 0:029ef267b1bc 591 }
Nike3221 0:029ef267b1bc 592
Nike3221 1:2992de38cf3e 593 int main()
Nike3221 1:2992de38cf3e 594 {
Nike3221 1:2992de38cf3e 595 bit1 = 1;
Nike3221 1:2992de38cf3e 596 pc.printf("start\n");
Nike3221 1:2992de38cf3e 597 //__disable_irq();//割り込み禁止
Nike3221 1:2992de38cf3e 598 init(); //初期化
Nike3221 1:2992de38cf3e 599 //cds.rise(first_sequence);//cds立ち上がり割り込み
Nike3221 1:2992de38cf3e 600 cds.fall(&first_sequence);//cds落ち込み割り込み
Nike3221 1:2992de38cf3e 601 mag.sampleRate(0x40);
Nike3221 0:029ef267b1bc 602 mag.enable();//コンパス有効化
Nike3221 1:2992de38cf3e 603 mag.getX(&con_x);//コンパス読み込み
Nike3221 1:2992de38cf3e 604 mag.getY(&con_y);
Nike3221 1:2992de38cf3e 605 mag.getZ(&con_z);
Nike3221 2:6f9881435f17 606 for(count_offset=0; count_offset < 300; count_offset++)//300個分のサンプ取得
Nike3221 2:6f9881435f17 607 {
Nike3221 2:6f9881435f17 608 mag.getX(&con_x);//コンパス読み込み
Nike3221 2:6f9881435f17 609 mag.getY(&con_y);
Nike3221 2:6f9881435f17 610 mag.getZ(&con_z);
Nike3221 2:6f9881435f17 611 offset(con_x,con_y);
Nike3221 2:6f9881435f17 612 cal_con();
Nike3221 2:6f9881435f17 613
Nike3221 2:6f9881435f17 614 if(local_on2==1)
Nike3221 2:6f9881435f17 615 {
Nike3221 2:6f9881435f17 616 FILE *fp = fopen("/local/first_con.txt","a");
Nike3221 2:6f9881435f17 617 fprintf(fp,"%c,%d,%d,%f,%f,%f,%f\n",angle_c,con_x,con_y,x_dat,y_dat,direction_target,direction);
Nike3221 2:6f9881435f17 618 fclose(fp);
Nike3221 2:6f9881435f17 619 }
Nike3221 2:6f9881435f17 620 }
Nike3221 1:2992de38cf3e 621 bit1 = 0;
Nike3221 0:029ef267b1bc 622
Nike3221 1:2992de38cf3e 623 while(1) {
Nike3221 1:2992de38cf3e 624
Nike3221 1:2992de38cf3e 625 __enable_irq();//割り込み許可
Nike3221 1:2992de38cf3e 626 mag.getX(&con_x);//コンパス読み込み
Nike3221 1:2992de38cf3e 627 mag.getY(&con_y);
Nike3221 1:2992de38cf3e 628 offset(con_x,con_y);//x,y オフセット計算
Nike3221 1:2992de38cf3e 629 cal_con();
Nike3221 1:2992de38cf3e 630
Nike3221 1:2992de38cf3e 631 gps_read();
Nike3221 1:2992de38cf3e 632 gps_save();
Nike3221 1:2992de38cf3e 633 cal_gps();
Nike3221 1:2992de38cf3e 634
Nike3221 1:2992de38cf3e 635 /*if(cds == 0)
Nike3221 1:2992de38cf3e 636 {first_sequence();
Nike3221 1:2992de38cf3e 637 bit5=1;}*/
Nike3221 1:2992de38cf3e 638
Nike3221 1:2992de38cf3e 639 move();
Nike3221 1:2992de38cf3e 640
Nike3221 1:2992de38cf3e 641 if(local_on == 1)
Nike3221 1:2992de38cf3e 642 { con_save(); } //pin20がhighの時、データ保存
Nike3221 1:2992de38cf3e 643 /*if(local_del == 1)
Nike3221 1:2992de38cf3e 644 { dele(); }*/ //pin19がhighの時、保存したデータを消去
Nike3221 1:2992de38cf3e 645
Nike3221 1:2992de38cf3e 646 }
Nike3221 0:029ef267b1bc 647 }