hitomi shio
a
Dependencies: mbed SDFileSystem BMP180
main.cpp
- Committer:
- ShioHitomi
- Date:
- 2021-11-19
- Revision:
- 0:0624addc6841
File content as of revision 0:0624addc6841:
#include "mbed.h"
#include "math.h"
#include "BMP180.h"
#include "SDFileSystem.h"
#define p0 1013.25f //海面気圧
#define PI 3.1415
#define Alt_num 3.0 //高度を何回の平均値にするか
#define Angle_num 3 //姿勢角を何回の平均値にするか
#define Angle_cnt 10 //何回姿勢角取得するか
#define Alt_bou 30 //この高度より低かったらFlight mode入ったとみなす
#define Z_0 0.962 //KXTC9-2050_rawを使ってキャリブレーションする
#define Z_90 1.62
#define Z_180 2.24
enum PHASE{CHECK, STANDBY, FLIGHT, EXPANSION, MISSION, WITHDRAW} Phase;
Serial pc(USBTX, USBRX, 115200);
Serial twe(p28, p27, 115200);
DigitalOut myled_1(LED1);
DigitalOut myled_2(LED2);
DigitalOut myled_3(LED3);
DigitalOut myled_4(LED4);
DigitalIn Flight_IN(p20);
//SDFileSystem sd(p5, p6, p7, p8, "sd");
BMP180 bmp(p9, p10); //(sda, scl)
AnalogIn z(p19); //加速度センサ
DigitalOut load_sck(p13); //ロードセル
DigitalIn load_data(p14);
DigitalOut fet(p18); //ニクロム用MOSFET
PwmOut servo(p24);
PwmOut motor1_1(p25); //motor1：回転用
PwmOut motor1_2(p26);
DigitalOut motor2_1(p22); //motor2：上昇下降用
DigitalOut motor2_2(p23);
PwmOut motor2_pwm(p21);
Timer time_0;
Timer time_flight;
int _getTime(); //time_0取得
float _getAlt(); //高度取得
float _getAngle(); //姿勢角取得
int _averageLoad(uint8_t times); //ロードセルのキャリブレーションする時に使う
int _getLoad();
float _getGram(); //質量取得
void _Rand_judge(); //着地判定
//void _Angle_judge();
int getTime;
float getAlt, getAngle, getGram;;
float Load_offset = 0;
FILE *fp;
int main(){
/*タイマー*/
time_0.start();
/*SD
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Start.\r\n");
fclose(fp);
*/
/*BMP180*/
bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
/*ロードセル*/
load_sck = 1;
wait_us(100);
load_sck = 0;
Load_offset = _averageLoad(10);
/*サーボ*/
servo.period(0.020);
Phase = CHECK;
switch(Phase){
case CHECK:
pc.printf("Check mode.\r\n");
twe.printf("Check mode.\r\n");
twe.printf("Check mode.\r\n");
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Check mode.\r\n");
fclose(fp);
*/
/*センサ値取得*/
for(int i=0; i<30; i++){
getTime = _getTime();
getAlt = _getAlt();
getAngle = _getAngle();
getGram = _getGram();
pc.printf("Time: %d Alt: %f Angle: %f Gram: %f\r\n", getTime, getAlt, getAngle, getGram);
twe.printf("Time: %d Alt: %f Angle: %f Gram: %f\r\n", getTime, getAlt, getAngle, getGram);
wait(1.0);
}
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Time: %d\r\n Alt: %f\r\n Angle: %f\r\n Gram: %f\r\n", getTime, getAlt, getAngle, getGram);
fclose(fp);
*/
myled_1 = 1;
Phase = STANDBY;
//break;
case STANDBY:
//pc.printf("Standby mode.\r\n");
twe.printf("Standby mode.\r\n");
twe.printf("Standby mode.\r\n");
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Standby mode.\r\n");
fclose(fp);
*/
/*放出判定*/
int fly_cnt = 0;
while(1) {
int FlightPin = Flight_IN;
getAlt = _getAlt();
//if((FlightPin == 0) && (getAlt < Alt_bou)){
if(FlightPin == 0){
time_flight.start();
break;
}else{
fly_cnt++;
getTime = _getTime();
getAlt = _getAlt();
getAngle = _getAngle();
if(fly_cnt == 100){ //100回に1回データ保存
fly_cnt = 0;
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Time: %d Alt: %f Angle: %f\r\n", getTime, getAlt, getAngle);
fclose(fp);
*/
}
}
}
myled_2 = 1;
Phase = FLIGHT;
case FLIGHT:
//pc.printf("Flight mode.\r\n");
twe.printf("Flight mode.\r\n");
twe.printf("Flight mode.\r\n");
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Flight mode.\r\n");
fclose(fp);
*/
/*着地判定*/
_Rand_judge();
//pc.printf("Rand judge success!!\r\n");
twe.printf("Rand judge success!!\r\n");
twe.printf("Rand judge success!!\r\n");
myled_3 = 1;
Phase = EXPANSION;
case EXPANSION:
//pc.printf("Expansion mode.\r\n");
twe.printf("Expansion mode.\r\n");
twe.printf("Expansion mode.\r\n");
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Expansion mode.\r\n");
fclose(fp);
*/
wait(30);
/*ニクロム線動作*/
fet = 1;
wait(0.3);
fet = 0;
wait(30.0);
for(int i=0; i<30; i++){
getAngle = _getAngle();
//pc.printf("Angle: %f\r\n", getAngle);
twe.printf("Angle: %f\r\n", getAngle);
/*
fp = fopen("/sd/test.txt", "a"); //毎回角度保存してるけどどうしようかな
fprintf(fp, "Angle: %f\r\n", getAngle);
fclose(fp);
*/
wait(1.0);
}
myled_4 = 1;
Phase = MISSION;
case MISSION:
//pc.printf("Mission mode.\r\n");
twe.printf("Mission mode.\r\n");
twe.printf("Mission mode.\r\n");
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Mission mode.\r\n");
fclose(fp);
*/
load_sck = 1;
wait_us(100);
load_sck = 0;
for(int i=0; i<10; i++){
getGram += _getGram();
wait(1.0);
}
float Gram_0 = 0.0;
Gram_0 = getGram/10.0;
//pc.printf("Gram_0: %f\r\n", Gram_0);
twe.printf("Gram_0: %f\r\n", Gram_0);
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Gram_0: %f\r\n", Gram_0);
fclose(fp);
*/
/*ちょっとあげる*/
motor2_pwm = 1.0;
motor2_1 = 1;
motor2_2 = 0;
wait(3.0);
/*サーボ*/
servo.pulsewidth(0.0015);
wait(1.0);
servo.pulsewidth(0.0018);
wait(1.0);
servo.pulsewidth(0.0020);
wait(1.0);
servo.pulsewidth(0.0022);
wait(1.0);
servo.pulsewidth(0.0023);
wait(1.0);
//servo.pulsewidth(0.0026);
//wait(1.0);
/*
servo.pulsewidth(0.0030);
*/
/*ドリル動作 motor1:回転、motor2:下降上昇*/
motor2_pwm = 1.0;
motor2_1 = 0; //下降・回転
motor2_2 = 1;
motor1_1 = 1.0;
motor1_2 = 0.0;
wait(9.0);
motor2_1 = 0; //止まる・回転
motor2_2 = 0;
motor1_1 = 1.0;
motor1_2 = 0.0;
wait(2.0);
motor1_1 = 0.0; //止まる・回転停止
motor1_2 = 0.0;
wait(3.0);
motor2_1 = 1; //上昇・回転停止
motor2_2 = 0;
motor1_1 = 0.0;
motor1_2 = 0.0;
wait(15.0);
motor2_1 = 0; //止まる・回転停止
motor2_2 = 0;
motor1_1 = 0.0;
motor1_2 = 0.0;
wait(2.0);
/*サーボ動作*/
servo.pulsewidth(0.0023);
wait(1.0);
servo.pulsewidth(0.0022);
wait(1.0);
servo.pulsewidth(0.0020);
wait(1.0);
servo.pulsewidth(0.0018);
wait(1.0);
servo.pulsewidth(0.0014);
wait(1.0);
/*サンプルをドリルから取るための回転*/
motor1_1 = 1.0;
motor1_2 = 0.0;
wait(10.0);
motor1_1 = 0.0;
motor1_2 = 0.0;
/*質量値取得*/
for(int i=0; i<30; i++){
getGram = _getGram();
//pc.printf("Gram: %f \r\n", getGram - Gram_0);
twe.printf("Gram: %f \r\n", getGram - Gram_0);
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Gram: %f\r\n", getGram - Gram_0);
fclose(fp);
*/
wait(1.0);
}
Phase = WITHDRAW;
case WITHDRAW:
//pc.printf("Withdraw mode.\r\n");
twe.printf("Withdraw mode.\r\n");
twe.printf("Withdraw mode.\r\n");
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "Withdraw mode.\r\n");
fclose(fp);
*/
while(1){
myled_1 = !myled_1;
myled_2 = !myled_2;
myled_3 = !myled_3;
myled_4 = !myled_4;
getTime = _getTime();
//pc.printf("Time: %d\r\n", getTime);
twe.printf("Time: %d\r\n", getTime);
wait(1.0);
}
}
}
int _getTime(){ //Timer_0取得
int time;
time = time_0.read();
return time;
}
float _getAlt(){ //高度取得
float Tem, Pre, Alt, sum = 0;
for(int i=0; i<Alt_num; i++){
bmp.ReadData(&Tem, &Pre);
Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
sum += Alt;
}
return sum/Alt_num;
}
float _getAngle(){
float Z, rad, angle, Angle_sum = 0;
for(int i=0; i<Angle_num; i++){
Z = 3.3*z.read();
if(Z <= Z_0){
angle = 0;
}else if(Z > Z_0 && Z < Z_90){
rad = asin((Z - Z_0)/(Z_90 - Z_0));
angle = 180*rad/PI;
}else if(Z >= Z_90 && Z < Z_180){
rad = asin((Z - Z_90)/(Z_180 - Z_90));
angle = 90+180*rad/PI;
}else{
angle = 180;
}
/*
if(Z <= Z_180){
angle = 180;
}else if(Z > Z_180 && Z < Z_90){
rad = asin((Z - Z_180)/(Z_90 - Z_180));
angle = 180 - 180*rad/PI;
}else if(Z >= Z_90 && Z < Z_0){
rad = asin((Z - Z_90)/(Z_0 - Z_90));
angle = 90 - 180*rad/PI;
}else{
angle = 0; //全部-180した
}
*/
Angle_sum += angle;
}
return Angle_sum/Angle_num;
}
void _Rand_judge(){
float Alt, Alt_old, Alt_new;
float Alt_sum = 0;
float Pre, Tem;
int rand_cnt = 0;
int flight_time;
for(int i=0; i<Alt_num; i++){
bmp.ReadData(&Tem, &Pre);
Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
Alt_sum += Alt;
}
Alt_old = Alt_sum/Alt_num;
Alt_sum = 0;
while(1) {
for(int j=0; j<Alt_num; j++){
bmp.ReadData(&Tem, &Pre);
Alt = (pow((p0/Pre), (1.0f/5.257f))-1.0f)*(Tem + 273.15f)/0.0065f;
Alt_sum += Alt;
}
Alt_new = Alt_sum/Alt_num;
Alt_sum = 0;
flight_time = time_flight.read();
/*
fp = fopen("/sd/test.txt", "a");
fprintf(fp, "flight_time: %d , Alt_new: %f, rand_cnt: %d\r\n", flight_time, Alt_new, rand_cnt);
fclose(fp);
*/
if(fabsf(Alt_new-Alt_old)<0.8){ //高度差が何m以内の時着地判定クリアとするか
rand_cnt++;
Alt_old = Alt_new;
wait(0.97); //1Hzになるように調整
if(rand_cnt == 3){ //3回連続 または 60秒
break;
}
}else if(flight_time >= 60){
break;
}else{
rand_cnt = 0;
Alt_old = Alt_new;
wait(0.97);
}
//pc.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt);
twe.printf("flightTime:%d s, Alt:%f m, cnt:%d \r\n", flight_time, Alt_old, rand_cnt);
}
}
int _averageLoad(uint8_t times){
int sum = 0;
for (int i = 0; i < times; i++){
sum += _getLoad();
}
return sum / times;
}
int _getLoad(){
int buffer;
buffer = 0 ;
while (load_data.read()==1);
for (uint8_t i = 24; i--;){
load_sck=1;
buffer = buffer << 1 ;
if (load_data.read()){
buffer ++;
}
load_sck=0;
}
for (int i = 0; i < 1; i++){
load_sck=1;
load_sck=0;
}
buffer = buffer ^ 0x800000;
return buffer;
}
float _getGram(){
long val = (_averageLoad(2) - Load_offset);
if(val < 0) val = 0;
float scale = 0.0003*4.2987*128.0/100.0; //定格出力：0.0006V， 定格容量：100g
float volt;
float gram;
volt = val*(4.2987/16777216.0);
gram = volt/scale;
return (float) gram;
}