albatross / Mbed 2 deprecated keiki2017

2017年度の製作を開始します。

Dependencies:   BufferedSoftSerial2 SDFileSystem-RTOS mbed mbed-rtos INA226_ver1

Fork of keiki2016ver5 by albatross

main.cpp@24:483dd4e61ead, 2017-01-07 (annotated)

Committer:
YusukeWakuta
Date:
Sat Jan 07 04:32:29 2017 +0000
Branch:
Android?????????
Revision:
24:483dd4e61ead
Parent:
18:fa3f9ba17af8 
????

Who changed what in which revision?

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