albatross / Mbed 2 deprecated keiki2018

2018年度計器mbed用プログラム

Dependencies:   BufferedSoftSerial2 INA226_ver1 mbed-rtos mbed SDFileSystem-RTOS

Fork of keiki2017 by albatross

main.cpp@18:0a0c4277d960, 2016-10-28 (annotated)

Committer:
YusukeWakuta
Date:
Fri Oct 28 15:08:14 2016 +0000
Branch:
7_????????????
Revision:
18:0a0c4277d960
Parent:
11:90e2b070f337
Child:
19:fa3f9ba17af8
#7 mpu???????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taurin 1:5ec2409854da 1 //計器プログラム
YusukeWakuta 18:0a0c4277d960 2
taurin 0:085b2c5e3254 3 #include "mbed.h"
taurin 0:085b2c5e3254 4 #include "Fusokukei.h"
taurin 0:085b2c5e3254 5 #include "MPU6050.h"
taurin 0:085b2c5e3254 6 #include "SDFileSystem.h"
taurin 0:085b2c5e3254 7
taurin 0:085b2c5e3254 8 #define KX_VALUE_MIN 0.4
taurin 0:085b2c5e3254 9 #define KX_VALUE_MAX 0.8
taurin 1:5ec2409854da 10 #define SOUDA_DATAS_NUM 13
taurin 5:1b3c8e5ee99e 11 #define WRITE_DATAS_NUM 20
taurin 0:085b2c5e3254 12 #define MPU_LOOP_TIME 0.01
taurin 0:085b2c5e3254 13 #define AIR_LOOP_TIME 0.01
taurin 3:8dc516be2e7e 14 #define WRITE_DATAS_LOOP_TIME 1
taurin 3:8dc516be2e7e 15 #define ROLL_R_MAX_DEG 2
taurin 3:8dc516be2e7e 16 #define ROLL_L_MAX_DEG 2
taurin 3:8dc516be2e7e 17 #define SD_WRITE_NUM 10
taurin 4:a863a092141c 18 #define INIT_SERVO_PERIOD_MS 20
taurin 0:085b2c5e3254 19
taurin 0:085b2c5e3254 20 Serial pc(USBTX,USBRX);
taurin 0:085b2c5e3254 21 Serial soudaSerial(p13,p14);
taurin 1:5ec2409854da 22 Serial twe(p9,p10);
taurin 0:085b2c5e3254 23 Ticker writeDatasTicker;
taurin 3:8dc516be2e7e 24 Timer writeTimer;
taurin 0:085b2c5e3254 25
taurin 0:085b2c5e3254 26 InterruptIn FusokukeiPin(p30);
taurin 0:085b2c5e3254 27 Ticker FusokukeiTicker;
taurin 0:085b2c5e3254 28 Fusokukei air;
taurin 0:085b2c5e3254 29 volatile int air_kaitensu= 0;
taurin 0:085b2c5e3254 30
YusukeWakuta 18:0a0c4277d960 31
taurin 0:085b2c5e3254 32 MPU6050 mpu6050;
taurin 0:085b2c5e3254 33 Timer t;
taurin 0:085b2c5e3254 34 Ticker mpu6050Ticker;
taurin 0:085b2c5e3254 35
taurin 0:085b2c5e3254 36 AnalogIn kx_X(p17);
taurin 0:085b2c5e3254 37 AnalogIn kx_Y(p16);
taurin 0:085b2c5e3254 38 AnalogIn kx_Z(p15);
taurin 3:8dc516be2e7e 39 float KX_X,KX_Y,KX_Z;
taurin 0:085b2c5e3254 40
taurin 0:085b2c5e3254 41 DigitalOut RollAlarmR(p20);
taurin 0:085b2c5e3254 42 DigitalOut RollAlarmL(p19);
taurin 0:085b2c5e3254 43 DigitalOut led(LED1);
taurin 1:5ec2409854da 44 DigitalOut led2(LED2);
taurin 0:085b2c5e3254 45
taurin 0:085b2c5e3254 46 SDFileSystem sd(p5, p6, p7, p8, "sd");
taurin 0:085b2c5e3254 47 FILE* fp;
taurin 0:085b2c5e3254 48
YusukeWakuta 11:90e2b070f337 49 PwmOut kisokuServo(p26);
YusukeWakuta 11:90e2b070f337 50 PwmOut geikakuServo(p22);
taurin 4:a863a092141c 51
taurin 0:085b2c5e3254 52 char soudaDatas[SOUDA_DATAS_NUM];
taurin 3:8dc516be2e7e 53 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
taurin 3:8dc516be2e7e 54 volatile int write_datas_index = 0;
taurin 0:085b2c5e3254 55
taurin 0:085b2c5e3254 56 void air_countUp();
taurin 0:085b2c5e3254 57 void call_calcAirSpeed();
taurin 0:085b2c5e3254 58 void init();
taurin 0:085b2c5e3254 59 void FusokukeiInit();
taurin 0:085b2c5e3254 60 void SdInit();
taurin 0:085b2c5e3254 61 void DataReceiveFromSouda();
taurin 0:085b2c5e3254 62 void WriteDatas();
taurin 0:085b2c5e3254 63 float calcAttackAngle();
taurin 0:085b2c5e3254 64 float calcKXdeg(float x);
taurin 0:085b2c5e3254 65
taurin 0:085b2c5e3254 66 void air_countUp(){
taurin 0:085b2c5e3254 67 air_kaitensu++;
taurin 0:085b2c5e3254 68 }
taurin 0:085b2c5e3254 69
taurin 0:085b2c5e3254 70 void call_calcAirSpeed(){
taurin 0:085b2c5e3254 71 air.calcAirSpeed(air_kaitensu);
taurin 0:085b2c5e3254 72 air_kaitensu = 0;
taurin 0:085b2c5e3254 73 }
taurin 0:085b2c5e3254 74
taurin 0:085b2c5e3254 75 void init(){
taurin 1:5ec2409854da 76 twe.baud(115200);
taurin 6:98a63127aa11 77 //writeTimer.start();
taurin 5:1b3c8e5ee99e 78 kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 5:1b3c8e5ee99e 79 geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:085b2c5e3254 80 FusokukeiInit();
YusukeWakuta 18:0a0c4277d960 81 mpu6050.MPUInit(t);
taurin 8:31e07f6ed0f7 82 SdInit();
taurin 3:8dc516be2e7e 83 //writeDatasTicker.attach(&WriteDatas,1);
taurin 1:5ec2409854da 84 //soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq);
taurin 0:085b2c5e3254 85 }
taurin 0:085b2c5e3254 86
taurin 0:085b2c5e3254 87 void FusokukeiInit(){
taurin 0:085b2c5e3254 88 FusokukeiPin.rise(air_countUp);
taurin 0:085b2c5e3254 89 FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
taurin 0:085b2c5e3254 90 }
taurin 0:085b2c5e3254 91
YusukeWakuta 18:0a0c4277d960 92
taurin 0:085b2c5e3254 93
taurin 4:a863a092141c 94 double calcPulse(int deg){
taurin 4:a863a092141c 95 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 4:a863a092141c 96
taurin 4:a863a092141c 97 }
taurin 4:a863a092141c 98
taurin 0:085b2c5e3254 99 void SdInit(){
taurin 0:085b2c5e3254 100 mkdir("/sd/mydir", 0777);
taurin 0:085b2c5e3254 101 fp = fopen("/sd/mydir/sdtest2.csv", "w");
taurin 0:085b2c5e3254 102 if(fp == NULL) {
taurin 0:085b2c5e3254 103 error("Could not open file for write\n");
taurin 0:085b2c5e3254 104 }
taurin 0:085b2c5e3254 105 fprintf(fp, "Hello fun SD Card World!\n\r");
taurin 0:085b2c5e3254 106 fclose(fp);
taurin 0:085b2c5e3254 107 }
taurin 0:085b2c5e3254 108
taurin 0:085b2c5e3254 109 void DataReceiveFromSouda(){
taurin 1:5ec2409854da 110 led2 = !led2;
taurin 5:1b3c8e5ee99e 111 //pc.printf("received\n\r");
taurin 1:5ec2409854da 112 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
taurin 1:5ec2409854da 113 soudaDatas[i] = soudaSerial.getc();
taurin 0:085b2c5e3254 114 }
taurin 0:085b2c5e3254 115 }
taurin 0:085b2c5e3254 116
taurin 3:8dc516be2e7e 117 void SDprintf(){
taurin 1:5ec2409854da 118 fp = fopen("/sd/mydir/sdtest.csv", "a");
taurin 1:5ec2409854da 119 if(fp == NULL) {
taurin 1:5ec2409854da 120 error("Could not open file for write\n");
taurin 1:5ec2409854da 121 }
taurin 3:8dc516be2e7e 122 for(int i = 0; i < SD_WRITE_NUM; i++){
taurin 3:8dc516be2e7e 123 for(int j = 0; j < WRITE_DATAS_NUM; j++){
taurin 5:1b3c8e5ee99e 124 fprintf(fp,"%f,", writeDatas[i][j]);
taurin 3:8dc516be2e7e 125 }
taurin 3:8dc516be2e7e 126 }
taurin 5:1b3c8e5ee99e 127 fprintf(fp,"\n\r");
taurin 3:8dc516be2e7e 128 fclose(fp);
taurin 3:8dc516be2e7e 129 }
taurin 3:8dc516be2e7e 130
taurin 3:8dc516be2e7e 131 void WriteDatas(){
taurin 8:31e07f6ed0f7 132 int i;
taurin 8:31e07f6ed0f7 133 for(i = 0; i < SOUDA_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 134 //writeDatas[write_datas_index][i] = 0.0;
taurin 8:31e07f6ed0f7 135 writeDatas[write_datas_index][i] = (float)soudaDatas[i];
taurin 8:31e07f6ed0f7 136 }
taurin 8:31e07f6ed0f7 137 writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
taurin 8:31e07f6ed0f7 138 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
taurin 8:31e07f6ed0f7 139 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
taurin 8:31e07f6ed0f7 140 writeDatas[write_datas_index][i++] = pitch;
taurin 8:31e07f6ed0f7 141 writeDatas[write_datas_index][i++] = roll;
taurin 8:31e07f6ed0f7 142 writeDatas[write_datas_index][i++] = yaw;
taurin 8:31e07f6ed0f7 143 writeDatas[write_datas_index][i++] = airSpeed;
taurin 8:31e07f6ed0f7 144 //writeDatas[write_datas_index][i++] = writeTimer.read();
taurin 9:95eb0bbdc2a9 145 //for(i = 0; i < WRITE_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 146 // pc.printf("%f ", writeDatas[write_datas_index][i]);
taurin 9:95eb0bbdc2a9 147 // twe.printf("%f,", writeDatas[write_datas_index][i]);
taurin 9:95eb0bbdc2a9 148 // }
taurin 9:95eb0bbdc2a9 149 // pc.printf("\n\r");
taurin 9:95eb0bbdc2a9 150 // twe.printf("\n\r");
taurin 8:31e07f6ed0f7 151 if(write_datas_index == SD_WRITE_NUM-1){
taurin 8:31e07f6ed0f7 152 SDprintf();
taurin 8:31e07f6ed0f7 153 write_datas_index=0;
taurin 8:31e07f6ed0f7 154 }
taurin 8:31e07f6ed0f7 155 else{
taurin 8:31e07f6ed0f7 156 write_datas_index++;
taurin 8:31e07f6ed0f7 157 }
taurin 9:95eb0bbdc2a9 158 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 159 pc.printf("%i ",soudaDatas[i]);
YusukeWakuta 10:1389f938f5f7 160 twe.printf("%i,",soudaDatas[i]);
taurin 9:95eb0bbdc2a9 161 }
taurin 9:95eb0bbdc2a9 162 //pc.printf("\n\r");
YusukeWakuta 10:1389f938f5f7 163 twe.printf("%f,%f,%f,",pitch,roll,yaw);
YusukeWakuta 10:1389f938f5f7 164 twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
YusukeWakuta 10:1389f938f5f7 165 twe.printf("%f,\r\n",airSpeed);
taurin 9:95eb0bbdc2a9 166 pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
taurin 9:95eb0bbdc2a9 167 //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
taurin 9:95eb0bbdc2a9 168 pc.printf("%f\n\r",airSpeed);
taurin 9:95eb0bbdc2a9 169 //SDprintf();
taurin 9:95eb0bbdc2a9 170 }
taurin 9:95eb0bbdc2a9 171
taurin 9:95eb0bbdc2a9 172 void WriteDatasF(){
taurin 9:95eb0bbdc2a9 173 pc.printf("airSpeed:%f\n\r",airSpeed);
taurin 0:085b2c5e3254 174 }
taurin 0:085b2c5e3254 175
taurin 0:085b2c5e3254 176 float calcKXdeg(float x){
taurin 0:085b2c5e3254 177 return -310.54*x+156.65;
taurin 0:085b2c5e3254 178 }
taurin 0:085b2c5e3254 179
taurin 0:085b2c5e3254 180 float calcAttackAngle(){
taurin 0:085b2c5e3254 181 return pitch-calcKXdeg(kx_Z.read());
taurin 0:085b2c5e3254 182 }
taurin 0:085b2c5e3254 183
taurin 0:085b2c5e3254 184 void RollAlarm(){
taurin 0:085b2c5e3254 185 if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
taurin 0:085b2c5e3254 186 RollAlarmL = 1;
taurin 0:085b2c5e3254 187 }
taurin 0:085b2c5e3254 188 else{
taurin 0:085b2c5e3254 189 RollAlarmL = 0;
taurin 0:085b2c5e3254 190 }
taurin 0:085b2c5e3254 191
taurin 0:085b2c5e3254 192 if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
taurin 0:085b2c5e3254 193 RollAlarmR = 1;
taurin 0:085b2c5e3254 194 }
taurin 0:085b2c5e3254 195 else{
taurin 0:085b2c5e3254 196 RollAlarmR = 0;
taurin 0:085b2c5e3254 197 }
taurin 0:085b2c5e3254 198 }
taurin 0:085b2c5e3254 199
taurin 4:a863a092141c 200 void WriteServo(){
taurin 9:95eb0bbdc2a9 201 //kisokuServo.pulsewidth(calcPulse(airSpeed*10));
taurin 9:95eb0bbdc2a9 202 kisokuServo.pulsewidth(calcPulse(9*airSpeed));
taurin 9:95eb0bbdc2a9 203 if(pitch<0){
taurin 9:95eb0bbdc2a9 204 geikakuServo.pulsewidth(calcPulse(0));
taurin 9:95eb0bbdc2a9 205 }
taurin 9:95eb0bbdc2a9 206 else{
taurin 9:95eb0bbdc2a9 207 geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
taurin 9:95eb0bbdc2a9 208 }
taurin 6:98a63127aa11 209 //pc.printf("a:%f",airSpeed);
taurin 6:98a63127aa11 210 //pc.printf("p:%f\r\n",pitch);
taurin 5:1b3c8e5ee99e 211 //kisokuServo.pulsewidth(calcPulse(0));
taurin 5:1b3c8e5ee99e 212 //geikakuServo.pulsewidth(calcPulse(0));
taurin 4:a863a092141c 213 }
taurin 4:a863a092141c 214
taurin 0:085b2c5e3254 215 int main(){
taurin 0:085b2c5e3254 216 init();
taurin 0:085b2c5e3254 217 while(1){
taurin 9:95eb0bbdc2a9 218 pc.printf("test\n\r");
YusukeWakuta 18:0a0c4277d960 219 mpu6050.mpuProcessing(t);
taurin 8:31e07f6ed0f7 220 RollAlarm();
taurin 8:31e07f6ed0f7 221 DataReceiveFromSouda();
taurin 3:8dc516be2e7e 222 WriteDatas();
taurin 8:31e07f6ed0f7 223 WriteServo();
taurin 0:085b2c5e3254 224 }
taurin 0:085b2c5e3254 225 }