albatross / Mbed 2 deprecated keiki2017

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

Dependencies:   BufferedSoftSerial2 SDFileSystem-RTOS mbed mbed-rtos INA226_ver1

Fork of keiki2016ver5 by albatross

main.cpp@22:5cbebf097600, 2017-01-07 (annotated)

Committer:
tsumagari
Date:
Sat Jan 07 00:00:11 2017 +0000
Branch:
Thread????
Revision:
22:5cbebf097600
Parent:
21:8802034b7810
Child:
23:e59afb2044df
SDprintf???Thread???????

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