5/13 計器プログラム
Dependencies: SDFileSystem mbed
Fork of keiki2016ver3 by
Diff: main.cpp
- Revision:
- 4:a863a092141c
- Parent:
- 3:8dc516be2e7e
- Child:
- 5:1b3c8e5ee99e
--- a/main.cpp Thu Mar 24 06:42:22 2016 +0000 +++ b/main.cpp Thu Mar 24 09:50:51 2016 +0000 @@ -15,6 +15,7 @@ #define ROLL_L_MAX_DEG 2 #define MPU_DELT_MIN 500 #define SD_WRITE_NUM 10 +#define INIT_SERVO_PERIOD_MS 20 Serial pc(USBTX,USBRX); Serial soudaSerial(p13,p14); @@ -46,6 +47,9 @@ SDFileSystem sd(p5, p6, p7, p8, "sd"); FILE* fp; +PwmOut drugServo(p22); +PwmOut eruronServo(p23); + char soudaDatas[SOUDA_DATAS_NUM]; float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM]; volatile int write_datas_index = 0; @@ -74,6 +78,8 @@ void init(){ twe.baud(115200); writeTimer.start(); + drugServo.period_ms(INIT_SERVO_PERIOD_MS); + eruronServo.period_ms(INIT_SERVO_PERIOD_MS); FusokukeiInit(); MpuInit(); SdInit(); @@ -106,6 +112,11 @@ } } +double calcPulse(int deg){ + return (0.0006+(deg/180.0)*(0.00235-0.00045)); + +} + void mpuProcessing(){ if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt mpu6050.readAccelData(accelCount); // Read the x/y/z adc values @@ -189,6 +200,7 @@ void WriteDatas(){ int i; + pc.printf("testw\n\r"); for(i = 0; i < SOUDA_DATAS_NUM; i++){ writeDatas[write_datas_index][i] = (float)soudaDatas[i]; } @@ -208,7 +220,7 @@ twe.printf("\n\r"); } } - if(write_datas_index == SD_WRITE_NUM){ + if(write_datas_index == SD_WRITE_NUM-1){ SDprintf(); write_datas_index=0; } @@ -254,6 +266,11 @@ } } +void WriteServo(){ + eruronServo.pulsewidth(calcPulse(airSpeed*10)); + drugServo.pulsewidth(calcPulse()); +} + int main(){ init(); while(1){ @@ -261,5 +278,6 @@ RollAlarm(); DataReceiveFromSouda(); WriteDatas(); + WriteServo(); } } \ No newline at end of file