albatross / Mbed 2 deprecated keiki2017

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

Dependencies:   BufferedSoftSerial2 SDFileSystem-RTOS mbed mbed-rtos INA226_ver1

Fork of keiki2016ver5 by albatross

main.cpp@23:e59afb2044df, 2017-01-07 (annotated)

Committer:
tsumagari
Date:
Sat Jan 07 02:03:08 2017 +0000
Branch:
Thread????
Revision:
23:e59afb2044df
Parent:
22:5cbebf097600
Child:
26:50272431cd1e
SD????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);
tsumagari 23:e59afb2044df 51 DigitalOut led4(LED4);
taurin 0:085b2c5e3254 52
taurin 0:085b2c5e3254 53 SDFileSystem sd(p5, p6, p7, p8, "sd");
taurin 0:085b2c5e3254 54 FILE* fp;
taurin 0:085b2c5e3254 55
YusukeWakuta 11:90e2b070f337 56 PwmOut kisokuServo(p26);
YusukeWakuta 11:90e2b070f337 57 PwmOut geikakuServo(p22);
taurin 4:a863a092141c 58
taurin 0:085b2c5e3254 59 char soudaDatas[SOUDA_DATAS_NUM];
taurin 3:8dc516be2e7e 60 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
taurin 3:8dc516be2e7e 61 volatile int write_datas_index = 0;
taurin 0:085b2c5e3254 62
tsumagari 15:6966299bea4c 63 void cadenceDataReceive();
taurin 0:085b2c5e3254 64 void air_countUp();
taurin 0:085b2c5e3254 65 void call_calcAirSpeed();
taurin 0:085b2c5e3254 66 void init();
taurin 0:085b2c5e3254 67 void FusokukeiInit();
taurin 0:085b2c5e3254 68 void SdInit();
tsumagari 21:8802034b7810 69 void SDprintf(void const *argument);
taurin 0:085b2c5e3254 70 void DataReceiveFromSouda();
taurin 0:085b2c5e3254 71 void WriteDatas();
taurin 0:085b2c5e3254 72 float calcAttackAngle();
taurin 0:085b2c5e3254 73 float calcKXdeg(float x);
taurin 0:085b2c5e3254 74
tsumagari 15:6966299bea4c 75 void cadenceDataReceive(){
tsumagari 15:6966299bea4c 76 // cadence.readData();
tsumagari 15:6966299bea4c 77 }
tsumagari 15:6966299bea4c 78
taurin 0:085b2c5e3254 79 void air_countUp(){
taurin 0:085b2c5e3254 80 air_kaitensu++;
taurin 0:085b2c5e3254 81 }
taurin 0:085b2c5e3254 82
taurin 0:085b2c5e3254 83 void call_calcAirSpeed(){
taurin 0:085b2c5e3254 84 air.calcAirSpeed(air_kaitensu);
taurin 0:085b2c5e3254 85 air_kaitensu = 0;
taurin 0:085b2c5e3254 86 }
taurin 0:085b2c5e3254 87
taurin 0:085b2c5e3254 88 void init(){
tsumagari 15:6966299bea4c 89 twe.baud(19200);
YusukeWakuta 18:fa3f9ba17af8 90 Android.baud(9600);
tsumagari 15:6966299bea4c 91 soudaSerial.baud(9600);
taurin 5:1b3c8e5ee99e 92 kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 5:1b3c8e5ee99e 93 geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:085b2c5e3254 94 FusokukeiInit();
YusukeWakuta 17:0a0c4277d960 95 mpu6050.MPUInit(t);
tsumagari 23:e59afb2044df 96 // SdInit();//thread 内で
taurin 0:085b2c5e3254 97 }
taurin 0:085b2c5e3254 98
taurin 0:085b2c5e3254 99 void FusokukeiInit(){
taurin 0:085b2c5e3254 100 FusokukeiPin.rise(air_countUp);
taurin 0:085b2c5e3254 101 FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
taurin 0:085b2c5e3254 102 }
taurin 0:085b2c5e3254 103
taurin 4:a863a092141c 104 double calcPulse(int deg){
taurin 4:a863a092141c 105 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 4:a863a092141c 106 }
taurin 4:a863a092141c 107
taurin 0:085b2c5e3254 108 void SdInit(){
taurin 0:085b2c5e3254 109 mkdir("/sd/mydir", 0777);
taurin 0:085b2c5e3254 110 fp = fopen("/sd/mydir/sdtest2.csv", "w");
taurin 0:085b2c5e3254 111 if(fp == NULL) {
taurin 0:085b2c5e3254 112 error("Could not open file for write\n");
taurin 0:085b2c5e3254 113 }
taurin 0:085b2c5e3254 114 fprintf(fp, "Hello fun SD Card World!\n\r");
taurin 0:085b2c5e3254 115 fclose(fp);
tsumagari 23:e59afb2044df 116 // Thread sd_thread(&SDprintf);
taurin 0:085b2c5e3254 117 }
taurin 0:085b2c5e3254 118
taurin 0:085b2c5e3254 119 void DataReceiveFromSouda(){
taurin 1:5ec2409854da 120 led2 = !led2;
taurin 1:5ec2409854da 121 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
tsumagari 15:6966299bea4c 122 if(soudaSerial.readable()) {
tsumagari 15:6966299bea4c 123 soudaDatas[i] = (char)soudaSerial.getc();
tsumagari 15:6966299bea4c 124 if(soudaDatas[i]==';') i=-1;
tsumagari 15:6966299bea4c 125 }else i--;
taurin 0:085b2c5e3254 126 }
taurin 0:085b2c5e3254 127 }
taurin 0:085b2c5e3254 128
tsumagari 21:8802034b7810 129 void SDprintf(void const *argument){
tsumagari 23:e59afb2044df 130 SdInit();
tsumagari 22:5cbebf097600 131 while(1){
tsumagari 23:e59afb2044df 132 if(write_datas_index == SD_WRITE_NUM-1){
tsumagari 23:e59afb2044df 133 led4 = !led4;
tsumagari 23:e59afb2044df 134 fp = fopen("/sd/mydir/sdtest.csv", "a");
tsumagari 23:e59afb2044df 135 if(fp == NULL) {
tsumagari 23:e59afb2044df 136 error("Could not open file for write\n");
tsumagari 23:e59afb2044df 137 }
tsumagari 23:e59afb2044df 138 for(int i = 0; i < SD_WRITE_NUM; i++){
tsumagari 23:e59afb2044df 139 for(int j = 0; j < WRITE_DATAS_NUM; j++){
tsumagari 23:e59afb2044df 140 fprintf(fp,"%f,", writeDatas[i][j]);
tsumagari 23:e59afb2044df 141 }
tsumagari 23:e59afb2044df 142 }
tsumagari 23:e59afb2044df 143 fprintf(fp,"\n\r");
tsumagari 23:e59afb2044df 144 fclose(fp);
tsumagari 23:e59afb2044df 145 write_datas_index=0;
tsumagari 21:8802034b7810 146 }
tsumagari 23:e59afb2044df 147 Thread::wait(10);
tsumagari 22:5cbebf097600 148 }
taurin 3:8dc516be2e7e 149 }
taurin 3:8dc516be2e7e 150
taurin 3:8dc516be2e7e 151 void WriteDatas(){
taurin 8:31e07f6ed0f7 152 int i;
taurin 8:31e07f6ed0f7 153 for(i = 0; i < SOUDA_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 154 //writeDatas[write_datas_index][i] = 0.0;
taurin 8:31e07f6ed0f7 155 writeDatas[write_datas_index][i] = (float)soudaDatas[i];
taurin 8:31e07f6ed0f7 156 }
tsumagari 15:6966299bea4c 157 // writeDatas[write_datas_index][i++] = cadence.cadence;
taurin 8:31e07f6ed0f7 158 writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
taurin 8:31e07f6ed0f7 159 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
taurin 8:31e07f6ed0f7 160 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
taurin 8:31e07f6ed0f7 161 writeDatas[write_datas_index][i++] = pitch;
taurin 8:31e07f6ed0f7 162 writeDatas[write_datas_index][i++] = roll;
taurin 8:31e07f6ed0f7 163 writeDatas[write_datas_index][i++] = yaw;
taurin 8:31e07f6ed0f7 164 writeDatas[write_datas_index][i++] = airSpeed;
taurin 8:31e07f6ed0f7 165 //writeDatas[write_datas_index][i++] = writeTimer.read();
taurin 9:95eb0bbdc2a9 166 //for(i = 0; i < WRITE_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 167 // pc.printf("%f ", writeDatas[write_datas_index][i]);
taurin 9:95eb0bbdc2a9 168 // twe.printf("%f,", writeDatas[write_datas_index][i]);
taurin 9:95eb0bbdc2a9 169 // }
taurin 9:95eb0bbdc2a9 170 // pc.printf("\n\r");
taurin 9:95eb0bbdc2a9 171 // twe.printf("\n\r");
tsumagari 23:e59afb2044df 172 if(write_datas_index < SD_WRITE_NUM-1){
taurin 8:31e07f6ed0f7 173 write_datas_index++;
taurin 8:31e07f6ed0f7 174 }
taurin 9:95eb0bbdc2a9 175 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 176 pc.printf("%i ",soudaDatas[i]);
YusukeWakuta 10:1389f938f5f7 177 twe.printf("%i,",soudaDatas[i]);
taurin 9:95eb0bbdc2a9 178 }
tsumagari 15:6966299bea4c 179 // twe.printf("%f\n\r",cadence.cadence);
tsumagari 15:6966299bea4c 180 // pc.printf("%f\n\r",cadence.cadence);
taurin 9:95eb0bbdc2a9 181 //pc.printf("\n\r");
YusukeWakuta 18:fa3f9ba17af8 182
YusukeWakuta 18:fa3f9ba17af8 183 if(Android.writeable()){
YusukeWakuta 18:fa3f9ba17af8 184 Android.printf("%f,%f,%f",airSpeed,roll,0);
YusukeWakuta 18:fa3f9ba17af8 185 }
YusukeWakuta 10:1389f938f5f7 186 twe.printf("%f,%f,%f,",pitch,roll,yaw);
YusukeWakuta 10:1389f938f5f7 187 twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
YusukeWakuta 10:1389f938f5f7 188 twe.printf("%f,\r\n",airSpeed);
taurin 9:95eb0bbdc2a9 189 pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
taurin 9:95eb0bbdc2a9 190 //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
taurin 9:95eb0bbdc2a9 191 pc.printf("%f\n\r",airSpeed);
taurin 9:95eb0bbdc2a9 192 //SDprintf();
tsumagari 23:e59afb2044df 193 pc.printf("%d\n\r",write_datas_index);
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(){
tsumagari 23:e59afb2044df 234 Thread sd_thread(&SDprintf);//必ずmain内で
taurin 0:085b2c5e3254 235 init();
taurin 0:085b2c5e3254 236 while(1){
taurin 9:95eb0bbdc2a9 237 pc.printf("test\n\r");
YusukeWakuta 17:0a0c4277d960 238 mpu6050.mpuProcessing(t);
taurin 8:31e07f6ed0f7 239 RollAlarm();
taurin 8:31e07f6ed0f7 240 DataReceiveFromSouda();
tsumagari 15:6966299bea4c 241 // cadence.readData();
taurin 3:8dc516be2e7e 242 WriteDatas();
taurin 8:31e07f6ed0f7 243 WriteServo();
taurin 0:085b2c5e3254 244 }
taurin 0:085b2c5e3254 245 }