albatross / Mbed 2 deprecated keiki2017

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

Dependencies:   BufferedSoftSerial2 SDFileSystem-RTOS mbed mbed-rtos INA226_ver1

Fork of keiki2016ver5 by albatross

main.cpp@15:6966299bea4c, 2016-11-26 (annotated)

Committer:
tsumagari
Date:
Sat Nov 26 01:12:10 2016 +0000
Branch:
fusokukei
Revision:
15:6966299bea4c
Parent:
13:09e05e7cfca1
Child:
16:22aae833bdae
Child:
18:fa3f9ba17af8
?????????????????????????????????????????13?????????;?????????soudaDatas???????

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