albatross / Mbed 2 deprecated keiki2017

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

Dependencies:   BufferedSoftSerial2 SDFileSystem-RTOS mbed mbed-rtos INA226_ver1

Fork of keiki2016ver5 by albatross

main.cpp@30:66fa18093418, 2017-02-17 (annotated)

Committer:
tsumagari
Date:
Fri Feb 17 09:04:48 2017 +0000
Revision:
30:66fa18093418
Parent:
29:2da9b8d03c0b
Child:
32:fe006d07c1ea
mpu6050_DMP6.h????????????????

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"
tsumagari 29:2da9b8d03c0b 6 //#include "xMPU6050.h"
tsumagari 29:2da9b8d03c0b 7 #include "MPU6050_DMP6.h"
taurin 0:085b2c5e3254 8 #include "SDFileSystem.h"
tsumagari 15:6966299bea4c 9 #include "BufferedSoftSerial.h"
tsumagari 15:6966299bea4c 10 #include "Cadence.h"
taurin 0:085b2c5e3254 11
taurin 0:085b2c5e3254 12 #define KX_VALUE_MIN 0.4
taurin 0:085b2c5e3254 13 #define KX_VALUE_MAX 0.8
tsumagari 26:50272431cd1e 14 #define SOUDA_DATAS_NUM 18
tsumagari 26:50272431cd1e 15 #define YOKUTAN_DATAS_NUM 14
tsumagari 26:50272431cd1e 16 #define WRITE_DATAS_NUM 18
taurin 0:085b2c5e3254 17 #define AIR_LOOP_TIME 0.01
taurin 3:8dc516be2e7e 18 #define WRITE_DATAS_LOOP_TIME 1
taurin 3:8dc516be2e7e 19 #define ROLL_R_MAX_DEG 2
taurin 3:8dc516be2e7e 20 #define ROLL_L_MAX_DEG 2
tsumagari 26:50272431cd1e 21 #define SD_WRITE_NUM 20
taurin 4:a863a092141c 22 #define INIT_SERVO_PERIOD_MS 20
taurin 0:085b2c5e3254 23
tsumagari 26:50272431cd1e 24 #define MPU_LOOP_TIME 0.01
tsumagari 26:50272431cd1e 25 #define MPU_DELT_MIN 250
tsumagari 26:50272431cd1e 26
tsumagari 26:50272431cd1e 27 Mutex ssMutex;
tsumagari 26:50272431cd1e 28
tsumagari 26:50272431cd1e 29 //-----------------------------------(resetInterrupt def)
tsumagari 26:50272431cd1e 30 extern "C" void mbed_reset();
tsumagari 26:50272431cd1e 31 InterruptIn resetPin(p26);
tsumagari 26:50272431cd1e 32 Timer resetTimeCount;
tsumagari 26:50272431cd1e 33 void resetInterrupt(){
tsumagari 26:50272431cd1e 34 while(resetPin){
tsumagari 26:50272431cd1e 35 resetTimeCount.start();
tsumagari 26:50272431cd1e 36 if(resetTimeCount.read()>3) mbed_reset();
tsumagari 26:50272431cd1e 37 }
tsumagari 26:50272431cd1e 38 resetTimeCount.reset();
tsumagari 26:50272431cd1e 39 }
tsumagari 26:50272431cd1e 40 //-------------------------------------------------------
tsumagari 26:50272431cd1e 41
tsumagari 26:50272431cd1e 42
tsumagari 21:8802034b7810 43 Cadence cadence(p13,p14);
tsumagari 15:6966299bea4c 44
tsumagari 21:8802034b7810 45 RawSerial pc(USBTX,USBRX);
tsumagari 26:50272431cd1e 46 BufferedSoftSerial twe(p11,p12);
tsumagari 26:50272431cd1e 47 RawSerial Android(p9,p10);
tsumagari 15:6966299bea4c 48 BufferedSoftSerial soudaSerial(p17,p18);
taurin 3:8dc516be2e7e 49 Timer writeTimer;
taurin 0:085b2c5e3254 50
tsumagari 30:66fa18093418 51 InterruptIn FusokukeiPin(p23);
taurin 0:085b2c5e3254 52 Ticker FusokukeiTicker;
taurin 0:085b2c5e3254 53 Fusokukei air;
taurin 0:085b2c5e3254 54 volatile int air_kaitensu= 0;
taurin 0:085b2c5e3254 55
tsumagari 26:50272431cd1e 56 float sum = 0;
tsumagari 26:50272431cd1e 57 uint32_t sumCount = 0;
tsumagari 29:2da9b8d03c0b 58 //MPU6050 mpu6050;
tsumagari 29:2da9b8d03c0b 59 float yaw,pitch,roll;
tsumagari 29:2da9b8d03c0b 60 float pre_ypr[3];
tsumagari 29:2da9b8d03c0b 61 float offset_ypr[3];
taurin 0:085b2c5e3254 62 Timer t;
tsumagari 26:50272431cd1e 63 //
tsumagari 26:50272431cd1e 64 //AnalogIn kx_X(p17);
tsumagari 26:50272431cd1e 65 //AnalogIn kx_Y(p16);
tsumagari 26:50272431cd1e 66 //AnalogIn kx_Z(p15);
tsumagari 26:50272431cd1e 67 //float KX_X,KX_Y,KX_Z;
taurin 0:085b2c5e3254 68
tsumagari 26:50272431cd1e 69
tsumagari 26:50272431cd1e 70 //Timer sonarTimer;
tsumagari 26:50272431cd1e 71 //InterruptIn sonarPin(p22);
tsumagari 26:50272431cd1e 72 AnalogIn sonarPin(p15);
tsumagari 26:50272431cd1e 73 //double sonarDistTime=0;
tsumagari 26:50272431cd1e 74 double sonarV = 0.0, sonarDist = 0.0;
taurin 0:085b2c5e3254 75
taurin 0:085b2c5e3254 76 DigitalOut RollAlarmR(p20);
taurin 0:085b2c5e3254 77 DigitalOut RollAlarmL(p19);
tsumagari 29:2da9b8d03c0b 78 //DigitalOut led(LED1);//for mpu
taurin 1:5ec2409854da 79 DigitalOut led2(LED2);
tsumagari 26:50272431cd1e 80 DigitalOut led3(LED3);
tsumagari 23:e59afb2044df 81 DigitalOut led4(LED4);
taurin 0:085b2c5e3254 82
taurin 0:085b2c5e3254 83 SDFileSystem sd(p5, p6, p7, p8, "sd");
taurin 0:085b2c5e3254 84 FILE* fp;
taurin 0:085b2c5e3254 85
YusukeWakuta 11:90e2b070f337 86 PwmOut kisokuServo(p26);
YusukeWakuta 11:90e2b070f337 87 PwmOut geikakuServo(p22);
taurin 4:a863a092141c 88
taurin 0:085b2c5e3254 89 char soudaDatas[SOUDA_DATAS_NUM];
taurin 3:8dc516be2e7e 90 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
taurin 3:8dc516be2e7e 91 volatile int write_datas_index = 0;
taurin 0:085b2c5e3254 92
tsumagari 30:66fa18093418 93 void cadenceDataReceive(void const *argument);
taurin 0:085b2c5e3254 94 void air_countUp();
taurin 0:085b2c5e3254 95 void call_calcAirSpeed();
taurin 0:085b2c5e3254 96 void init();
taurin 0:085b2c5e3254 97 void FusokukeiInit();
taurin 0:085b2c5e3254 98 void SdInit();
tsumagari 30:66fa18093418 99 //void MpuInit();
tsumagari 26:50272431cd1e 100 //void SonarInit();
tsumagari 30:66fa18093418 101 void mpuProcessing(void const *argument);
tsumagari 30:66fa18093418 102 void sonarCalc(void const *argument);
tsumagari 30:66fa18093418 103 void DataReceiveFromSouda(void const *argument);
tsumagari 21:8802034b7810 104 void SDprintf(void const *argument);
taurin 0:085b2c5e3254 105 void WriteDatas();
tsumagari 26:50272431cd1e 106 //float calcAttackAngle();
tsumagari 26:50272431cd1e 107 //float calcKXdeg(float x);
taurin 0:085b2c5e3254 108
tsumagari 30:66fa18093418 109 void cadenceDataReceive(void const *argument){
tsumagari 26:50272431cd1e 110 while(1){
tsumagari 26:50272431cd1e 111 cadence.readData();
tsumagari 26:50272431cd1e 112 Thread::wait(0.01);
tsumagari 26:50272431cd1e 113 }
tsumagari 15:6966299bea4c 114 }
tsumagari 15:6966299bea4c 115
taurin 0:085b2c5e3254 116 void air_countUp(){
taurin 0:085b2c5e3254 117 air_kaitensu++;
taurin 0:085b2c5e3254 118 }
taurin 0:085b2c5e3254 119
taurin 0:085b2c5e3254 120 void call_calcAirSpeed(){
taurin 0:085b2c5e3254 121 air.calcAirSpeed(air_kaitensu);
taurin 0:085b2c5e3254 122 air_kaitensu = 0;
tsumagari 29:2da9b8d03c0b 123 led3 = !led3;
taurin 0:085b2c5e3254 124 }
taurin 0:085b2c5e3254 125
tsumagari 26:50272431cd1e 126 //void sonarInterruptStart(){
tsumagari 26:50272431cd1e 127 // sonarTimer.start();
tsumagari 26:50272431cd1e 128 // led3 = 1;
tsumagari 26:50272431cd1e 129 //}
tsumagari 26:50272431cd1e 130 //void sonarInterruptStop(){
tsumagari 26:50272431cd1e 131 // sonarTimer.stop();
tsumagari 26:50272431cd1e 132 // led3 = 0;
tsumagari 26:50272431cd1e 133 // sonarDistTime = sonarTimer.read_us();
tsumagari 26:50272431cd1e 134 // sonarTimer.reset();
tsumagari 26:50272431cd1e 135 //}
tsumagari 26:50272431cd1e 136
taurin 0:085b2c5e3254 137 void init(){
tsumagari 26:50272431cd1e 138 //--------------------------------------(resetInterrupt init)
tsumagari 26:50272431cd1e 139 resetPin.rise(resetInterrupt);
tsumagari 26:50272431cd1e 140 resetPin.mode(PullDown);
tsumagari 26:50272431cd1e 141 //-----------------------------------------------------------
tsumagari 26:50272431cd1e 142 twe.baud(115200);
YusukeWakuta 18:fa3f9ba17af8 143 Android.baud(9600);
tsumagari 15:6966299bea4c 144 soudaSerial.baud(9600);
taurin 5:1b3c8e5ee99e 145 kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 5:1b3c8e5ee99e 146 geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:085b2c5e3254 147 FusokukeiInit();
tsumagari 26:50272431cd1e 148 // MpuInit(); //mpuProcessing内で
tsumagari 26:50272431cd1e 149 // SonarInit();
tsumagari 26:50272431cd1e 150 // SdInit();//SDprintf 内で
taurin 0:085b2c5e3254 151 }
taurin 0:085b2c5e3254 152
taurin 0:085b2c5e3254 153 void FusokukeiInit(){
tsumagari 29:2da9b8d03c0b 154 FusokukeiPin.rise(&air_countUp);
taurin 0:085b2c5e3254 155 FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
taurin 0:085b2c5e3254 156 }
taurin 0:085b2c5e3254 157
taurin 4:a863a092141c 158 double calcPulse(int deg){
taurin 4:a863a092141c 159 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 4:a863a092141c 160 }
taurin 4:a863a092141c 161
taurin 0:085b2c5e3254 162 void SdInit(){
taurin 0:085b2c5e3254 163 mkdir("/sd/mydir", 0777);
taurin 0:085b2c5e3254 164 fp = fopen("/sd/mydir/sdtest2.csv", "w");
taurin 0:085b2c5e3254 165 if(fp == NULL) {
taurin 0:085b2c5e3254 166 error("Could not open file for write\n");
taurin 0:085b2c5e3254 167 }
taurin 0:085b2c5e3254 168 fprintf(fp, "Hello fun SD Card World!\n\r");
taurin 0:085b2c5e3254 169 fclose(fp);
taurin 0:085b2c5e3254 170 }
tsumagari 29:2da9b8d03c0b 171
tsumagari 30:66fa18093418 172 //void MpuInit(){
tsumagari 29:2da9b8d03c0b 173 // i2c.frequency(400000); // use fast (400 kHz) I2C
tsumagari 29:2da9b8d03c0b 174 // t.start();
tsumagari 29:2da9b8d03c0b 175 // uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
tsumagari 29:2da9b8d03c0b 176 // if (whoami == 0x68) { // WHO_AM_I should always be 0x68
tsumagari 29:2da9b8d03c0b 177 // Thread::wait(1);
tsumagari 29:2da9b8d03c0b 178 // mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
tsumagari 29:2da9b8d03c0b 179 // Thread::wait(1);
tsumagari 29:2da9b8d03c0b 180 // if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
tsumagari 29:2da9b8d03c0b 181 // mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
tsumagari 29:2da9b8d03c0b 182 // mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
tsumagari 29:2da9b8d03c0b 183 // mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
tsumagari 29:2da9b8d03c0b 184 // Thread::wait(2);
tsumagari 29:2da9b8d03c0b 185 // } else {
tsumagari 29:2da9b8d03c0b 186 // }
tsumagari 29:2da9b8d03c0b 187 // } else {
tsumagari 29:2da9b8d03c0b 188 // pc.printf("MPU6050 not ready...\n\r");
tsumagari 29:2da9b8d03c0b 189 // //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
tsumagari 29:2da9b8d03c0b 190 // }
tsumagari 30:66fa18093418 191 // using namespace MPU6050DMP6;
tsumagari 30:66fa18093418 192 // setup();
tsumagari 30:66fa18093418 193 // do{
tsumagari 30:66fa18093418 194 // for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
tsumagari 30:66fa18093418 195 // getYPR();
tsumagari 30:66fa18093418 196 // Thread::wait(0.01);
tsumagari 30:66fa18093418 197 // }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
tsumagari 30:66fa18093418 198 // for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
tsumagari 30:66fa18093418 199 //}
taurin 0:085b2c5e3254 200
tsumagari 26:50272431cd1e 201 //void SonarInit(){
tsumagari 26:50272431cd1e 202 // sonarPin.rise(&sonarInterruptStart);
tsumagari 26:50272431cd1e 203 // sonarPin.fall(&sonarInterruptStop);
tsumagari 26:50272431cd1e 204 //}
tsumagari 26:50272431cd1e 205
tsumagari 29:2da9b8d03c0b 206 //void mpuProcessing(void const *argument){
tsumagari 29:2da9b8d03c0b 207 // MpuInit();
tsumagari 29:2da9b8d03c0b 208 // while(1){
tsumagari 29:2da9b8d03c0b 209 // Thread::wait(0.1);
tsumagari 29:2da9b8d03c0b 210 // if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
tsumagari 29:2da9b8d03c0b 211 // mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
tsumagari 29:2da9b8d03c0b 212 // mpu6050.getAres();
tsumagari 29:2da9b8d03c0b 213 // ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
tsumagari 29:2da9b8d03c0b 214 // ay = (float)accelCount[1]*aRes - accelBias[1];
tsumagari 29:2da9b8d03c0b 215 // az = (float)accelCount[2]*aRes - accelBias[2];
tsumagari 29:2da9b8d03c0b 216 // mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
tsumagari 29:2da9b8d03c0b 217 // mpu6050.getGres();
tsumagari 29:2da9b8d03c0b 218 // gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
tsumagari 29:2da9b8d03c0b 219 // gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
tsumagari 29:2da9b8d03c0b 220 // gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
tsumagari 29:2da9b8d03c0b 221 // tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
tsumagari 29:2da9b8d03c0b 222 // temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
tsumagari 29:2da9b8d03c0b 223 // }
tsumagari 29:2da9b8d03c0b 224 // Now = t.read_us();
tsumagari 29:2da9b8d03c0b 225 // deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
tsumagari 29:2da9b8d03c0b 226 // lastUpdate = Now;
tsumagari 29:2da9b8d03c0b 227 // sum += deltat;
tsumagari 29:2da9b8d03c0b 228 // sumCount++;
tsumagari 29:2da9b8d03c0b 229 // if(lastUpdate - firstUpdate > 10000000.0f) {
tsumagari 29:2da9b8d03c0b 230 // beta = 0.04; // decrease filter gain after stabilized
tsumagari 29:2da9b8d03c0b 231 // zeta = 0.015; // increasey bias drift gain after stabilized
tsumagari 29:2da9b8d03c0b 232 // }
tsumagari 29:2da9b8d03c0b 233 // mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
tsumagari 29:2da9b8d03c0b 234 // delt_t = t.read_ms() - count;
tsumagari 29:2da9b8d03c0b 235 // if (delt_t > MPU_DELT_MIN) {
tsumagari 29:2da9b8d03c0b 236 // yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
tsumagari 29:2da9b8d03c0b 237 // pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
tsumagari 29:2da9b8d03c0b 238 // roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
tsumagari 29:2da9b8d03c0b 239 // pitch *= 180.0f / PI;
tsumagari 29:2da9b8d03c0b 240 // yaw *= 180.0f / PI;
tsumagari 29:2da9b8d03c0b 241 // roll *= 180.0f / PI;
tsumagari 29:2da9b8d03c0b 242 // myled= !myled;
tsumagari 29:2da9b8d03c0b 243 // count = t.read_ms();
tsumagari 29:2da9b8d03c0b 244 // sum = 0;
tsumagari 29:2da9b8d03c0b 245 // sumCount = 0;
tsumagari 29:2da9b8d03c0b 246 // }
tsumagari 29:2da9b8d03c0b 247 // }
tsumagari 29:2da9b8d03c0b 248 //}
tsumagari 26:50272431cd1e 249 void mpuProcessing(void const *argument){
tsumagari 29:2da9b8d03c0b 250 using namespace MPU6050DMP6;
tsumagari 29:2da9b8d03c0b 251 setup();
tsumagari 30:66fa18093418 252 // do{
tsumagari 30:66fa18093418 253 // for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
tsumagari 29:2da9b8d03c0b 254 getYPR();
tsumagari 30:66fa18093418 255 // Thread::wait(0.05);
tsumagari 30:66fa18093418 256 // }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
tsumagari 29:2da9b8d03c0b 257 for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
tsumagari 29:2da9b8d03c0b 258 // MpuInit();
tsumagari 26:50272431cd1e 259 while(1){
tsumagari 29:2da9b8d03c0b 260 getYPR();
tsumagari 29:2da9b8d03c0b 261 yaw = (ypr[0] - offset_ypr[0]) *180/M_PI;
tsumagari 29:2da9b8d03c0b 262 pitch = (ypr[1] - offset_ypr[1]) *180/M_PI;
tsumagari 29:2da9b8d03c0b 263 roll = (ypr[2] - offset_ypr[2]) *180/M_PI;
tsumagari 30:66fa18093418 264 Thread::wait(0.1);
tsumagari 30:66fa18093418 265 }
tsumagari 30:66fa18093418 266 }
tsumagari 30:66fa18093418 267
tsumagari 30:66fa18093418 268 void sonarCalc(void const *argument){
tsumagari 30:66fa18093418 269 // return sonarDistTime*0.018624 - 13.511;
tsumagari 30:66fa18093418 270 while(1){
tsumagari 30:66fa18093418 271 sonarV = 0;
tsumagari 30:66fa18093418 272 for(int i =0; i<20; i++){
tsumagari 30:66fa18093418 273 sonarV += sonarPin.read();
tsumagari 30:66fa18093418 274 Thread::wait(0.01);
tsumagari 30:66fa18093418 275 // wait(0.01);
tsumagari 30:66fa18093418 276 }
tsumagari 30:66fa18093418 277 sonarDist = (sonarV/20)*2062.5;
tsumagari 29:2da9b8d03c0b 278 Thread::wait(0.01);
tsumagari 30:66fa18093418 279 // wait(0.01);
tsumagari 26:50272431cd1e 280 }
tsumagari 26:50272431cd1e 281 }
tsumagari 26:50272431cd1e 282
tsumagari 26:50272431cd1e 283 void DataReceiveFromSouda(void const *argument){
tsumagari 26:50272431cd1e 284 while(1){
tsumagari 26:50272431cd1e 285 led2 = !led2;
tsumagari 26:50272431cd1e 286 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
tsumagari 26:50272431cd1e 287 if(soudaSerial.readable()) {
tsumagari 26:50272431cd1e 288 // ssMutex.lock();
tsumagari 26:50272431cd1e 289 soudaDatas[i] = (char)soudaSerial.getc();
tsumagari 26:50272431cd1e 290 // ssMutex.unlock();
tsumagari 26:50272431cd1e 291 if(soudaDatas[i]==';') i=-1;
tsumagari 26:50272431cd1e 292 }else i--;
tsumagari 26:50272431cd1e 293 }
taurin 0:085b2c5e3254 294 }
taurin 0:085b2c5e3254 295 }
taurin 0:085b2c5e3254 296
tsumagari 21:8802034b7810 297 void SDprintf(void const *argument){
tsumagari 23:e59afb2044df 298 SdInit();
tsumagari 22:5cbebf097600 299 while(1){
tsumagari 23:e59afb2044df 300 if(write_datas_index == SD_WRITE_NUM-1){
tsumagari 26:50272431cd1e 301 // led4 = !led4;
tsumagari 23:e59afb2044df 302 fp = fopen("/sd/mydir/sdtest.csv", "a");
tsumagari 23:e59afb2044df 303 if(fp == NULL) {
tsumagari 23:e59afb2044df 304 error("Could not open file for write\n");
tsumagari 23:e59afb2044df 305 }
tsumagari 23:e59afb2044df 306 for(int i = 0; i < SD_WRITE_NUM; i++){
tsumagari 23:e59afb2044df 307 for(int j = 0; j < WRITE_DATAS_NUM; j++){
tsumagari 23:e59afb2044df 308 fprintf(fp,"%f,", writeDatas[i][j]);
tsumagari 23:e59afb2044df 309 }
tsumagari 23:e59afb2044df 310 }
tsumagari 23:e59afb2044df 311 fprintf(fp,"\n\r");
tsumagari 23:e59afb2044df 312 fclose(fp);
tsumagari 23:e59afb2044df 313 write_datas_index=0;
tsumagari 21:8802034b7810 314 }
tsumagari 26:50272431cd1e 315 Thread::wait(0.01);
tsumagari 22:5cbebf097600 316 }
taurin 3:8dc516be2e7e 317 }
taurin 3:8dc516be2e7e 318
taurin 3:8dc516be2e7e 319 void WriteDatas(){
taurin 8:31e07f6ed0f7 320 int i;
tsumagari 26:50272431cd1e 321 for(i = 0; i < 6; i++){ //翼端のmpu
tsumagari 26:50272431cd1e 322 if(!(i%2)){
tsumagari 26:50272431cd1e 323 writeDatas[write_datas_index][i] = (int)( (soudaDatas[i*2]<<8) + soudaDatas[(i+1)*2] );
tsumagari 26:50272431cd1e 324 }else{
tsumagari 26:50272431cd1e 325 writeDatas[write_datas_index][i] = (int)( (soudaDatas[i*2-1]<<8) + soudaDatas[i*2+1] );
tsumagari 26:50272431cd1e 326 }
tsumagari 26:50272431cd1e 327 // writeDatas[write_datas_index][i] = i;
taurin 8:31e07f6ed0f7 328 }
tsumagari 26:50272431cd1e 329 for(i = 6; i < 12; i++){//翼端のV,操舵
tsumagari 26:50272431cd1e 330 //writeDatas[write_datas_index][i] = 0.0;
tsumagari 26:50272431cd1e 331 writeDatas[write_datas_index][i] = (float)soudaDatas[i+6];
tsumagari 26:50272431cd1e 332 }
taurin 8:31e07f6ed0f7 333 writeDatas[write_datas_index][i++] = pitch;
taurin 8:31e07f6ed0f7 334 writeDatas[write_datas_index][i++] = roll;
taurin 8:31e07f6ed0f7 335 writeDatas[write_datas_index][i++] = yaw;
taurin 8:31e07f6ed0f7 336 writeDatas[write_datas_index][i++] = airSpeed;
tsumagari 29:2da9b8d03c0b 337 writeDatas[write_datas_index][i++] = sonarDist;
tsumagari 29:2da9b8d03c0b 338 writeDatas[write_datas_index][i++] = cadence.cadence;
taurin 8:31e07f6ed0f7 339 //writeDatas[write_datas_index][i++] = writeTimer.read();
tsumagari 26:50272431cd1e 340 for(i = 0; i < WRITE_DATAS_NUM; i++){
tsumagari 26:50272431cd1e 341 pc.printf("%f ", writeDatas[write_datas_index][i]);
taurin 9:95eb0bbdc2a9 342 // twe.printf("%f,", writeDatas[write_datas_index][i]);
tsumagari 26:50272431cd1e 343 }
tsumagari 26:50272431cd1e 344 pc.printf("\n\r");
taurin 9:95eb0bbdc2a9 345 // twe.printf("\n\r");
tsumagari 23:e59afb2044df 346 if(write_datas_index < SD_WRITE_NUM-1){
taurin 8:31e07f6ed0f7 347 write_datas_index++;
taurin 8:31e07f6ed0f7 348 }
taurin 9:95eb0bbdc2a9 349 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
tsumagari 26:50272431cd1e 350 // pc.printf("%i ",soudaDatas[i]);
tsumagari 26:50272431cd1e 351 ssMutex.lock();
YusukeWakuta 10:1389f938f5f7 352 twe.printf("%i,",soudaDatas[i]);
tsumagari 26:50272431cd1e 353 ssMutex.unlock();
taurin 9:95eb0bbdc2a9 354 }
tsumagari 29:2da9b8d03c0b 355
tsumagari 29:2da9b8d03c0b 356 if(Android.writeable()){
tsumagari 29:2da9b8d03c0b 357 Android.printf("%f,%f,%f",airSpeed,roll,0);
tsumagari 29:2da9b8d03c0b 358 }
tsumagari 29:2da9b8d03c0b 359
tsumagari 26:50272431cd1e 360 ssMutex.lock();
YusukeWakuta 10:1389f938f5f7 361 twe.printf("%f,%f,%f,",pitch,roll,yaw);
tsumagari 26:50272431cd1e 362 // twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
YusukeWakuta 10:1389f938f5f7 363 twe.printf("%f,\r\n",airSpeed);
tsumagari 26:50272431cd1e 364 ssMutex.unlock();
tsumagari 26:50272431cd1e 365 // pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
taurin 9:95eb0bbdc2a9 366 //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
tsumagari 26:50272431cd1e 367 // pc.printf("%f\n\r",airSpeed);
taurin 9:95eb0bbdc2a9 368 //SDprintf();
tsumagari 23:e59afb2044df 369 pc.printf("%d\n\r",write_datas_index);
taurin 9:95eb0bbdc2a9 370 }
taurin 9:95eb0bbdc2a9 371
taurin 9:95eb0bbdc2a9 372 void WriteDatasF(){
taurin 9:95eb0bbdc2a9 373 pc.printf("airSpeed:%f\n\r",airSpeed);
taurin 0:085b2c5e3254 374 }
tsumagari 29:2da9b8d03c0b 375
tsumagari 26:50272431cd1e 376 //float calcKXdeg(float x){
tsumagari 26:50272431cd1e 377 // return -310.54*x+156.65;
tsumagari 26:50272431cd1e 378 //}
taurin 0:085b2c5e3254 379
tsumagari 26:50272431cd1e 380 //float calcAttackAngle(){
tsumagari 26:50272431cd1e 381 // return pitch-calcKXdeg(kx_Z.read());
tsumagari 26:50272431cd1e 382 //}
taurin 0:085b2c5e3254 383
taurin 0:085b2c5e3254 384 void RollAlarm(){
taurin 0:085b2c5e3254 385 if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
taurin 0:085b2c5e3254 386 RollAlarmL = 1;
taurin 0:085b2c5e3254 387 }
taurin 0:085b2c5e3254 388 else{
taurin 0:085b2c5e3254 389 RollAlarmL = 0;
taurin 0:085b2c5e3254 390 }
taurin 0:085b2c5e3254 391 if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
taurin 0:085b2c5e3254 392 RollAlarmR = 1;
taurin 0:085b2c5e3254 393 }
taurin 0:085b2c5e3254 394 else{
taurin 0:085b2c5e3254 395 RollAlarmR = 0;
taurin 0:085b2c5e3254 396 }
taurin 0:085b2c5e3254 397 }
taurin 0:085b2c5e3254 398
tsumagari 26:50272431cd1e 399
taurin 4:a863a092141c 400
tsumagari 29:2da9b8d03c0b 401 int main(){
tsumagari 23:e59afb2044df 402 Thread sd_thread(&SDprintf);//必ずmain内で
tsumagari 26:50272431cd1e 403 Thread sonar_thread(&sonarCalc);
tsumagari 26:50272431cd1e 404 Thread mpu_thread(&mpuProcessing);
tsumagari 26:50272431cd1e 405 Thread soudaSerial_thread(&DataReceiveFromSouda);
tsumagari 26:50272431cd1e 406 Thread cadence_thread(&cadenceDataReceive);
taurin 0:085b2c5e3254 407 init();
taurin 0:085b2c5e3254 408 while(1){
taurin 9:95eb0bbdc2a9 409 pc.printf("test\n\r");
taurin 8:31e07f6ed0f7 410 RollAlarm();
tsumagari 26:50272431cd1e 411 // DataReceiveFromSouda();
tsumagari 15:6966299bea4c 412 // cadence.readData();
taurin 3:8dc516be2e7e 413 WriteDatas();
tsumagari 29:2da9b8d03c0b 414 // mpuProcessing();
taurin 0:085b2c5e3254 415 }
taurin 0:085b2c5e3254 416 }