統合プログラム
Dependencies: mbed Servo BMP180
Revision 8:7209c810309d, committed 2021-11-01
- Comitter:
- tsubasa_nakajima
- Date:
- Mon Nov 01 16:52:17 2021 +0000
- Parent:
- 7:74994694ec04
- Commit message:
- completed
Changed in this revision
diff -r 74994694ec04 -r 7209c810309d BMP180.h --- a/BMP180.h Thu Oct 28 14:44:25 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,156 +0,0 @@ -/* - @file BMP180.h - - @brief Barometric Pressure and Temperature Sensor BMP180 Breakout I2C Library - - @Author spiridion (http://mailme.spiridion.net) - - Tested on LPC1768 and FRDM-KL25Z - - Copyright (c) 2014 spiridion - Released under the MIT License (see http://mbed.org/license/mit) - - Documentation regarding the BMP180 can be found here: - http://mbed.org/media/uploads/spiridion/bst-bmp180-ds000-09.pdf -*/ - -#ifndef BMP180_H -#define BMP180_H - -#include "mbed.h" - -/// default address is 0xEF -#define BMP180_I2C_ADDRESS 0xEF - -// Oversampling settings -#define BMP180_OSS_ULTRA_LOW_POWER 0 // 1 sample and 4.5ms for conversion -#define BMP180_OSS_NORMAL 1 // 2 samples and 7.5ms for conversion -#define BMP180_OSS_HIGH_RESOLUTION 2 // 4 samples and 13.5ms for conversion -#define BMP180_OSS_ULTRA_HIGH_RESOLUTION 3 // 8 samples and 25.5ms for conversion - -#define UNSET_BMP180_PRESSURE_VALUE 0.F -#define UNSET_BMP180_TEMPERATURE_VALUE -273.15F // absolute zero - -/** BMP180 class. - * Read Pressure and temperature from the BMP180 Breakout I2C sensor - * - * Example: - * @code - * #include "mbed.h" - * #include "BMP180.h" - * - * #if defined(TARGET_LPC1768) - * #define PIN_SDA p9 - * #define PIN_SCL p10 - * #elif defined(TARGET_KL25Z) // watch out for the PTE0/PTE1 mixed up in the KL25Z doc - * #define PIN_SDA PTE0 - * #define PIN_SCL PTE1 - * #endif - * - * int main() - * { - * BMP180 bmp180(PIN_SDA, PIN_SCL); - * float pressure, temperature; - * - * // bmp180.Initialize(); // no altitude compensation and normal oversampling - * bmp180.Initialize(64, BMP180_OSS_ULTRA_LOW_POWER); // 64m altitude compensation and low power oversampling - * - * while(1) - * { - * if (bmp180.ReadData(&pressure, &temperature)) - * printf("Pressure(hPa): %8.2f \t Temperature(C): %8.2f\n", pressure, temperature); - * wait(1); - * } - * } - * @endcode - */ -class BMP180 -{ - -public: - - /** Create a BMP180 instance - * @param sda pin - * @param scl pin - * @param address: I2C slave address - */ - BMP180(PinName sda, PinName scl, int address = BMP180_I2C_ADDRESS); - - /** Create a BMP180 instance - * @param i2c object - * @param address: I2C slave address - */ - BMP180(I2C& i2c, int address = BMP180_I2C_ADDRESS); - - /** Initialization: set member values and read BMP180 calibration parameters - * @param altitude (in meter) - * @param overSamplingSetting - */ - int Initialize(float altitude = 0.F, int overSamplingSetting = BMP180_OSS_NORMAL); - - /** Read pressure and temperature from the BMP180. - * @param pressure (hPa) - * @param temperature (C) - * @returns - * 1 on success, - * 0 on error - */ - int ReadData(float* pTemperature = NULL, float* pPressure = NULL); - - /** Get temperature from a previous measurement - * - * @returns - * temperature (C) - */ - float GetTemperature() {return m_temperature;}; - - /** Get pressure from a previous measurement - * - * @returns - * pressure (hPa) - */ - float GetPressure() {return m_pressure;}; - -protected: - - /** Perform temperature measurement - * - * @returns - * temperature (C) - */ - int ReadRawTemperature(long* pUt); - - /** Perform pressure measurement - * - * @returns - * temperature (C) - */ - int ReadRawPressure(long* pUp); - - /** Calculation of the temperature from the digital output - */ - float TrueTemperature(long ut); - - /** Calculation of the pressure from the digital output - */ - float TruePressure(long up); - - int m_oss; - float m_temperature; - float m_pressure; - float m_altitude; - - I2C m_i2c; - int m_addr; - char m_data[4]; - - short ac1, ac2, ac3; - unsigned short ac4, ac5, ac6; - short b1, b2; - short mb, mc, md; - long x1, x2, x3, b3, b5, b6; - unsigned long b4, b7; - -}; - -#endif \ No newline at end of file
diff -r 74994694ec04 -r 7209c810309d BMP180.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BMP180.lib Mon Nov 01 16:52:17 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/spiridion/code/BMP180/#072073c79cfd
diff -r 74994694ec04 -r 7209c810309d Landing_Judgement.h --- a/Landing_Judgement.h Thu Oct 28 14:44:25 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,81 +0,0 @@ -#include "mbed.h" -#include "BMP180.h" -#include "calculate.h" -#define PIN_SDA D4 -#define PIN_SCL D5 - -class Landing_Judgement:public BMP180{ - private: - - int x ,y ,n1 , n2 ; - int landing_judgement ; - float h,dp,dt,dp0; - float a ,b ,dp_ave,dt_ave; - //hは高度、dpは気圧、dtは温度、dp0は海面気圧 - - public: - /*ここの関数をlanding_judgementの中に入れ込んじゃいます - //landing()が1を返せば着地、0を返せば未着地 - int landing(){ - return landing_judgement; - } - */ - - //着地判定の計算を開始させる関数 - int land(){ - - BMP180 bmp180(PIN_SDA,PIN_SCL); - landing_judgement = 0; - x = 0; - y = 0; - n1 = 0; - n2 = 0; - a = 0; - b = 0; - - for(int i=0;i<15;i++){ - if(bmp180.ReadData(&dt,&dp)){ - a = a + dp; - b = b + dt; - n1 = n1 + 1; - n2 = n2 + 1; - wait(1); - } - } - - dp_ave = a / n1; - dt_ave = b / n2; - dp0 = calculate_dp0(dp_ave,dt_ave); - - while(x<10){ - if(bmp180.ReadData(&dt,&dp)){ - h = calculate_h(dp0,dp,dt); - if(h >= 30){ - x = x + 1; - } - wait(1); - } - } - //10秒以上高度30mにいた場合離陸判定 - - wait(10); - - while(y<10){ - if(bmp180.ReadData(&dt,&dp)){ - h = calculate_h(dp0,dp,dt); - if(h <= 10){ - y = y + 1; - } - wait(1); - } - } - - wait(5); - - landing_judgement = landing_judgement + 1; - return landing_judgement; - - //離陸判定後、10秒以上高度10m以下にいた場合着地判定 - -} -};
diff -r 74994694ec04 -r 7209c810309d Movement.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Movement.cpp Mon Nov 01 16:52:17 2021 +0000 @@ -0,0 +1,79 @@ +#include "mbed.h" +#include "Servo.h" +#include "Movement.h" + +Servo servo1(D7); +Servo servo2(A3); +Servo servo3(A1); +Servo servo4(D12); +Servo servo5(D10); +Servo servo6(A5); + + void Movement::stop(){ + servo1 = 0.5; + servo2 = 0.5; + servo3 = 0.5; + servo4 = 0.5; + servo5 = 0.5; + servo6 = 0.5; + wait(1); + } + +//前進 + void Movement::move_forward(int time = 20) + { + servo1 = 0; + servo2 = 0; + servo3 = 0; + servo4 = 0; + servo5 = 0; + servo6 = 0; + wait(time); + } + +//後退 + void Movement::move_backward() + { + servo1 = 1; + servo2 = 1; + servo3 = 1; + servo4 = 1; + servo5 = 1; + servo6 = 1; + wait(5); + } + +//右に曲がる + void Movement::turn_right(int theta = 15) + { + servo1 = 1; + servo2 = 1; + servo3 = 1; + servo4 = 0; + servo5 = 0; + servo6 = 0; + wait(theta/15); + } + +//左に曲がる + void Movement::turn_left(int theta = 15) + { + servo1 = 0; + servo2 = 0; + servo3 = 0; + servo4 = 1; + servo5 = 1; + servo6 = 1; + wait(theta/15); + } + +//倒れているときの処理 +void Movement::wakeup(int time){ + for(int i=1;i<=time;i++) + { + move_forward(5); + move_backward(); + turn_right(); + turn_left(); + } +} \ No newline at end of file
diff -r 74994694ec04 -r 7209c810309d Movement.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Movement.h Mon Nov 01 16:52:17 2021 +0000 @@ -0,0 +1,25 @@ +#ifndef MOVEMENT_H +#define MOVEMENT_H + +#include "mbed.h" +#include "Servo.h" + +class Movement +{ +public: + +//停止 + void stop(); +//前進 + void move_forward(int time); +//後退 + void move_backward(); +//右に曲がる + void turn_right(int theta); +//左に曲がる + void turn_left(int theta); +//横転から復帰 + void wakeup(int time); +}; + +#endif \ No newline at end of file
diff -r 74994694ec04 -r 7209c810309d Servo.h --- a/Servo.h Thu Oct 28 14:44:25 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,98 +0,0 @@ -/* mbed R/C Servo Library - * Copyright (c) 2007-2010 sford, cstyles - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifndef MBED_SERVO_H -#define MBED_SERVO_H - -#include "mbed.h" - -/** Servo control class, based on a PwmOut - * - * Example: - * @code - * // Continuously sweep the servo through it's full range - * #include "mbed.h" - * #include "Servo.h" - * - * Servo myservo(p21); - * - * int main() { - * while(1) { - * for(int i=0; i<100; i++) { - * myservo = i/100.0; - * wait(0.01); - * } - * for(int i=100; i>0; i--) { - * myservo = i/100.0; - * wait(0.01); - * } - * } - * } - * @endcode - */ -class Servo { - -public: - /** Create a servo object connected to the specified PwmOut pin - * - * @param pin PwmOut pin to connect to - */ - Servo(PinName pin); - - /** Set the servo position, normalised to it's full range - * - * @param percent A normalised number 0.0-1.0 to represent the full range. - */ - void write(float percent); - - /** Read the servo motors current position - * - * @param returns A normalised number 0.0-1.0 representing the full range. - */ - float read(); - - /** Set the servo position - * - * @param degrees Servo position in degrees - */ - void position(float degrees); - - /** Allows calibration of the range and angles for a particular servo - * - * @param range Pulsewidth range from center (1.5ms) to maximum/minimum position in seconds - * @param degrees Angle from centre to maximum/minimum position in degrees - */ - void calibrate(float range = 0.0005, float degrees = 45.0); - - /** Shorthand for the write and read functions */ - Servo& operator= (float percent); - Servo& operator= (Servo& rhs); - operator float(); - -protected: - PwmOut _pwm; - float _range; - float _degrees; - float _p; -}; - -#endif
diff -r 74994694ec04 -r 7209c810309d Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Mon Nov 01 16:52:17 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07
diff -r 74994694ec04 -r 7209c810309d calculate.h --- a/calculate.h Thu Oct 28 14:44:25 2021 +0000 +++ b/calculate.h Mon Nov 01 16:52:17 2021 +0000 @@ -1,10 +1,11 @@ #include "mbed.h" - + //nの階乗を計算する関数 -int fact(int n){ - int i, result = 1; +int fact(int n) +{ + int i, result = 1; - if(n == 0) + if(n == 0) return 1; else { @@ -15,7 +16,7 @@ return result; } } - + float my_pow(float x, int n) { int i; @@ -32,39 +33,39 @@ return pow_result; } } - + float my_exp(float x) { int i; float result = 0; - - + + for(i = 1;i <= 25; i++) { result += my_pow(x, i) / fact(i); } - + return result + 1; } - + float my_log(float x) { int i; float result1, result2; - + x -= 1; result1 = 0; result2 = 0; - + for(i = 1;i <= 40;i++) { if(i % 2 == 1) result1 += my_pow(x, i) / i; else - + result2 += my_pow(x, i) / i; } - + return result1 - result2; } @@ -73,7 +74,7 @@ { return my_exp(y * my_log(x)); } - + //高度計算 float calculate_h(float dP0FIX,float dp,float dt){ float dpow = 1.0/5.256; @@ -82,7 +83,7 @@ float s = (mypow(dP0/dp,dpow)- mypow(dP0/dP0FIX,dpow))*a - 27; return s; } - + //海面気圧計算 float calculate_dp0(float dp,float dt){ float s = dp*mypow(1 - (0.0065*27)/(dt+0.0065*27+273.15),-5.257);
diff -r 74994694ec04 -r 7209c810309d direction.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/direction.cpp Mon Nov 01 16:52:17 2021 +0000 @@ -0,0 +1,266 @@ +#include "mbed.h" +#include "getGPS.h" +#include "Movement.h" +#include "direction.h" + + +// 球面三角法により、大円距離(メートル)を求める +double distance1(double lat1, double lng1, double lat2, double lng2){ + // 円周率 + const double pi = 3.14159265359; + + // 緯度経度をラジアンに変換 + double rlat1 = lat1 * pi / 180; + double rlng1 = lng1 * pi / 180; + double rlat2 = lat2 * pi / 180; + double rlng2 = lng2 * pi / 180; + + // 2点の中心角(ラジアン)を求める + double a = + sin(rlat1) * sin(rlat2) + + cos(rlat1) * cos(rlat2) * + cos(rlng1 - rlng2); + double rr = acos(a); + + // 地球赤道半径(メートル) + const double earth_radius = 6378140; + + // 2点間の距離(メートル) + double distance2 = earth_radius * rr; + + return distance2; +} + + + +//CanSatから見た目的地の角度theta(-180<=theta<=180で、角度は正面が0で反時計回り)の計算 +float calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2){ + + //float x_0 ,y_0: 目的地 , float x_1 ,y_1: 現在地, float x_2 ,y_2: 20秒前の現在地 + //theta_0:目的地の角度,theta_1:CanSatの角度theta:CanSatから見た目的地の角度(-180) + //theta_0,theta_1は北が90、東が0 + //theta:CanSatから見た目的地の角度(-180<=theta<=180で、角度は正面が0で反時計回り) + + float theta_0,theta_1; + + if(x_0 == x_1 && x_1 == x_2){ + + if(y_0 - y_1 > 0){ + theta_0 = 90; + } + if(y_0 - y_1 < 0){ + theta_0 = 270; + } + if(y_0 - y_1 == 0){ + theta_0 = 0; + } + + if(y_1 - y_2 > 0){ + theta_1 = 90; + } + if(y_1 - y_2 < 0){ + theta_1 = 270; + } + if(y_1 - y_2 == 0){ + theta_1 = 0; + } + } + + if(x_0 == x_1 && x_1 != x_2){ + theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2)); + + if(y_0 - y_1 > 0){ + theta_0 = 90; + } + + if(y_0 - y_1 < 0){ + theta_0 = 270; + } + + if(y_0 - y_1 == 0){ + theta_0 = 0; + } + + if(y_1 - y_2 > 0 && x_1 - x_2 < 0){ + theta_1 = theta_1 - 180; + } + if(y_1 - y_2 == 0 && x_1 - x_2 < 0){ + theta_1 = 180; + } + if(y_1 - y_2 < 0 && x_1 - x_2 > 0){ + theta_1 = theta_1 + 180; + } + } + + if(x_0 != x_1 && x_1 == x_2){ + + theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1)); + + if(y_0 - y_1 > 0 && x_0 - x_1 < 0){ + theta_0 = theta_0 - 180; + } + if(y_0 - y_1 == 0 && x_0 - x_1 < 0){ + theta_0 = 180; + } + if(y_0 - y_1 < 0 && x_0 - x_1 > 0){ + theta_0 = theta_0 + 180; + } + + if(y_1 - y_2 > 0){ + theta_1 = 90; + } + if(y_1 - y_2 < 0){ + theta_1 = 270; + } + if(y_1 - y_2 == 0){ + theta_1 = 0; + } + } + + else{ + theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1)); + theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2)); + + if(y_0 - y_1 > 0 && x_0 - x_1 < 0){ + theta_0 = theta_0 - 180; + } + if(y_0 - y_1 == 0 && x_0 - x_1 < 0){ + theta_0 = 180; + } + if(y_0 - y_1 < 0 && x_0 - x_1 > 0){ + theta_0 = theta_0 + 180; + } + + if(y_1 - y_2 > 0 && x_1 - x_2 < 0){ + theta_1 = theta_1 - 180; + } + if(y_1 - y_2 == 0 && x_1 - x_2 < 0){ + theta_1 = 180; + } + if(y_1 - y_2 < 0 && x_1- x_2 > 0){ + theta_1 = theta_1 + 180; + } + + + } + + float theta = theta_0 - theta_1; + if(theta < -180){ + theta = 360 - theta; + } + if(theta > 180){ + theta = -360 + theta; + } + + return theta; +} + +void direction::walk(){ + GPS gps(D1, D0); + Movement idou; + s = 0; + x_0 = 0; + y_0 = 0; + x_01 = 0; + y_01 = 0; + + while(s<1){ + if(gps.getgps()){ + x_1 = gps.longitude; + y_1 = gps.latitude; + idou.move_forward(20); + s = s + 1; + } + } + + while(1){ + if(gps.getgps()){ //現在地取得 + x_2 = x_1; + y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 + x_1 = gps.longitude; + y_1 = gps.latitude; //現在地の緯度と経度を取得 + d1 = distance1(y_1,x_1,y_0,x_0); + d = distance1(y_1,x_1,y_2,x_2); + theta = calculate_theta(x_0,y_0,x_1,y_1,x_2,y_2); + + //中間地点に到達したらbreak + if(d1 < 15){ + idou.stop(); + break; + } + + //移動距離が短ければ横転と判定し復帰 + if(d < 3){ + idou.wakeup(2); + idou.stop(); + } + + //Θによって回転か直進か決定 + if(-30 < theta && theta < 30){ + idou.move_forward(20); + } + if(theta >=30){ + idou.turn_right(theta); + idou.stop(); + idou.move_forward(20); + } + if(theta<=-30){ + idou.turn_left(theta); + idou.stop(); + idou.move_forward(20); + } + + } + } + while(s < 2){ + if(gps.getgps()){ + x_1 = gps.longitude; + y_1 = gps.latitude; + idou.move_forward(20); + s = s + 1; + } + } + + while(1){ + + if(gps.getgps()){ //現在地取得 + x_2 = x_1; + y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 + x_1 = gps.longitude; + y_1 = gps.latitude; //現在地の緯度と経度を取得 + d2 = distance1(y_1,x_1,y_01,x_01); + d = distance1(y_1,x_1,y_2,x_2); + theta = calculate_theta(x_01,y_01,x_1,y_1,x_2,y_2); + + //ゴール地点に到達したらbreak + if(d2 < 15){ + idou.stop(); + break; + } + + //移動距離が短ければ横転と判定し復帰 + if(d < 3){ + idou.wakeup(2); + idou.stop(); + } + + //thetaによって回転か直進か決定 + if(-30 < theta && theta < 30){ + idou.move_forward(20); + } + if(theta >=30){ + idou.turn_right(theta); + idou.move_forward(20); + } + if(theta<= -30){ + idou.turn_left(theta); + idou.move_forward(20); + } + + } + + + } + + idou.stop(); +} \ No newline at end of file
diff -r 74994694ec04 -r 7209c810309d direction.h --- a/direction.h Thu Oct 28 14:44:25 2021 +0000 +++ b/direction.h Mon Nov 01 16:52:17 2021 +0000 @@ -1,273 +1,9 @@ #include "mbed.h" #include "getGPS.h" +#include "Movement.h" #include "Servo.h" -// 球面三角法により、大円距離(メートル)を求める -double distance(double lat1, double lng1, double lat2, double lng2){ - // 円周率 - const double pi = 3.14159265359; - - // 緯度経度をラジアンに変換 - double rlat1 = lat1 * pi / 180; - double rlng1 = lng1 * pi / 180; - double rlat2 = lat2 * pi / 180; - double rlng2 = lng2 * pi / 180; - - // 2点の中心角(ラジアン)を求める - double a = - sin(rlat1) * sin(rlat2) + - cos(rlat1) * cos(rlat2) * - cos(rlng1 - rlng2); - double rr = acos(a); - - // 地球赤道半径(メートル) - const double earth_radius = 6378140; - - // 2点間の距離(メートル) - double distance = earth_radius * rr; - - return distance; -} - - - -//CanSatから見た目的地の角度theta(-180<=theta<=180で、角度は正面が0で反時計回り)の計算 -float calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2){ - - //float x_0 ,y_0: 目的地 , float x_1 ,y_1: 現在地, float x_2 ,y_2: 20秒前の現在地 - //theta_0:目的地の角度,theta_1:CanSatの角度theta:CanSatから見た目的地の角度(-180) - //theta_0,theta_1は北が90、東が0 - //theta:CanSatから見た目的地の角度(-180<=theta<=180で、角度は正面が0で反時計回り) - - float theta_0,theta_1; - - if(x_0 == x_1 && x_1 == x_2){ - - if(y_0 - y_1 > 0){ - theta_0 = 90; - } - if(y_0 - y_1 < 0){ - theta_0 = 270; - } - if(y_0 - y_1 == 0){ - theta_0 = 0; - } - - if(y_1 - y_2 > 0){ - theta_1 = 90; - } - if(y_1 - y_2 < 0){ - theta_1 = 270; - } - if(y_1 - y_2 == 0){ - theta_1 = 0; - } - } - - if(x_0 == x_1 && x_1 != x_2){ - theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2)); - - if(y_0 - y_1 > 0){ - theta_0 = 90; - } - - if(y_0 - y_1 < 0){ - theta_0 = 270; - } - - if(y_0 - y_1 == 0){ - theta_0 = 0; - } - - if(y_1 - y_2 > 0 && x_1 - x_2 < 0){ - theta_1 = theta_1 - 180; - } - if(y_1 - y_2 == 0 && x_1 - x_2 < 0){ - theta_1 = 180; - } - if(y_1 - y_2 < 0 && x_1 - x_2 > 0){ - theta_1 = theta_1 + 180; - } - } - - if(x_0 != x_1 && x_1 == x_2){ - - theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1)); - - if(y_0 - y_1 > 0 && x_0 - x_1 < 0){ - theta_0 = theta_0 - 180; - } - if(y_0 - y_1 == 0 && x_0 - x_1 < 0){ - theta_0 = 180; - } - if(y_0 - y_1 < 0 && x_0 - x_1 > 0){ - theta_0 = theta_0 + 180; - } - - if(y_1 - y_2 > 0){ - theta_1 = 90; - } - if(y_1 - y_2 < 0){ - theta_1 = 270; - } - if(y_1 - y_2 == 0){ - theta_1 = 0; - } - } - - else{ - theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1)); - theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2)); - - if(y_0 - y_1 > 0 && x_0 - x_1 < 0){ - theta_0 = theta_0 - 180; - } - if(y_0 - y_1 == 0 && x_0 - x_1 < 0){ - theta_0 = 180; - } - if(y_0 - y_1 < 0 && x_0 - x_1 > 0){ - theta_0 = theta_0 + 180; - } - - if(y_1 - y_2 > 0 && x_1 - x_2 < 0){ - theta_1 = theta_1 - 180; - } - if(y_1 - y_2 == 0 && x_1 - x_2 < 0){ - theta_1 = 180; - } - if(y_1 - y_2 < 0 && x_1- x_2 > 0){ - theta_1 = theta_1 + 180; - } - - - } - - float theta = theta_0 - theta_1; - if(theta < -180){ - theta = 360 - theta; - } - if(theta > 180){ - theta = -360 + theta; - } - - return theta; -} - - -//停止 - void stop(){ - Servo servo1(D7); -Servo servo2(A3); -Servo servo3(A1); -Servo servo4(D12); -Servo servo5(D10); -Servo servo6(A5); - - servo1 = 0.5; - servo2 = 0.5; - servo3 = 0.5; - servo4 = 0.5; - servo5 = 0.5; - servo6 = 0.5; - wait(1); - } - -//前進 - void move_forward(int time = 20) - { - Servo servo1(D7); -Servo servo2(A3); -Servo servo3(A1); -Servo servo4(D12); -Servo servo5(D10); -Servo servo6(A5); - - servo1 = 0; - servo2 = 0; - servo3 = 0; - servo4 = 0; - servo5 = 0; - servo6 = 0; - wait(time); - } - -//後退 - void move_backward() - { - Servo servo1(D7); -Servo servo2(A3); -Servo servo3(A1); -Servo servo4(D12); -Servo servo5(D10); -Servo servo6(A5); - - servo1 = 1; - servo2 = 1; - servo3 = 1; - servo4 = 1; - servo5 = 1; - servo6 = 1; - wait(5); - } - -//右に曲がる - void turn_right(int theta = 15) - { - Servo servo1(D7); -Servo servo2(A3); -Servo servo3(A1); -Servo servo4(D12); -Servo servo5(D10); -Servo servo6(A5); - - servo1 = 1; - servo2 = 1; - servo3 = 1; - servo4 = 0; - servo5 = 0; - servo6 = 0; - wait(theta/15); - } - -//左に曲がる - void turn_left(int theta = 15) - { - Servo servo1(D7); -Servo servo2(A3); -Servo servo3(A1); -Servo servo4(D12); -Servo servo5(D10); -Servo servo6(A5); - - servo1 = 0; - servo2 = 0; - servo3 = 0; - servo4 = 1; - servo5 = 1; - servo6 = 1; - wait(theta/15); - } - -//倒れているときの処理 - void wakeup(int time) - { - Servo servo1(D7); -Servo servo2(A3); -Servo servo3(A1); -Servo servo4(D12); -Servo servo5(D10); -Servo servo6(A5); - - int i; - for(i=1;i<=time;i++) - { - move_forward(5); - move_backward(); - turn_right(); - turn_left(); -} - -class direction:Servo +class direction { private: @@ -282,116 +18,9 @@ public: //歩行 - void walk(){ - GPS gps(D1, D0); - s = 0; - x_0 = 0; - y_0 = 0; - x_01 = 0; - y_01 = 0; - - while(s<1){ - if(gps.getgps()){ - x_1 = gps.longitude; - y_1 = gps.latitude; - move_forward(); - s = s + 1; - } - } - - while(1){ - if(gps.getgps()){ //現在地取得 - x_2 = x_1; - y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 - x_1 = gps.longitude; - y_1 = gps.latitude; //現在地の緯度と経度を取得 - d1 = distance(y_1,x_1,y_0,x_0); - d = distance(y_1,x_1,y_2,x_2); - theta = calculate_theta(x_0,y_0,x_1,y_1,x_2,y_2); - - //中間地点に到達したらbreak - if(d1 < 15){ - stop(); - break; - } - - //移動距離が短ければ横転と判定し復帰 - if(d < 3){ - wakeup(2); - stop(); - } - - //Θによって回転か直進か決定 - if(-30 < theta && theta < 30){ - move_forward(); - } - if(theta >=30){ - turn_right(theta); - stop(); - move_forward(); - } - if(theta<=-30){ - turn_left(theta); - stop(); - move_forward(); - } - - } - } - while(s < 2){ - if(gps.getgps()){ - x_1 = gps.longitude; - y_1 = gps.latitude; - move_forward(); - s = s + 1; - } - } - - while(1){ - - if(gps.getgps()){ //現在地取得 - x_2 = x_1; - y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 - x_1 = gps.longitude; - y_1 = gps.latitude; //現在地の緯度と経度を取得 - d2 = distance(y_1,x_1,y_01,x_01); - d = distance(y_1,x_1,y_2,x_2); - theta = calculate_theta(x_01,y_01,x_1,y_1,x_2,y_2); - - //ゴール地点に到達したらbreak - if(d2 < 15){ - stop(); - break; - } - - //移動距離が短ければ横転と判定し復帰 - if(d < 3){ - wakeup(2); - stop(); - } - - //thetaによって回転か直進か決定 - if(-30 < theta && theta < 30){ - move_forward(); - } - if(theta >=30){ - turn_right(theta); - move_forward(); - } - if(theta<= -30){ - turn_left(theta); - move_forward(); - } - - } - - - } - - stop(); - } + void walk(); - }; + \ No newline at end of file
diff -r 74994694ec04 -r 7209c810309d getGPS.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/getGPS.cpp Mon Nov 01 16:52:17 2021 +0000 @@ -0,0 +1,57 @@ +#include "mbed.h" +#include "getGPS.h" + +GPS::GPS(PinName gpstx,PinName gpsrx): _gps(gpstx,gpsrx) +{ + latitude = 0; + longitude = 0; + _gps.baud(GPSBAUD); + _gps.printf("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"); +} + +bool GPS::getgps() +{ + char gps_data[256]; + int i; + + do { + while(_gps.getc() != '$'); //$マークまで読み飛ばし + i = 0; + + /* gpa_data初期化 */ + for(int j = 0; j < 256; j++) + gps_data[j] = '\0'; + + /* NMEAから一行読み込み */ + while((gps_data[i] = _gps.getc()) != '\r') { + i++; + if(i == 256) { + i = 255; + break; + } + } + } while(strstr(gps_data, "GPGGA") == NULL); //GGAセンテンスまで一行ずつ読み込み続ける + + int rlock; + char ns,ew; + double w_time, raw_longitude, raw_latitude; + int satnum; + double hdop; + + if(sscanf(gps_data, "GPGGA, %lf, %lf, %c, %lf, %c, %d, %d, %lf", &w_time, &raw_latitude, &ns, &raw_longitude, &ew, &rlock, &satnum, &hdop) > 1) { + /* 座標1(度部分) */ + int latitude_dd = (int)(raw_latitude / 100); + int longitude_dd = (int)(raw_longitude / 100); + + /* 座標2(分部分 → 度) */ + double latitude_md = (raw_latitude - latitude_dd * 100) / 60; + double longitude_md = (raw_longitude - longitude_dd * 100) / 60; + + /* 座標1 + 2 */ + latitude = latitude_dd + latitude_md; + longitude = longitude_dd + longitude_md; + + return true; + } else + return false; //GGAセンテンスの情報が欠けている時 +}
diff -r 74994694ec04 -r 7209c810309d main.cpp --- a/main.cpp Thu Oct 28 14:44:25 2021 +0000 +++ b/main.cpp Mon Nov 01 16:52:17 2021 +0000 @@ -1,30 +1,71 @@ #include "mbed.h" #include "direction.h" -#include "Landing_Judgement.h" +#include "BMP180.h" +#include "calculate.h" +#define PIN_SDA D4 +#define PIN_SCL D5 + DigitalOut Nichrome(A6); - + int main(){ - int land_judgement1=0; - //着地判定 - while(1){ - land_judgement1 = Landing_Judgement.land(); - if(land_judgement1==1){ - break; - } + BMP180 bmp180(PIN_SDA,PIN_SCL); + int x = 0,y = 0,n1 = 0, n2 = 0; + int i; + float h,dp,dt,dp0; + float a = 0,b = 0,dp_ave,dt_ave; + //hは高度、dpは気圧、dtは温度、dp0は海面気圧 + bmp180.Initialize(27,BMP180_OSS_ULTRA_LOW_POWER);//27は府大の海抜高度 + + //海面気圧を計算 + for(i=0;i<15;i++){ + if(bmp180.ReadData(&dt,&dp)){ + a = a + dp; + b = b + dt; + n1 = n1 + 1; + n2 = n2 + 1; + wait(1); + } + } + + dp_ave = a / n1; + dt_ave = b / n2; + dp0 = calculate_dp0(dp_ave,dt_ave); + + //10秒以上高度10mにいた場合離陸判定 + while(x<10){ + if(bmp180.ReadData(&dt,&dp)){ + h = calculate_h(dp0,dp,dt); + if(h >= 10){ + x = x + 1; + } + wait(1); + } } - wait(5); + wait(10); + + //離陸判定後、10秒以上高度2m以下にいた場合着地判定 + while(y<10){ + if(bmp180.ReadData(&dt,&dp)){ + h = calculate_h(dp0,dp,dt); + if(h <= 2){ + y = y + 1; + } + wait(1); + } + } + //30秒待機 wait(30); //パラシュート分離 Nichrome=1; wait(10); Nichrome=0; - - //中間地点を経由してゴール地点まで自律移動 - direction.walk(); + + //中間地点を経由してゴール地点まで移動 + direction hokou; + hokou.walk(); return 0; - -} - \ No newline at end of file + + } \ No newline at end of file