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