Diff: main.cpp

Revision:

35:4535af88f7bf

Parent:

33:e79457192a4b

Child:

36:e68ce293306e

--- a/main.cpp	Tue Mar 02 04:29:30 2021 +0000
+++ b/main.cpp	Tue Mar 02 05:01:20 2021 +0000
@@ -5,7 +5,7 @@
 #include "LoopTicker.hpp"
 #include "MPU6050.h"
 #include <I2Cdev.h>
-#include "Matrix.h"
+
 
 #define MPU6050_PWR_MGMT_1   0x6B
 #define MPU_ADDRESS  0x68
@@ -32,10 +32,6 @@
 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
-Quaternion qhat(1,0,0,0);
-Matrix Phat(4,4);
-Matrix Qgyro(3,3);
-Matrix Racc(3,3);
 Timer t;
 
 int loop_count = 0;
@@ -51,26 +47,26 @@
 int out1, out2;
 float scaledServoOut[3]= {0,0,0};
 float scaledMotorOut[1]= {-1};
-int servoOut[3] = {1500,1500,1500};
-int motorOut[1] = {1000};
-int servoPwmMax = 1800;
-int servoPwmMin = 1200;
-int motorPwmMax = 2000;
-int motorPwmMin = 950;
+float servoOut[3] = {0.0015,0.0015,0.0015};
+float motorOut[1] = {0.00095};
+float servoPwmMax = 0.002;
+float servoPwmMin = 0.0012;
+float motorPwmMax = 0.002;
+float motorPwmMin = 0.00095;
 int offsetVal[6] = {0,0,0,0,0,0};
 
 const double pitch_align = 0.0;
 const double roll_align = -0.0;
 
 
-long map(long x, long in_min, long in_max, long out_min, long out_max)
+float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
 {
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
 void writeSdcard()
 {
-    pc.printf("motor: %d, servo1: %d, servo2: %d \r\n",motorOut[0],servoOut[0],servoOut[1]);
+    pc.printf("motor: %f, servo1: %f, servo2: %f \r\n",motorOut[0],servoOut[0],servoOut[1]);
 }
 
 // 割り込まれた時点での出力(computeの結果）を返す関数
@@ -78,7 +74,7 @@
     if(sbus.failSafe == false) {
         // sbusデータの読み込み
         for (int i =0 ; i < 16;i ++){
-            rc[i] = 0.65f * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - 0.65f) * rc[i]; // mapped input
+            rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
         }
     }
     
@@ -97,7 +93,7 @@
     scaledMotorOut[0]= dT;
     
     for(int i = 0;i<4;i++){
-        servoOut[i] = int(map(scaledServoOut[i]*1000.0,-1000,1000,servoPwmMin,servoPwmMax));
+        servoOut[i] = float(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax));
         if(servoOut[i]<servoPwmMin) {
             servoOut[i] = servoPwmMin;
         };
@@ -107,7 +103,7 @@
     }
     
     for(int i = 0;i<1;i++){
-        motorOut[i] = int(map(scaledMotorOut[i]*1000.0,-1000,1000,motorPwmMin,motorPwmMax));
+        motorOut[i] = float(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax));
         if(motorOut[i]<motorPwmMin) {
             motorOut[i] = motorPwmMin;
         };
@@ -116,9 +112,9 @@
         };
     }
     
-    servoRight.pulsewidth_us(servoOut[0]);
-    servoLeft.pulsewidth_us(servoOut[1]); 
-    servoThrust.pulsewidth_us(motorOut[0]);
+    servoRight.pulsewidth(servoOut[0]);
+    servoLeft.pulsewidth(servoOut[1]); 
+    servoThrust.pulsewidth(motorOut[0]);
      
     if(loop_count > int(1/PID_dt/10.0)){
         writeSdcard();
@@ -178,9 +174,9 @@
     rollPID.setInputLimits(-pi,pi); 
     rollratePID.setInputLimits(-pi,pi);
     
-    servoRight.period_us(20000);
-    servoLeft.period_us(20000);
-    servoThrust.period_us(20000);
+    servoRight.period(0.02f);
+    servoLeft.period(0.02f);
+    servoThrust.period(0.02f);
     MadgwickFilter.begin(100); //サンプリング周波数Hza
     t.start();
     while(1) {