albatross / Mbed 2 deprecated keiki2017

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

Dependencies:   BufferedSoftSerial2 SDFileSystem-RTOS mbed mbed-rtos INA226_ver1

Fork of keiki2016ver5 by albatross

main.cpp@12:f01c1ba847ac, 2016-11-12 (annotated)

Committer:
tsumagari
Date:
Sat Nov 12 12:03:02 2016 +0000
Revision:
12:f01c1ba847ac
Parent:
11:90e2b070f337
Child:
13:09e05e7cfca1
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taurin 1:5ec2409854da 1 //計器プログラム
taurin 0:085b2c5e3254 2 #include "mbed.h"
taurin 0:085b2c5e3254 3 #include "Fusokukei.h"
taurin 0:085b2c5e3254 4 #include "MPU6050.h"
taurin 0:085b2c5e3254 5 #include "SDFileSystem.h"
tsumagari 12:f01c1ba847ac 6 #include "SoftwareSerial.h"
taurin 0:085b2c5e3254 7
taurin 0:085b2c5e3254 8 #define KX_VALUE_MIN 0.4
taurin 0:085b2c5e3254 9 #define KX_VALUE_MAX 0.8
taurin 1:5ec2409854da 10 #define SOUDA_DATAS_NUM 13
taurin 5:1b3c8e5ee99e 11 #define WRITE_DATAS_NUM 20
taurin 0:085b2c5e3254 12 #define MPU_LOOP_TIME 0.01
taurin 0:085b2c5e3254 13 #define AIR_LOOP_TIME 0.01
taurin 3:8dc516be2e7e 14 #define WRITE_DATAS_LOOP_TIME 1
taurin 3:8dc516be2e7e 15 #define ROLL_R_MAX_DEG 2
taurin 3:8dc516be2e7e 16 #define ROLL_L_MAX_DEG 2
taurin 9:95eb0bbdc2a9 17 #define MPU_DELT_MIN 250
taurin 3:8dc516be2e7e 18 #define SD_WRITE_NUM 10
taurin 4:a863a092141c 19 #define INIT_SERVO_PERIOD_MS 20
taurin 0:085b2c5e3254 20
taurin 0:085b2c5e3254 21 Serial pc(USBTX,USBRX);
taurin 1:5ec2409854da 22 Serial twe(p9,p10);
tsumagari 12:f01c1ba847ac 23 Serial cadenceTwe(p13,p14);
tsumagari 12:f01c1ba847ac 24 SoftwareSerial soudaSerial(p24,p23);
tsumagari 12:f01c1ba847ac 25 //Ticker writeDatasTicker;
taurin 3:8dc516be2e7e 26 Timer writeTimer;
taurin 0:085b2c5e3254 27
taurin 0:085b2c5e3254 28 InterruptIn FusokukeiPin(p30);
taurin 0:085b2c5e3254 29 Ticker FusokukeiTicker;
taurin 0:085b2c5e3254 30 Fusokukei air;
taurin 0:085b2c5e3254 31 volatile int air_kaitensu= 0;
taurin 0:085b2c5e3254 32
taurin 0:085b2c5e3254 33 float sum = 0;
taurin 0:085b2c5e3254 34 uint32_t sumCount = 0;
taurin 0:085b2c5e3254 35 MPU6050 mpu6050;
taurin 0:085b2c5e3254 36 Timer t;
taurin 0:085b2c5e3254 37 Ticker mpu6050Ticker;
taurin 0:085b2c5e3254 38
taurin 0:085b2c5e3254 39 AnalogIn kx_X(p17);
taurin 0:085b2c5e3254 40 AnalogIn kx_Y(p16);
taurin 0:085b2c5e3254 41 AnalogIn kx_Z(p15);
taurin 3:8dc516be2e7e 42 float KX_X,KX_Y,KX_Z;
taurin 0:085b2c5e3254 43
taurin 0:085b2c5e3254 44 DigitalOut RollAlarmR(p20);
taurin 0:085b2c5e3254 45 DigitalOut RollAlarmL(p19);
taurin 0:085b2c5e3254 46 DigitalOut led(LED1);
taurin 1:5ec2409854da 47 DigitalOut led2(LED2);
taurin 0:085b2c5e3254 48
taurin 0:085b2c5e3254 49 SDFileSystem sd(p5, p6, p7, p8, "sd");
taurin 0:085b2c5e3254 50 FILE* fp;
taurin 0:085b2c5e3254 51
YusukeWakuta 11:90e2b070f337 52 PwmOut kisokuServo(p26);
YusukeWakuta 11:90e2b070f337 53 PwmOut geikakuServo(p22);
taurin 4:a863a092141c 54
tsumagari 12:f01c1ba847ac 55 float cadence;
taurin 0:085b2c5e3254 56 char soudaDatas[SOUDA_DATAS_NUM];
taurin 3:8dc516be2e7e 57 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
taurin 3:8dc516be2e7e 58 volatile int write_datas_index = 0;
taurin 0:085b2c5e3254 59
taurin 0:085b2c5e3254 60 void air_countUp();
taurin 0:085b2c5e3254 61 void call_calcAirSpeed();
taurin 0:085b2c5e3254 62 void init();
taurin 0:085b2c5e3254 63 void FusokukeiInit();
taurin 0:085b2c5e3254 64 void MpuInit();
taurin 0:085b2c5e3254 65 void mpuProcessing();
tsumagari 12:f01c1ba847ac 66 void cadenceRead();
tsumagari 12:f01c1ba847ac 67 void cadenceInit();
taurin 0:085b2c5e3254 68 void SdInit();
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
taurin 0:085b2c5e3254 74 void air_countUp(){
taurin 0:085b2c5e3254 75 air_kaitensu++;
taurin 0:085b2c5e3254 76 }
taurin 0:085b2c5e3254 77
taurin 0:085b2c5e3254 78 void call_calcAirSpeed(){
taurin 0:085b2c5e3254 79 air.calcAirSpeed(air_kaitensu);
taurin 0:085b2c5e3254 80 air_kaitensu = 0;
taurin 0:085b2c5e3254 81 }
taurin 0:085b2c5e3254 82
taurin 0:085b2c5e3254 83 void init(){
taurin 1:5ec2409854da 84 twe.baud(115200);
tsumagari 12:f01c1ba847ac 85 // test.baud(300);
taurin 6:98a63127aa11 86 //writeTimer.start();
taurin 5:1b3c8e5ee99e 87 kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 5:1b3c8e5ee99e 88 geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
taurin 0:085b2c5e3254 89 FusokukeiInit();
taurin 8:31e07f6ed0f7 90 MpuInit();
taurin 8:31e07f6ed0f7 91 SdInit();
taurin 3:8dc516be2e7e 92 //writeDatasTicker.attach(&WriteDatas,1);
taurin 1:5ec2409854da 93 //soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq);
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 0:085b2c5e3254 101 void MpuInit(){
taurin 0:085b2c5e3254 102 i2c.frequency(400000); // use fast (400 kHz) I2C
taurin 0:085b2c5e3254 103 t.start();
taurin 0:085b2c5e3254 104 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
taurin 0:085b2c5e3254 105 if (whoami == 0x68) { // WHO_AM_I should always be 0x68
taurin 0:085b2c5e3254 106 wait(1);
taurin 0:085b2c5e3254 107 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
taurin 0:085b2c5e3254 108 wait(1);
taurin 0:085b2c5e3254 109 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) {
taurin 0:085b2c5e3254 110 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
taurin 0:085b2c5e3254 111 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
taurin 0:085b2c5e3254 112 mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
taurin 0:085b2c5e3254 113 wait(2);
taurin 0:085b2c5e3254 114 } else {
taurin 0:085b2c5e3254 115 }
taurin 0:085b2c5e3254 116 } else {
taurin 5:1b3c8e5ee99e 117 //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
taurin 0:085b2c5e3254 118 }
taurin 0:085b2c5e3254 119 }
taurin 0:085b2c5e3254 120
taurin 4:a863a092141c 121 double calcPulse(int deg){
taurin 4:a863a092141c 122 return (0.0006+(deg/180.0)*(0.00235-0.00045));
taurin 4:a863a092141c 123
taurin 4:a863a092141c 124 }
taurin 4:a863a092141c 125
taurin 0:085b2c5e3254 126 void mpuProcessing(){
taurin 0:085b2c5e3254 127 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
taurin 3:8dc516be2e7e 128 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
taurin 3:8dc516be2e7e 129 mpu6050.getAres();
taurin 3:8dc516be2e7e 130 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
taurin 3:8dc516be2e7e 131 ay = (float)accelCount[1]*aRes - accelBias[1];
taurin 3:8dc516be2e7e 132 az = (float)accelCount[2]*aRes - accelBias[2];
taurin 3:8dc516be2e7e 133 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
taurin 3:8dc516be2e7e 134 mpu6050.getGres();
taurin 3:8dc516be2e7e 135 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
taurin 3:8dc516be2e7e 136 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
taurin 3:8dc516be2e7e 137 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
taurin 3:8dc516be2e7e 138 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
taurin 3:8dc516be2e7e 139 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
taurin 3:8dc516be2e7e 140 }
taurin 3:8dc516be2e7e 141 Now = t.read_us();
taurin 3:8dc516be2e7e 142 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
taurin 3:8dc516be2e7e 143 lastUpdate = Now;
taurin 3:8dc516be2e7e 144 sum += deltat;
taurin 3:8dc516be2e7e 145 sumCount++;
taurin 3:8dc516be2e7e 146 if(lastUpdate - firstUpdate > 10000000.0f) {
taurin 3:8dc516be2e7e 147 beta = 0.04; // decrease filter gain after stabilized
taurin 3:8dc516be2e7e 148 zeta = 0.015; // increasey bias drift gain after stabilized
taurin 3:8dc516be2e7e 149 }
taurin 3:8dc516be2e7e 150 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
taurin 3:8dc516be2e7e 151 delt_t = t.read_ms() - count;
taurin 3:8dc516be2e7e 152 if (delt_t > MPU_DELT_MIN) {
taurin 3:8dc516be2e7e 153 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]);
taurin 3:8dc516be2e7e 154 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
taurin 3:8dc516be2e7e 155 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]);
taurin 3:8dc516be2e7e 156 pitch *= 180.0f / PI;
taurin 3:8dc516be2e7e 157 yaw *= 180.0f / PI;
taurin 3:8dc516be2e7e 158 roll *= 180.0f / PI;
taurin 3:8dc516be2e7e 159 myled= !myled;
taurin 3:8dc516be2e7e 160 count = t.read_ms();
taurin 3:8dc516be2e7e 161 sum = 0;
taurin 3:8dc516be2e7e 162 sumCount = 0;
taurin 0:085b2c5e3254 163 }
taurin 0:085b2c5e3254 164 }
taurin 0:085b2c5e3254 165
tsumagari 12:f01c1ba847ac 166 void cadenceInit(){
tsumagari 12:f01c1ba847ac 167 cadenceTwe.baud(115200);
tsumagari 12:f01c1ba847ac 168
tsumagari 12:f01c1ba847ac 169 }
tsumagari 12:f01c1ba847ac 170
tsumagari 12:f01c1ba847ac 171 void cadenceRead(){
tsumagari 12:f01c1ba847ac 172 if(twe.readable()){
tsumagari 12:f01c1ba847ac 173 data[data_count]=twe.getc();
tsumagari 12:f01c1ba847ac 174
tsumagari 12:f01c1ba847ac 175 if(data[data_count]=='\r'){
tsumagari 12:f01c1ba847ac 176 for(int i=0;i<73;i++) data[i]= NULL;
tsumagari 12:f01c1ba847ac 177 data_num=0;
tsumagari 12:f01c1ba847ac 178 data_count=0;
tsumagari 12:f01c1ba847ac 179 }else{
tsumagari 12:f01c1ba847ac 180 if(data[data_count]==';'){
tsumagari 12:f01c1ba847ac 181 data_num++;
tsumagari 12:f01c1ba847ac 182 }
tsumagari 12:f01c1ba847ac 183 if(data_num==15){
tsumagari 12:f01c1ba847ac 184 z_int = (int)data[data_count-1] + (int)data[data_count-2]*10 + (int)data[data_count-3]*100 - offset*111;
tsumagari 12:f01c1ba847ac 185 if(data[data_count-4]!='0') z_int *= -1;
tsumagari 12:f01c1ba847ac 186 cadence = (double)z_int/6; //角度補正前ふつうに
tsumagari 12:f01c1ba847ac 187 }
tsumagari 12:f01c1ba847ac 188 data_count++;
tsumagari 12:f01c1ba847ac 189 }//else
tsumagari 12:f01c1ba847ac 190
tsumagari 12:f01c1ba847ac 191 } //readable
tsumagari 12:f01c1ba847ac 192 }
tsumagari 12:f01c1ba847ac 193
taurin 0:085b2c5e3254 194 void SdInit(){
taurin 0:085b2c5e3254 195 mkdir("/sd/mydir", 0777);
taurin 0:085b2c5e3254 196 fp = fopen("/sd/mydir/sdtest2.csv", "w");
taurin 0:085b2c5e3254 197 if(fp == NULL) {
taurin 0:085b2c5e3254 198 error("Could not open file for write\n");
taurin 0:085b2c5e3254 199 }
taurin 0:085b2c5e3254 200 fprintf(fp, "Hello fun SD Card World!\n\r");
taurin 0:085b2c5e3254 201 fclose(fp);
taurin 0:085b2c5e3254 202 }
taurin 0:085b2c5e3254 203
taurin 0:085b2c5e3254 204 void DataReceiveFromSouda(){
taurin 1:5ec2409854da 205 led2 = !led2;
taurin 5:1b3c8e5ee99e 206 //pc.printf("received\n\r");
taurin 1:5ec2409854da 207 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
tsumagari 12:f01c1ba847ac 208 if(soudaSerial.readable()) soudaDatas[i] = (char)soudaSerial.getc();
taurin 0:085b2c5e3254 209 }
taurin 0:085b2c5e3254 210 }
taurin 0:085b2c5e3254 211
taurin 3:8dc516be2e7e 212 void SDprintf(){
taurin 1:5ec2409854da 213 fp = fopen("/sd/mydir/sdtest.csv", "a");
taurin 1:5ec2409854da 214 if(fp == NULL) {
taurin 1:5ec2409854da 215 error("Could not open file for write\n");
taurin 1:5ec2409854da 216 }
taurin 3:8dc516be2e7e 217 for(int i = 0; i < SD_WRITE_NUM; i++){
taurin 3:8dc516be2e7e 218 for(int j = 0; j < WRITE_DATAS_NUM; j++){
taurin 5:1b3c8e5ee99e 219 fprintf(fp,"%f,", writeDatas[i][j]);
taurin 3:8dc516be2e7e 220 }
taurin 3:8dc516be2e7e 221 }
taurin 5:1b3c8e5ee99e 222 fprintf(fp,"\n\r");
taurin 3:8dc516be2e7e 223 fclose(fp);
taurin 3:8dc516be2e7e 224 }
taurin 3:8dc516be2e7e 225
taurin 3:8dc516be2e7e 226 void WriteDatas(){
taurin 8:31e07f6ed0f7 227 int i;
taurin 8:31e07f6ed0f7 228 for(i = 0; i < SOUDA_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 229 //writeDatas[write_datas_index][i] = 0.0;
taurin 8:31e07f6ed0f7 230 writeDatas[write_datas_index][i] = (float)soudaDatas[i];
taurin 8:31e07f6ed0f7 231 }
taurin 8:31e07f6ed0f7 232 writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
taurin 8:31e07f6ed0f7 233 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
taurin 8:31e07f6ed0f7 234 writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
taurin 8:31e07f6ed0f7 235 writeDatas[write_datas_index][i++] = pitch;
taurin 8:31e07f6ed0f7 236 writeDatas[write_datas_index][i++] = roll;
taurin 8:31e07f6ed0f7 237 writeDatas[write_datas_index][i++] = yaw;
taurin 8:31e07f6ed0f7 238 writeDatas[write_datas_index][i++] = airSpeed;
taurin 8:31e07f6ed0f7 239 //writeDatas[write_datas_index][i++] = writeTimer.read();
taurin 9:95eb0bbdc2a9 240 //for(i = 0; i < WRITE_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 241 // pc.printf("%f ", writeDatas[write_datas_index][i]);
taurin 9:95eb0bbdc2a9 242 // twe.printf("%f,", writeDatas[write_datas_index][i]);
taurin 9:95eb0bbdc2a9 243 // }
taurin 9:95eb0bbdc2a9 244 // pc.printf("\n\r");
taurin 9:95eb0bbdc2a9 245 // twe.printf("\n\r");
taurin 8:31e07f6ed0f7 246 if(write_datas_index == SD_WRITE_NUM-1){
taurin 8:31e07f6ed0f7 247 SDprintf();
taurin 8:31e07f6ed0f7 248 write_datas_index=0;
taurin 8:31e07f6ed0f7 249 }
taurin 8:31e07f6ed0f7 250 else{
taurin 8:31e07f6ed0f7 251 write_datas_index++;
taurin 8:31e07f6ed0f7 252 }
taurin 9:95eb0bbdc2a9 253 for(int i = 0; i < SOUDA_DATAS_NUM; i++){
taurin 9:95eb0bbdc2a9 254 pc.printf("%i ",soudaDatas[i]);
YusukeWakuta 10:1389f938f5f7 255 twe.printf("%i,",soudaDatas[i]);
taurin 9:95eb0bbdc2a9 256 }
taurin 9:95eb0bbdc2a9 257 //pc.printf("\n\r");
YusukeWakuta 10:1389f938f5f7 258 twe.printf("%f,%f,%f,",pitch,roll,yaw);
YusukeWakuta 10:1389f938f5f7 259 twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
YusukeWakuta 10:1389f938f5f7 260 twe.printf("%f,\r\n",airSpeed);
taurin 9:95eb0bbdc2a9 261 pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
taurin 9:95eb0bbdc2a9 262 //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
taurin 9:95eb0bbdc2a9 263 pc.printf("%f\n\r",airSpeed);
taurin 9:95eb0bbdc2a9 264 //SDprintf();
taurin 9:95eb0bbdc2a9 265 }
taurin 9:95eb0bbdc2a9 266
taurin 9:95eb0bbdc2a9 267 void WriteDatasF(){
taurin 9:95eb0bbdc2a9 268 pc.printf("airSpeed:%f\n\r",airSpeed);
taurin 0:085b2c5e3254 269 }
taurin 0:085b2c5e3254 270
taurin 0:085b2c5e3254 271 float calcKXdeg(float x){
taurin 0:085b2c5e3254 272 return -310.54*x+156.65;
taurin 0:085b2c5e3254 273 }
taurin 0:085b2c5e3254 274
taurin 0:085b2c5e3254 275 float calcAttackAngle(){
taurin 0:085b2c5e3254 276 return pitch-calcKXdeg(kx_Z.read());
taurin 0:085b2c5e3254 277 }
taurin 0:085b2c5e3254 278
taurin 0:085b2c5e3254 279 void RollAlarm(){
taurin 0:085b2c5e3254 280 if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
taurin 0:085b2c5e3254 281 RollAlarmL = 1;
taurin 0:085b2c5e3254 282 }
taurin 0:085b2c5e3254 283 else{
taurin 0:085b2c5e3254 284 RollAlarmL = 0;
taurin 0:085b2c5e3254 285 }
taurin 0:085b2c5e3254 286
taurin 0:085b2c5e3254 287 if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
taurin 0:085b2c5e3254 288 RollAlarmR = 1;
taurin 0:085b2c5e3254 289 }
taurin 0:085b2c5e3254 290 else{
taurin 0:085b2c5e3254 291 RollAlarmR = 0;
taurin 0:085b2c5e3254 292 }
taurin 0:085b2c5e3254 293 }
taurin 0:085b2c5e3254 294
taurin 4:a863a092141c 295 void WriteServo(){
taurin 9:95eb0bbdc2a9 296 //kisokuServo.pulsewidth(calcPulse(airSpeed*10));
taurin 9:95eb0bbdc2a9 297 kisokuServo.pulsewidth(calcPulse(9*airSpeed));
taurin 9:95eb0bbdc2a9 298 if(pitch<0){
taurin 9:95eb0bbdc2a9 299 geikakuServo.pulsewidth(calcPulse(0));
taurin 9:95eb0bbdc2a9 300 }
taurin 9:95eb0bbdc2a9 301 else{
taurin 9:95eb0bbdc2a9 302 geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
taurin 9:95eb0bbdc2a9 303 }
taurin 6:98a63127aa11 304 //pc.printf("a:%f",airSpeed);
taurin 6:98a63127aa11 305 //pc.printf("p:%f\r\n",pitch);
taurin 5:1b3c8e5ee99e 306 //kisokuServo.pulsewidth(calcPulse(0));
taurin 5:1b3c8e5ee99e 307 //geikakuServo.pulsewidth(calcPulse(0));
taurin 4:a863a092141c 308 }
taurin 4:a863a092141c 309
taurin 0:085b2c5e3254 310 int main(){
taurin 0:085b2c5e3254 311 init();
taurin 0:085b2c5e3254 312 while(1){
tsumagari 12:f01c1ba847ac 313 // pc.printf("test\n\r");
tsumagari 12:f01c1ba847ac 314 // if(test.readable()) pc.printf("%d ",test.getc());
tsumagari 12:f01c1ba847ac 315 // pc.printf("<-softserial\n\r");
taurin 8:31e07f6ed0f7 316 mpuProcessing();
taurin 8:31e07f6ed0f7 317 RollAlarm();
taurin 8:31e07f6ed0f7 318 DataReceiveFromSouda();
taurin 3:8dc516be2e7e 319 WriteDatas();
taurin 8:31e07f6ed0f7 320 WriteServo();
taurin 0:085b2c5e3254 321 }
taurin 0:085b2c5e3254 322 }