伊生希 小林 / Mbed 2 deprecated cansat_main_copy

temp

Dependencies:   mbed SDFileSystem MS5607 ADXL345_I2C FATFileSystem

main.cpp@0:c88c3b616c00, 2020-03-16 (annotated)

Committer:
IKobayashi
Date:
Mon Mar 16 23:37:42 2020 +0900
Revision:
0:c88c3b616c00
copy

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IKobayashi 0:c88c3b616c00 1 #include<mbed.h>
IKobayashi 0:c88c3b616c00 2 #include"MS5607I2C.h"
IKobayashi 0:c88c3b616c00 3 #include"ADXL345_I2C.h"
IKobayashi 0:c88c3b616c00 4 #include "SDFileSystem.h"
IKobayashi 0:c88c3b616c00 5 #include "HMC5883L.h"
IKobayashi 0:c88c3b616c00 6 #include<stdio.h>
IKobayashi 0:c88c3b616c00 7 //#include<math.h>
IKobayashi 0:c88c3b616c00 8
IKobayashi 0:c88c3b616c00 9 #define SDA p9
IKobayashi 0:c88c3b616c00 10 #define SCL p10
IKobayashi 0:c88c3b616c00 11 #define Rs p21 //right servo motor
IKobayashi 0:c88c3b616c00 12 #define Ls p22 //left servo motor
IKobayashi 0:c88c3b616c00 13 #define PI 3.14159265
IKobayashi 0:c88c3b616c00 14 #define N_SAMPLE 256
IKobayashi 0:c88c3b616c00 15 #define WAIT_TIME 50
IKobayashi 0:c88c3b616c00 16 #define N_DIM 4
IKobayashi 0:c88c3b616c00 17 #define EPS 1e-8
IKobayashi 0:c88c3b616c00 18 #define goallat 4008.544783
IKobayashi 0:c88c3b616c00 19 #define goallon 13959.2389
IKobayashi 0:c88c3b616c00 20
IKobayashi 0:c88c3b616c00 21 SDFileSystem sd(p5, p6, p7, p8, "sd");
IKobayashi 0:c88c3b616c00 22 Serial pc(USBTX, USBRX);
IKobayashi 0:c88c3b616c00 23 Serial xbee(p28,p27);
IKobayashi 0:c88c3b616c00 24 Serial gps(p13, p14);
IKobayashi 0:c88c3b616c00 25 MS5607I2C ms5607(SDA, SCL, true);
IKobayashi 0:c88c3b616c00 26 ADXL345_I2C accelerometer(SDA, SCL);
IKobayashi 0:c88c3b616c00 27 HMC5883L hmc5883l(SDA, SCL);
IKobayashi 0:c88c3b616c00 28 Timer timer;
IKobayashi 0:c88c3b616c00 29 Ticker decision;
IKobayashi 0:c88c3b616c00 30 DigitalOut led1(LED1);
IKobayashi 0:c88c3b616c00 31 DigitalOut led2(LED2);
IKobayashi 0:c88c3b616c00 32 DigitalOut led3(LED3);
IKobayashi 0:c88c3b616c00 33 DigitalOut led4(LED4);
IKobayashi 0:c88c3b616c00 34 InterruptIn StartPort(p24);
IKobayashi 0:c88c3b616c00 35 PwmOut servoLeft(Ls);
IKobayashi 0:c88c3b616c00 36 PwmOut servoRight(Rs);
IKobayashi 0:c88c3b616c00 37
IKobayashi 0:c88c3b616c00 38 FILE *dt;
IKobayashi 0:c88c3b616c00 39 FILE *lo;
IKobayashi 0:c88c3b616c00 40
IKobayashi 0:c88c3b616c00 41 bool flight_pin = false;
IKobayashi 0:c88c3b616c00 42 bool gp = false;
IKobayashi 0:c88c3b616c00 43 int i,rlock,mode,num;
IKobayashi 0:c88c3b616c00 44 char gps_data[256];
IKobayashi 0:c88c3b616c00 45 char ns,ew;
IKobayashi 0:c88c3b616c00 46 float w_time,hokui,tokei;
IKobayashi 0:c88c3b616c00 47 float g_hokui,g_tokei;
IKobayashi 0:c88c3b616c00 48 // float d_hokui,m_hokui,d_tokei,m_tokei;
IKobayashi 0:c88c3b616c00 49 unsigned char c;
IKobayashi 0:c88c3b616c00 50 double dev = 0;
IKobayashi 0:c88c3b616c00 51 double gpsdata[4]; //gps data
IKobayashi 0:c88c3b616c00 52
IKobayashi 0:c88c3b616c00 53
IKobayashi 0:c88c3b616c00 54 void ini();
IKobayashi 0:c88c3b616c00 55 void FP();
IKobayashi 0:c88c3b616c00 56 void getGPS();
IKobayashi 0:c88c3b616c00 57 void getdata(double *aldata, double acdata[], double mgdata[]);
IKobayashi 0:c88c3b616c00 58 void servo(float x);
IKobayashi 0:c88c3b616c00 59 void correction(double ac[], double mg[], double delta[]);
IKobayashi 0:c88c3b616c00 60 void dev_calc(double exam[], double *s, double *t);
IKobayashi 0:c88c3b616c00 61 void calc_dir_lati_longi(double newl[], double *result);
IKobayashi 0:c88c3b616c00 62
IKobayashi 0:c88c3b616c00 63
IKobayashi 0:c88c3b616c00 64 int main(){
IKobayashi 0:c88c3b616c00 65 pc.baud(9600); //sekect baudrate
IKobayashi 0:c88c3b616c00 66 gps.baud(9600);
IKobayashi 0:c88c3b616c00 67 xbee.baud(9600);
IKobayashi 0:c88c3b616c00 68 double aldata;
IKobayashi 0:c88c3b616c00 69 double acdata[3];
IKobayashi 0:c88c3b616c00 70 double mgdata[3];
IKobayashi 0:c88c3b616c00 71 double delta[2];
IKobayashi 0:c88c3b616c00 72 double result;
IKobayashi 0:c88c3b616c00 73 double chigh = 2000;
IKobayashi 0:c88c3b616c00 74 double dhigh = 0;
IKobayashi 0:c88c3b616c00 75 int k;
IKobayashi 0:c88c3b616c00 76 mkdir("/sd/log", 0777);
IKobayashi 0:c88c3b616c00 77 mkdir("/sd/sensordata", 0777);
IKobayashi 0:c88c3b616c00 78 lo = fopen("/sd/log/log.txt", "a");
IKobayashi 0:c88c3b616c00 79 if(lo == NULL) {
IKobayashi 0:c88c3b616c00 80 error("Could not open file for write\n");
IKobayashi 0:c88c3b616c00 81 }
IKobayashi 0:c88c3b616c00 82 dt = fopen("/sd/sensordata/data.csv", "a");
IKobayashi 0:c88c3b616c00 83 if(dt == NULL) {
IKobayashi 0:c88c3b616c00 84 error("Could not open file for write\n");
IKobayashi 0:c88c3b616c00 85 }
IKobayashi 0:c88c3b616c00 86 fprintf(dt,"UTC,lon,lat,satnum,altitude,acce_x,acce_y,acce_z,mag_x,mag_y,mag_z\n");
IKobayashi 0:c88c3b616c00 87 fclose(dt);
IKobayashi 0:c88c3b616c00 88 pc.printf("Cansat activate.....\r\n");
IKobayashi 0:c88c3b616c00 89 xbee.printf("Cansat activate.....\r\n");
IKobayashi 0:c88c3b616c00 90 fprintf(lo,"Cansat activate.....\r\n");
IKobayashi 0:c88c3b616c00 91 pc.printf("make directory and start initialize\r\n");
IKobayashi 0:c88c3b616c00 92 xbee.printf("make directory and start initialize\r\n");
IKobayashi 0:c88c3b616c00 93 fprintf(lo,"make directory and start initialize\r\n");
IKobayashi 0:c88c3b616c00 94 ini(); //initialize
IKobayashi 0:c88c3b616c00 95 fclose(lo);
IKobayashi 0:c88c3b616c00 96
IKobayashi 0:c88c3b616c00 97 StartPort.mode(PullUp);
IKobayashi 0:c88c3b616c00 98 StartPort.fall(&FP);
IKobayashi 0:c88c3b616c00 99 servo(0);
IKobayashi 0:c88c3b616c00 100
IKobayashi 0:c88c3b616c00 101 while(flight_pin != true){
IKobayashi 0:c88c3b616c00 102 led1 =! led1;
IKobayashi 0:c88c3b616c00 103 wait(0.1);
IKobayashi 0:c88c3b616c00 104 }
IKobayashi 0:c88c3b616c00 105 led1 = 0;
IKobayashi 0:c88c3b616c00 106 while(true){
IKobayashi 0:c88c3b616c00 107 while(true){
IKobayashi 0:c88c3b616c00 108 gps.attach(getGPS,Serial::RxIrq);
IKobayashi 0:c88c3b616c00 109 if(gp == true)break;
IKobayashi 0:c88c3b616c00 110 }
IKobayashi 0:c88c3b616c00 111 gp=false;
IKobayashi 0:c88c3b616c00 112 getdata(&aldata,acdata,mgdata);
IKobayashi 0:c88c3b616c00 113 dhigh = chigh - aldata;
IKobayashi 0:c88c3b616c00 114 chigh = aldata;
IKobayashi 0:c88c3b616c00 115 dt = fopen("/sd/sensordata/data.csv", "a");
IKobayashi 0:c88c3b616c00 116 if(dt == NULL) {
IKobayashi 0:c88c3b616c00 117 error("Could not open file for write\n");
IKobayashi 0:c88c3b616c00 118 }
IKobayashi 0:c88c3b616c00 119 fprintf(dt,"%lf,%lf,%lf,%2.0lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",gpsdata[0],gpsdata[1],gpsdata[2],gpsdata[3],aldata,acdata[0],acdata[1],acdata[2],mgdata[0],mgdata[1],mgdata[2]);
IKobayashi 0:c88c3b616c00 120 fclose(dt);
IKobayashi 0:c88c3b616c00 121 correction(acdata,mgdata,delta);
IKobayashi 0:c88c3b616c00 122 calc_dir_lati_longi(gpsdata,&result);
IKobayashi 0:c88c3b616c00 123 dev_calc(delta,&result,&dev);
IKobayashi 0:c88c3b616c00 124 lo = fopen("/sd/log/log.txt", "a");
IKobayashi 0:c88c3b616c00 125 if(lo == NULL) {
IKobayashi 0:c88c3b616c00 126 error("Could not open file for write\n");
IKobayashi 0:c88c3b616c00 127 }
IKobayashi 0:c88c3b616c00 128 if (-180 < dev && dev <= -150) {
IKobayashi 0:c88c3b616c00 129 servo(0);
IKobayashi 0:c88c3b616c00 130 fprintf(lo,"Left:180\r\n");
IKobayashi 0:c88c3b616c00 131 pc.printf("Left:180\r\n");
IKobayashi 0:c88c3b616c00 132 }
IKobayashi 0:c88c3b616c00 133 else if (-150 < dev && dev <= -120) {
IKobayashi 0:c88c3b616c00 134 servo(30);
IKobayashi 0:c88c3b616c00 135 fprintf(lo,"Left:150\r\n");
IKobayashi 0:c88c3b616c00 136 pc.printf("Left:150\r\n");
IKobayashi 0:c88c3b616c00 137 }
IKobayashi 0:c88c3b616c00 138 else if (-120 < dev && dev <= -90) {
IKobayashi 0:c88c3b616c00 139 servo(60);
IKobayashi 0:c88c3b616c00 140 fprintf(lo,"Left:120\r\n");
IKobayashi 0:c88c3b616c00 141 pc.printf("Left:120\r\n");
IKobayashi 0:c88c3b616c00 142 }
IKobayashi 0:c88c3b616c00 143 else if (-90 < dev && dev <= -60) {
IKobayashi 0:c88c3b616c00 144 servo(90);
IKobayashi 0:c88c3b616c00 145 fprintf(lo,"Left:90\r\n");
IKobayashi 0:c88c3b616c00 146 pc.printf("Left:90\r\n");
IKobayashi 0:c88c3b616c00 147 }
IKobayashi 0:c88c3b616c00 148 else if (-60 < dev && dev <= -30) {
IKobayashi 0:c88c3b616c00 149 servo(120);
IKobayashi 0:c88c3b616c00 150 fprintf(lo,"Left:60\r\n");
IKobayashi 0:c88c3b616c00 151 pc.printf("Left:60\r\n");
IKobayashi 0:c88c3b616c00 152 }
IKobayashi 0:c88c3b616c00 153 else if(-30 < dev && dev < 0) {
IKobayashi 0:c88c3b616c00 154 servo(150);
IKobayashi 0:c88c3b616c00 155 fprintf(lo,"Left:30\r\n");
IKobayashi 0:c88c3b616c00 156 pc.printf("Left:30\r\n");
IKobayashi 0:c88c3b616c00 157 }
IKobayashi 0:c88c3b616c00 158 else if (dev == 0) {
IKobayashi 0:c88c3b616c00 159 servo(0);
IKobayashi 0:c88c3b616c00 160 fprintf(lo,"Straight\r\n");
IKobayashi 0:c88c3b616c00 161 pc.printf("Straight\r\n");
IKobayashi 0:c88c3b616c00 162 }
IKobayashi 0:c88c3b616c00 163 else if (0 < dev && dev <= 30) {
IKobayashi 0:c88c3b616c00 164 servo(30);
IKobayashi 0:c88c3b616c00 165 fprintf(lo,"Right:30\r\n");
IKobayashi 0:c88c3b616c00 166 pc.printf("Right:30\r\n");
IKobayashi 0:c88c3b616c00 167 }
IKobayashi 0:c88c3b616c00 168 else if (30 < dev && dev <= 60) {
IKobayashi 0:c88c3b616c00 169 servo(60);
IKobayashi 0:c88c3b616c00 170 fprintf(lo,"Right:60\r\n");
IKobayashi 0:c88c3b616c00 171 pc.printf("Right:60\r\n");
IKobayashi 0:c88c3b616c00 172 }
IKobayashi 0:c88c3b616c00 173 else if (60 < dev && dev <= 90) {
IKobayashi 0:c88c3b616c00 174 servo(90);
IKobayashi 0:c88c3b616c00 175 fprintf(lo,"Right:90\r\n");
IKobayashi 0:c88c3b616c00 176 pc.printf("Right:90\r\n");
IKobayashi 0:c88c3b616c00 177 }
IKobayashi 0:c88c3b616c00 178 else if (90 < dev && dev <= 120) {
IKobayashi 0:c88c3b616c00 179 servo(120);
IKobayashi 0:c88c3b616c00 180 fprintf(lo,"Right:120\r\n");
IKobayashi 0:c88c3b616c00 181 pc.printf("Right:120\r\n");
IKobayashi 0:c88c3b616c00 182 }
IKobayashi 0:c88c3b616c00 183 else if (120 < dev && dev <= 150) {
IKobayashi 0:c88c3b616c00 184 servo(150);
IKobayashi 0:c88c3b616c00 185 fprintf(lo,"Right:150\r\n");
IKobayashi 0:c88c3b616c00 186 pc.printf("Right:150\r\n");
IKobayashi 0:c88c3b616c00 187 }
IKobayashi 0:c88c3b616c00 188 else if (150 < dev && dev <= 180) {
IKobayashi 0:c88c3b616c00 189 servo(180);
IKobayashi 0:c88c3b616c00 190 fprintf(lo,"Right:180\r\n");
IKobayashi 0:c88c3b616c00 191 pc.printf("Right:180\r\n");
IKobayashi 0:c88c3b616c00 192 }
IKobayashi 0:c88c3b616c00 193 fclose(lo);
IKobayashi 0:c88c3b616c00 194 /*
IKobayashi 0:c88c3b616c00 195 if (-0.1 < dhigh < 0.1) {
IKobayashi 0:c88c3b616c00 196 k++;
IKobayashi 0:c88c3b616c00 197 if (k >= 20) {
IKobayashi 0:c88c3b616c00 198 break;
IKobayashi 0:c88c3b616c00 199 }
IKobayashi 0:c88c3b616c00 200 }
IKobayashi 0:c88c3b616c00 201 else {
IKobayashi 0:c88c3b616c00 202 k = 0;
IKobayashi 0:c88c3b616c00 203 }
IKobayashi 0:c88c3b616c00 204 */
IKobayashi 0:c88c3b616c00 205 }
IKobayashi 0:c88c3b616c00 206 pc.printf("Cansat touchdown.....\r\n");
IKobayashi 0:c88c3b616c00 207 xbee.printf("Cansat touchdown.....\r\n");
IKobayashi 0:c88c3b616c00 208 fprintf(lo,"Cansat touchdown.....\r\n");
IKobayashi 0:c88c3b616c00 209 fclose(lo);
IKobayashi 0:c88c3b616c00 210 while(true){
IKobayashi 0:c88c3b616c00 211 led4=!led4;
IKobayashi 0:c88c3b616c00 212 }
IKobayashi 0:c88c3b616c00 213 return 0;
IKobayashi 0:c88c3b616c00 214 }
IKobayashi 0:c88c3b616c00 215
IKobayashi 0:c88c3b616c00 216 void ini(){
IKobayashi 0:c88c3b616c00 217 //accelermeter initialization
IKobayashi 0:c88c3b616c00 218 accelerometer.setPowerControl(0x00); //Go into standby mode to configure the device.
IKobayashi 0:c88c3b616c00 219 accelerometer.setDataFormatControl(0x0B); //Full resolution, +/-16g, 4mg/LSB.
IKobayashi 0:c88c3b616c00 220 accelerometer.setDataRate(ADXL345_3200HZ); //3.2kHz data rate.
IKobayashi 0:c88c3b616c00 221 accelerometer.setPowerControl(0x08); //Measurement mode.
IKobayashi 0:c88c3b616c00 222 pc.printf("All sensor is initialized\r\n");
IKobayashi 0:c88c3b616c00 223 xbee.printf("All sensor is initialized\r\n");
IKobayashi 0:c88c3b616c00 224 fprintf(lo,"All sensor is initialized\r\n");
IKobayashi 0:c88c3b616c00 225 }
IKobayashi 0:c88c3b616c00 226
IKobayashi 0:c88c3b616c00 227 void FP(){
IKobayashi 0:c88c3b616c00 228 flight_pin = true;
IKobayashi 0:c88c3b616c00 229 }
IKobayashi 0:c88c3b616c00 230
IKobayashi 0:c88c3b616c00 231 void getGPS() {
IKobayashi 0:c88c3b616c00 232
IKobayashi 0:c88c3b616c00 233 c = gps.getc();
IKobayashi 0:c88c3b616c00 234 if( c=='$' || i == 256){
IKobayashi 0:c88c3b616c00 235 mode = 0;
IKobayashi 0:c88c3b616c00 236 i = 0;
IKobayashi 0:c88c3b616c00 237 for(int j=0; j<256; j++){
IKobayashi 0:c88c3b616c00 238 gps_data[j]=NULL;
IKobayashi 0:c88c3b616c00 239 }
IKobayashi 0:c88c3b616c00 240 }
IKobayashi 0:c88c3b616c00 241 if(mode==0){
IKobayashi 0:c88c3b616c00 242 if((gps_data[i]=c) != '\r'){
IKobayashi 0:c88c3b616c00 243 i++;
IKobayashi 0:c88c3b616c00 244 }else{
IKobayashi 0:c88c3b616c00 245 gps_data[i]='\0';
IKobayashi 0:c88c3b616c00 246
IKobayashi 0:c88c3b616c00 247 if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock,&num) >= 1){
IKobayashi 0:c88c3b616c00 248 led1=!led1;
IKobayashi 0:c88c3b616c00 249 if(rlock==1){
IKobayashi 0:c88c3b616c00 250 //pc.printf("Status:Lock(%d)\n\r",rlock);
IKobayashi 0:c88c3b616c00 251 //logitude
IKobayashi 0:c88c3b616c00 252 // d_tokei= int(tokei/100);
IKobayashi 0:c88c3b616c00 253 // m_tokei= (tokei-d_tokei*100)/60;
IKobayashi 0:c88c3b616c00 254 // g_tokei= d_tokei+m_tokei;
IKobayashi 0:c88c3b616c00 255 //Latitude
IKobayashi 0:c88c3b616c00 256 // d_hokui=int(hokui/100);
IKobayashi 0:c88c3b616c00 257 // m_hokui=(hokui-d_hokui*100)/60;
IKobayashi 0:c88c3b616c00 258 // g_hokui=d_hokui+m_hokui;
IKobayashi 0:c88c3b616c00 259 pc.printf("Lon:%.6f, Lat:%.6f, num:%d\n\r",tokei, hokui, num);
IKobayashi 0:c88c3b616c00 260 xbee.printf("Lon:%.6f, Lat:%.6f, num:%d\n\r",tokei, hokui, num);
IKobayashi 0:c88c3b616c00 261 gpsdata[0] = (double)w_time;
IKobayashi 0:c88c3b616c00 262 gpsdata[2] = (double)tokei;
IKobayashi 0:c88c3b616c00 263 gpsdata[1] = (double)hokui;
IKobayashi 0:c88c3b616c00 264 gpsdata[3] = (double)num;
IKobayashi 0:c88c3b616c00 265 gp = true;
IKobayashi 0:c88c3b616c00 266 }
IKobayashi 0:c88c3b616c00 267 else{
IKobayashi 0:c88c3b616c00 268 // pc.printf("\n\rStatus:unLock(%d)\n\r",rlock);
IKobayashi 0:c88c3b616c00 269 pc.printf("%s\r\n",gps_data);
IKobayashi 0:c88c3b616c00 270 //xbee.printf("\n\rStatus:unLock(%d)\n\r",rlock);
IKobayashi 0:c88c3b616c00 271 xbee.printf("%s\r\n",gps_data);
IKobayashi 0:c88c3b616c00 272 // gp = true;
IKobayashi 0:c88c3b616c00 273 }
IKobayashi 0:c88c3b616c00 274 sprintf(gps_data, "");
IKobayashi 0:c88c3b616c00 275 }//if
IKobayashi 0:c88c3b616c00 276 }
IKobayashi 0:c88c3b616c00 277 }
IKobayashi 0:c88c3b616c00 278 }
IKobayashi 0:c88c3b616c00 279
IKobayashi 0:c88c3b616c00 280 void getdata(double *aldata, double acdata[], double mgdata[]){
IKobayashi 0:c88c3b616c00 281 led2=!led2;
IKobayashi 0:c88c3b616c00 282 int readings[3];
IKobayashi 0:c88c3b616c00 283 *aldata = ms5607.getAltitude();
IKobayashi 0:c88c3b616c00 284 accelerometer.getOutput(readings);
IKobayashi 0:c88c3b616c00 285 acdata[0] = (int16_t)readings[1]*0.039;
IKobayashi 0:c88c3b616c00 286 acdata[1] = (int16_t)readings[0]*0.039*(-1);
IKobayashi 0:c88c3b616c00 287 acdata[2] = (int16_t)readings[2]*0.039;
IKobayashi 0:c88c3b616c00 288 mgdata[0] = (double)hmc5883l.getMy();
IKobayashi 0:c88c3b616c00 289 mgdata[1] = (double)hmc5883l.getMx()*(-1);
IKobayashi 0:c88c3b616c00 290 mgdata[2] = (double)hmc5883l.getMz();
IKobayashi 0:c88c3b616c00 291 }
IKobayashi 0:c88c3b616c00 292 void correction(double ac[], double mg[], double delta[2]) {
IKobayashi 0:c88c3b616c00 293 double Hx, Hy, r, p;
IKobayashi 0:c88c3b616c00 294 if ((-1 < ac[0])&&(ac[0] < 1) && (-1 < (ac[1] / sqrt(1 - ac[0] * ac[0])))&&((ac[1] / sqrt(1 - ac[0] * ac[0])) < 1)) {
IKobayashi 0:c88c3b616c00 295 led3=!led3;
IKobayashi 0:c88c3b616c00 296 r = asin(ac[0]);
IKobayashi 0:c88c3b616c00 297 p = asin(ac[1] / sqrt(1 - ac[0] * ac[0]));
IKobayashi 0:c88c3b616c00 298 Hx = cos(r) * mg[0] + sin(p) * sin(r) * mg[1] - cos(p) * sin(r) * mg[2];
IKobayashi 0:c88c3b616c00 299 Hy = cos(p) * mg[1] + sin(p) * mg[2];
IKobayashi 0:c88c3b616c00 300 delta[0]=Hx;
IKobayashi 0:c88c3b616c00 301 delta[1]=Hy;
IKobayashi 0:c88c3b616c00 302 /*
IKobayashi 0:c88c3b616c00 303 *delta = atan2(Hy, Hx);
IKobayashi 0:c88c3b616c00 304 if (*delta < 0)
IKobayashi 0:c88c3b616c00 305 *delta += 2 * PI;
IKobayashi 0:c88c3b616c00 306 if (*delta > 2 * PI)
IKobayashi 0:c88c3b616c00 307 *delta -= 2 * PI;
IKobayashi 0:c88c3b616c00 308 *delta = *delta * 180 / PI;
IKobayashi 0:c88c3b616c00 309 */
IKobayashi 0:c88c3b616c00 310 }
IKobayashi 0:c88c3b616c00 311 }
IKobayashi 0:c88c3b616c00 312
IKobayashi 0:c88c3b616c00 313 void servo(float x) {
IKobayashi 0:c88c3b616c00 314
IKobayashi 0:c88c3b616c00 315 float y;
IKobayashi 0:c88c3b616c00 316 y = 0.000284*(x / 30) + 0.0007; //left
IKobayashi 0:c88c3b616c00 317
IKobayashi 0:c88c3b616c00 318 if (dev < 0) {
IKobayashi 0:c88c3b616c00 319 servoLeft.pulsewidth(y);
IKobayashi 0:c88c3b616c00 320 }
IKobayashi 0:c88c3b616c00 321 else if(dev == 0){
IKobayashi 0:c88c3b616c00 322 servoLeft.pulsewidth(y+0.000284*(180 / 30));
IKobayashi 0:c88c3b616c00 323 servoRight.pulsewidth(y);
IKobayashi 0:c88c3b616c00 324 }
IKobayashi 0:c88c3b616c00 325 else {
IKobayashi 0:c88c3b616c00 326 servoRight.pulsewidth(y);
IKobayashi 0:c88c3b616c00 327 }
IKobayashi 0:c88c3b616c00 328
IKobayashi 0:c88c3b616c00 329 }
IKobayashi 0:c88c3b616c00 330 void dev_calc(double exam[], double *s, double *t) {
IKobayashi 0:c88c3b616c00 331 double nowdir;
IKobayashi 0:c88c3b616c00 332 nowdir = atan(exam[1] / exam[0]);
IKobayashi 0:c88c3b616c00 333 if(nowdir < 0)nowdir += 2*PI;
IKobayashi 0:c88c3b616c00 334 if(nowdir > 2*PI)nowdir -= 2*PI;
IKobayashi 0:c88c3b616c00 335 nowdir = nowdir * 180 / PI;
IKobayashi 0:c88c3b616c00 336 if (*s > nowdir) {
IKobayashi 0:c88c3b616c00 337 if ((180 - (*s - nowdir)) < 0) {
IKobayashi 0:c88c3b616c00 338 *t = *s - nowdir - 360; //dir_lati_longi:*s dev;*t
IKobayashi 0:c88c3b616c00 339 }
IKobayashi 0:c88c3b616c00 340 else {
IKobayashi 0:c88c3b616c00 341 *t = *s - nowdir;
IKobayashi 0:c88c3b616c00 342 }
IKobayashi 0:c88c3b616c00 343 }
IKobayashi 0:c88c3b616c00 344 else {
IKobayashi 0:c88c3b616c00 345 if ((180 - (nowdir - *s)) < 0) {
IKobayashi 0:c88c3b616c00 346 *t = 360 - nowdir + *s;
IKobayashi 0:c88c3b616c00 347 }
IKobayashi 0:c88c3b616c00 348 else {
IKobayashi 0:c88c3b616c00 349 *t = *s - nowdir;
IKobayashi 0:c88c3b616c00 350 }
IKobayashi 0:c88c3b616c00 351 }
IKobayashi 0:c88c3b616c00 352 pc.printf("%lf\r\n",*t);
IKobayashi 0:c88c3b616c00 353 }
IKobayashi 0:c88c3b616c00 354 void calc_dir_lati_longi(double newl[], double *result) {
IKobayashi 0:c88c3b616c00 355 double diflongi, diflati; //difference
IKobayashi 0:c88c3b616c00 356 diflati = (goallat) - newl[1]; //difference
IKobayashi 0:c88c3b616c00 357 diflongi = (goallon) - newl[2];
IKobayashi 0:c88c3b616c00 358 if(diflongi == 0 && diflati >= 0)*result = 0;
IKobayashi 0:c88c3b616c00 359 else if(diflongi == 0 && diflati < 0)*result = 180;
IKobayashi 0:c88c3b616c00 360 else if(diflongi > 0 && diflati == 0)*result = 90;
IKobayashi 0:c88c3b616c00 361 else if(diflongi < 0 && diflati == 0)*result = 270;
IKobayashi 0:c88c3b616c00 362 else if (diflongi >= 0 && diflati >= 0) { //1
IKobayashi 0:c88c3b616c00 363 *result = atan(diflongi / diflati) * 180 / PI;
IKobayashi 0:c88c3b616c00 364 }
IKobayashi 0:c88c3b616c00 365 else if (diflongi >= 0 && diflati < 0) { //4
IKobayashi 0:c88c3b616c00 366 *result = atan(diflongi / diflati) * 180 / PI + 180;
IKobayashi 0:c88c3b616c00 367 }
IKobayashi 0:c88c3b616c00 368 else if (diflongi < 0 && diflati >= 0) { //2
IKobayashi 0:c88c3b616c00 369 *result = atan(diflongi / diflati) * 180 / PI + 360;
IKobayashi 0:c88c3b616c00 370 }
IKobayashi 0:c88c3b616c00 371 else if (diflongi < 0 && diflati < 0) { //3
IKobayashi 0:c88c3b616c00 372 *result = atan(diflongi / diflati) * 180 / PI + 180;
IKobayashi 0:c88c3b616c00 373 }
IKobayashi 0:c88c3b616c00 374 }