albatross / Mbed 2 deprecated keiki2017

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

Dependencies:   BufferedSoftSerial2 SDFileSystem-RTOS mbed mbed-rtos INA226_ver1

Fork of keiki2016ver5 by albatross

main.cpp@16:22aae833bdae, 2016-11-30 (annotated)

Committer:
tsumagari
Date:
Wed Nov 30 08:08:14 2016 +0000
Branch:
fusokukei
Revision:
16:22aae833bdae
Parent:
15:6966299bea4c
Child:
20:7056239a71dd
?????twe-lite?????????BufferedSoftSerial???????Cadence????BufferedSoftSerial????????????p11(=tx),p12(=rx)?????????

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