Diff: main.cpp

Revision:

32:c4f06cb0e1d6

Parent:

31:8d02f3b9a067

Child:

33:e79457192a4b

--- a/main.cpp	Tue Feb 16 06:22:16 2021 +0000
+++ b/main.cpp	Mon Mar 01 09:08:09 2021 +0000
@@ -24,7 +24,9 @@
 DigitalOut led3(LED3);
 
 PwmOut servoRight(PE_9);
-const double PID_dt = 0.005;
+PwmOut servoLeft(PE_11);
+PwmOut servoThrust(PA_0);
+const double PID_dt = 1.0/200;
 PID pitchPID(3.0, 0,0,PID_dt); //rad
 PID pitchratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
 PID rollPID(3.0,0.0,0.0,PID_dt);
@@ -49,7 +51,7 @@
 int servoPwmMax = 1800;
 int servoPwmMin = 1200;
 int motorPwmMax = 2000;
-int motorPwmMin = 1000;
+int motorPwmMin = 950;
 int offsetVal[6] = {0,0,0,0,0,0};
 
 const double pitch_align = 0.0;
@@ -63,7 +65,7 @@
 
 void writeSdcard()
 {
-    sd.printf("pitch: %lf, yaw; %lf, roll: % lf\n",pitch,yaw,roll);
+    pc.printf("motor: %d, servo1: %d, servo2: %d \r\n",motorOut[0],servoOut[0],servoOut[1]);
 }
 
 // 割り込まれた時点での出力(computeの結果）を返す関数
@@ -108,9 +110,11 @@
             motorOut[i] = motorPwmMax;
         };
     }
+    
     servoRight.pulsewidth_us(servoOut[0]);
-    //servoLeft.pulsewidth_us(servoOut[1]); 
-        
+    servoLeft.pulsewidth_us(servoOut[1]); 
+    servoThrust.pulsewidth_us(motorOut[0]);
+     
     if(loop_count > int(1/PID_dt/10.0)){
         writeSdcard();
         loop_count = 0;
@@ -138,8 +142,8 @@
 {
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
-    pc.baud(115200);
-    sd.baud(115200);
+    //pc.baud(115200);
+    //sd.baud(115200);
     accelgyro.initialize();
     //加速度計のフルスケールレンジを設定
     accelgyro.setFullScaleAccelRange(ACCEL_FSR);
@@ -166,6 +170,8 @@
     rollratePID.setInputLimits(-pi,pi);
     
     servoRight.period_us(20000);
+    servoLeft.period_us(20000);
+    servoThrust.period_us(20000);
     MadgwickFilter.begin(100); //サンプリング周波数Hza
     t.start();
     while(1) {