Diff: main.cpp

Revision:

31:8d02f3b9a067

Parent:

30:214ddc613a35

Child:

32:c4f06cb0e1d6

--- a/main.cpp	Tue Feb 16 03:29:55 2021 +0000
+++ b/main.cpp	Tue Feb 16 06:22:16 2021 +0000
@@ -24,11 +24,11 @@
 DigitalOut led3(LED3);
 
 PwmOut servoRight(PE_9);
-const double PID_dt = 0.01;
-PID pitchPID(1.0, 0,0,PID_dt); //rad
-PID pitchratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
-PID rollPID(1.0,0.0,0.0,PID_dt);
-PID rollratePID(0.1, 0.0, 0.0, PID_dt);//rad/s
+const double PID_dt = 0.005;
+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);
+PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s
 Timer t;
 
 int loop_count = 0;
@@ -54,7 +54,6 @@
 
 const double pitch_align = 0.0;
 const double roll_align = -0.0;
-float tstart;
 
 
 long map(long x, long in_min, long in_max, long out_min, long out_max)
@@ -89,8 +88,9 @@
     scaledServoOut[0]=de+da;
     scaledServoOut[1]=de-da;
     scaledMotorOut[0]= dT;
-    for(int i = 0;i<sizeof(servoOut);i++){
-        servoOut[i] = int(map(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax));
+    
+    for(int i = 0;i<4;i++){
+        servoOut[i] = int(map(scaledServoOut[i]*1000.0,-1000,1000,servoPwmMin,servoPwmMax));
         if(servoOut[i]<servoPwmMin) {
             servoOut[i] = servoPwmMin;
         };
@@ -99,8 +99,8 @@
         };
     }
     
-    for(int i = 0;i<sizeof(motorOut);i++){
-        motorOut[i] = int(map(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax));
+    for(int i = 0;i<1;i++){
+        motorOut[i] = int(map(scaledMotorOut[i]*1000.0,-1000,1000,motorPwmMin,motorPwmMax));
         if(motorOut[i]<motorPwmMin) {
             motorOut[i] = motorPwmMin;
         };
@@ -108,12 +108,11 @@
             motorOut[i] = motorPwmMax;
         };
     }
-    
     servoRight.pulsewidth_us(servoOut[0]);
     //servoLeft.pulsewidth_us(servoOut[1]); 
         
-    if(loop_count > 10){
-        writeSdcard()
+    if(loop_count > int(1/PID_dt/10.0)){
+        writeSdcard();
         loop_count = 0;
     }else{
         loop_count +=1;
@@ -137,7 +136,6 @@
 
 int main()
 {
-    LoopTicker sdcard_loop;
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     pc.baud(115200);
@@ -171,13 +169,12 @@
     MadgwickFilter.begin(100); //サンプリング周波数Hza
     t.start();
     while(1) {
-        tstart = t.read();
+        float tstart = t.read();
         //姿勢角を更新
         updateAttitude();
         PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する
         
         float tend = t.read();
-        pc.printf("time%f \r\n", (tend-tstart));
         MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hz
         
     }