albatross / Mbed 2 deprecated keiki2017

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

Dependencies:   BufferedSoftSerial2 SDFileSystem-RTOS mbed mbed-rtos INA226_ver1

Fork of keiki2016ver5 by albatross

main.cpp@21:8802034b7810, 2017-01-06 (annotated)

Committer:
tsumagari
Date:
Fri Jan 06 11:41:32 2017 +0000
Branch:
Thread????
Revision:
21:8802034b7810
Parent:
18:fa3f9ba17af8
Child:
22:5cbebf097600
Serial?RawSerial??????

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 21:8802034b7810 22 Cadence cadence(p13,p14);
tsumagari 15:6966299bea4c 23 //Ticker cadenceTicker;
tsumagari 15:6966299bea4c 24
tsumagari 21:8802034b7810 25 RawSerial pc(USBTX,USBRX);
tsumagari 21:8802034b7810 26 RawSerial 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();
tsumagari 21:8802034b7810 67 void SDprintf(void const *argument);
taurin 0:085b2c5e3254 68 void DataReceiveFromSouda();
taurin 0:085b2c5e3254 69 void WriteDatas();
taurin 0:085b2c5e3254 70 float calcAttackAngle();
taurin 0:085b2c5e3254 71 float calcKXdeg(float x);
taurin 0:085b2c5e3254 72
tsumagari 15:6966299bea4c 73 void cadenceDataReceive(){
tsumagari 15:6966299bea4c 74 // cadence.readData();
tsumagari 15:6966299bea4c 75 }
tsumagari 15:6966299bea4c 76
taurin 0:085b2c5e3254 77 void air_countUp(){
taurin 0:085b2c5e3254 78 air_kaitensu++;
taurin 0:085b2c5e3254 79 }
taurin 0:085b2c5e3254 80
taurin 0:085b2c5e3254 81 void call_calcAirSpeed(){
taurin 0:085b2c5e3254 82 air.calcAirSpeed(air_kaitensu);
taurin 0:085b2c5e3254 83 air_kaitensu = 0;
taurin 0:085b2c5e3254 84 }
taurin 0:085b2c5e3254 85
taurin 0:085b2c5e3254 86 void init(){
tsumagari 15:6966299bea4c 87 twe.baud(19200);
YusukeWakuta 18:fa3f9ba17af8 88 Android.baud(9600);
tsumagari 15:6966299bea4c 89 soudaSerial.baud(9600);
taurin 5:1b3c8e5ee99e 90 kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 5:1b3c8e5ee99e 91 geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:085b2c5e3254 92 FusokukeiInit();
YusukeWakuta 17:0a0c4277d960 93 mpu6050.MPUInit(t);
taurin 8:31e07f6ed0f7 94 SdInit();
taurin 0:085b2c5e3254 95 }
taurin 0:085b2c5e3254 96
taurin 0:085b2c5e3254 97 void FusokukeiInit(){
taurin 0:085b2c5e3254 98 FusokukeiPin.rise(air_countUp);
taurin 0:085b2c5e3254 99 FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
taurin 0:085b2c5e3254 100 }
taurin 0:085b2c5e3254 101
taurin 4:a863a092141c 102 double calcPulse(int deg){
taurin 4:a863a092141c 103 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 4:a863a092141c 104 }
taurin 4:a863a092141c 105
taurin 0:085b2c5e3254 106 void SdInit(){
taurin 0:085b2c5e3254 107 mkdir("/sd/mydir", 0777);
taurin 0:085b2c5e3254 108 fp = fopen("/sd/mydir/sdtest2.csv", "w");
taurin 0:085b2c5e3254 109 if(fp == NULL) {
taurin 0:085b2c5e3254 110 error("Could not open file for write\n");
taurin 0:085b2c5e3254 111 }
taurin 0:085b2c5e3254 112 fprintf(fp, "Hello fun SD Card World!\n\r");
taurin 0:085b2c5e3254 113 fclose(fp);
taurin 0:085b2c5e3254 114 }
taurin 0:085b2c5e3254 115
taurin 0:085b2c5e3254 116 void DataReceiveFromSouda(){
taurin 1:5ec2409854da 117 led2 = !led2;
taurin 1:5ec2409854da 118 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
tsumagari 15:6966299bea4c 119 if(soudaSerial.readable()) {
tsumagari 15:6966299bea4c 120 soudaDatas[i] = (char)soudaSerial.getc();
tsumagari 15:6966299bea4c 121 if(soudaDatas[i]==';') i=-1;
tsumagari 15:6966299bea4c 122 }else i--;
taurin 0:085b2c5e3254 123 }
taurin 0:085b2c5e3254 124 }
taurin 0:085b2c5e3254 125
tsumagari 21:8802034b7810 126 void SDprintf(void const *argument){
tsumagari 21:8802034b7810 127 if(write_datas_index == SD_WRITE_NUM-1){
tsumagari 21:8802034b7810 128 fp = fopen("/sd/mydir/sdtest.csv", "a");
tsumagari 21:8802034b7810 129 if(fp == NULL) {
tsumagari 21:8802034b7810 130 error("Could not open file for write\n");
tsumagari 21:8802034b7810 131 }
tsumagari 21:8802034b7810 132 for(int i = 0; i < SD_WRITE_NUM; i++){
tsumagari 21:8802034b7810 133 for(int j = 0; j < WRITE_DATAS_NUM; j++){
tsumagari 21:8802034b7810 134 fprintf(fp,"%f,", writeDatas[i][j]);
tsumagari 21:8802034b7810 135 }
tsumagari 21:8802034b7810 136 }
tsumagari 21:8802034b7810 137 fprintf(fp,"\n\r");
tsumagari 21:8802034b7810 138 fclose(fp);
tsumagari 21:8802034b7810 139 write_datas_index=0;
taurin 1:5ec2409854da 140 }
taurin 3:8dc516be2e7e 141 }
taurin 3:8dc516be2e7e 142
taurin 3:8dc516be2e7e 143 void WriteDatas(){
taurin 8:31e07f6ed0f7 144 int i;
taurin 8:31e07f6ed0f7 145 for(i = 0; i < SOUDA_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 146 //writeDatas[write_datas_index][i] = 0.0;
taurin 8:31e07f6ed0f7 147 writeDatas[write_datas_index][i] = (float)soudaDatas[i];
taurin 8:31e07f6ed0f7 148 }
tsumagari 15:6966299bea4c 149 // writeDatas[write_datas_index][i++] = cadence.cadence;
taurin 8:31e07f6ed0f7 150 writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
taurin 8:31e07f6ed0f7 151 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
taurin 8:31e07f6ed0f7 152 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
taurin 8:31e07f6ed0f7 153 writeDatas[write_datas_index][i++] = pitch;
taurin 8:31e07f6ed0f7 154 writeDatas[write_datas_index][i++] = roll;
taurin 8:31e07f6ed0f7 155 writeDatas[write_datas_index][i++] = yaw;
taurin 8:31e07f6ed0f7 156 writeDatas[write_datas_index][i++] = airSpeed;
taurin 8:31e07f6ed0f7 157 //writeDatas[write_datas_index][i++] = writeTimer.read();
taurin 9:95eb0bbdc2a9 158 //for(i = 0; i < WRITE_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 159 // pc.printf("%f ", writeDatas[write_datas_index][i]);
taurin 9:95eb0bbdc2a9 160 // twe.printf("%f,", writeDatas[write_datas_index][i]);
taurin 9:95eb0bbdc2a9 161 // }
taurin 9:95eb0bbdc2a9 162 // pc.printf("\n\r");
taurin 9:95eb0bbdc2a9 163 // twe.printf("\n\r");
taurin 8:31e07f6ed0f7 164 if(write_datas_index == SD_WRITE_NUM-1){
tsumagari 21:8802034b7810 165 // SDprintf();
taurin 8:31e07f6ed0f7 166 write_datas_index=0;
taurin 8:31e07f6ed0f7 167 }
taurin 8:31e07f6ed0f7 168 else{
taurin 8:31e07f6ed0f7 169 write_datas_index++;
taurin 8:31e07f6ed0f7 170 }
taurin 9:95eb0bbdc2a9 171 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 172 pc.printf("%i ",soudaDatas[i]);
YusukeWakuta 10:1389f938f5f7 173 twe.printf("%i,",soudaDatas[i]);
taurin 9:95eb0bbdc2a9 174 }
tsumagari 15:6966299bea4c 175 // twe.printf("%f\n\r",cadence.cadence);
tsumagari 15:6966299bea4c 176 // pc.printf("%f\n\r",cadence.cadence);
taurin 9:95eb0bbdc2a9 177 //pc.printf("\n\r");
YusukeWakuta 18:fa3f9ba17af8 178
YusukeWakuta 18:fa3f9ba17af8 179 if(Android.writeable()){
YusukeWakuta 18:fa3f9ba17af8 180 Android.printf("%f,%f,%f",airSpeed,roll,0);
YusukeWakuta 18:fa3f9ba17af8 181 }
YusukeWakuta 10:1389f938f5f7 182 twe.printf("%f,%f,%f,",pitch,roll,yaw);
YusukeWakuta 10:1389f938f5f7 183 twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
YusukeWakuta 10:1389f938f5f7 184 twe.printf("%f,\r\n",airSpeed);
taurin 9:95eb0bbdc2a9 185 pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
taurin 9:95eb0bbdc2a9 186 //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
taurin 9:95eb0bbdc2a9 187 pc.printf("%f\n\r",airSpeed);
taurin 9:95eb0bbdc2a9 188 //SDprintf();
taurin 9:95eb0bbdc2a9 189 }
taurin 9:95eb0bbdc2a9 190
taurin 9:95eb0bbdc2a9 191 void WriteDatasF(){
taurin 9:95eb0bbdc2a9 192 pc.printf("airSpeed:%f\n\r",airSpeed);
taurin 0:085b2c5e3254 193 }
taurin 0:085b2c5e3254 194
taurin 0:085b2c5e3254 195 float calcKXdeg(float x){
taurin 0:085b2c5e3254 196 return -310.54*x+156.65;
taurin 0:085b2c5e3254 197 }
taurin 0:085b2c5e3254 198
taurin 0:085b2c5e3254 199 float calcAttackAngle(){
taurin 0:085b2c5e3254 200 return pitch-calcKXdeg(kx_Z.read());
taurin 0:085b2c5e3254 201 }
taurin 0:085b2c5e3254 202
taurin 0:085b2c5e3254 203 void RollAlarm(){
taurin 0:085b2c5e3254 204 if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
taurin 0:085b2c5e3254 205 RollAlarmL = 1;
taurin 0:085b2c5e3254 206 }
taurin 0:085b2c5e3254 207 else{
taurin 0:085b2c5e3254 208 RollAlarmL = 0;
taurin 0:085b2c5e3254 209 }
taurin 0:085b2c5e3254 210 if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
taurin 0:085b2c5e3254 211 RollAlarmR = 1;
taurin 0:085b2c5e3254 212 }
taurin 0:085b2c5e3254 213 else{
taurin 0:085b2c5e3254 214 RollAlarmR = 0;
taurin 0:085b2c5e3254 215 }
taurin 0:085b2c5e3254 216 }
taurin 0:085b2c5e3254 217
taurin 4:a863a092141c 218 void WriteServo(){
taurin 9:95eb0bbdc2a9 219 kisokuServo.pulsewidth(calcPulse(9*airSpeed));
taurin 9:95eb0bbdc2a9 220 if(pitch<0){
taurin 9:95eb0bbdc2a9 221 geikakuServo.pulsewidth(calcPulse(0));
taurin 9:95eb0bbdc2a9 222 }
taurin 9:95eb0bbdc2a9 223 else{
taurin 9:95eb0bbdc2a9 224 geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
taurin 9:95eb0bbdc2a9 225 }
taurin 4:a863a092141c 226 }
taurin 4:a863a092141c 227
taurin 0:085b2c5e3254 228 int main(){
taurin 0:085b2c5e3254 229 init();
taurin 0:085b2c5e3254 230 while(1){
taurin 9:95eb0bbdc2a9 231 pc.printf("test\n\r");
YusukeWakuta 17:0a0c4277d960 232 mpu6050.mpuProcessing(t);
taurin 8:31e07f6ed0f7 233 RollAlarm();
taurin 8:31e07f6ed0f7 234 DataReceiveFromSouda();
tsumagari 15:6966299bea4c 235 // cadence.readData();
taurin 3:8dc516be2e7e 236 WriteDatas();
taurin 8:31e07f6ed0f7 237 WriteServo();
taurin 0:085b2c5e3254 238 }
taurin 0:085b2c5e3254 239 }