Diff: main.cpp

Revision:

48:1065506191f2

Parent:

45:b439a135c24b

Child:

49:73fe59148dd4

Child:

50:e07ffa9f565a

Child:

51:be2cd03fca3b

--- a/main.cpp	Wed May 19 05:10:41 2021 +0000
+++ b/main.cpp	Wed May 19 05:39:14 2021 +0000
@@ -31,15 +31,10 @@
 MAG3110 mag(PB_9,PB_8);
 SBUS sbus(PD_5, PD_6);
 Serial pc(USBTX, USBRX);
-Serial sd(PG_14,PG_9);
 DigitalIn userButton(USER_BUTTON);
-FastPWM servoRight(PE_9);
-FastPWM servoLeft(PE_11);
-FastPWM servoThrust(PE_13);
+FastPWM servo(PE_9);
 PID pitchPID(5.0, 0,0,PID_dt); //rad
 PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
-PID rollPID(5.0,0.0,0.0,PID_dt);
-PID rollratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
 Timer t;
 
 Matrix qhat(4,1);
@@ -79,10 +74,8 @@
 float LPmag_z = 0.0;;
 
 int out1, out2;
-float scaledServoOut[3]= {0,0};
-float scaledMotorOut[1]= {-1};
-float servoOut[3] = {1500.0,1500.0};
-float motorOut[1] = {1100.0};
+float scaledServoOut[1]= {0};
+float servoOut[1] = {1500.0};
  
 float accnorm;
 float magnorm;
@@ -107,7 +100,7 @@
     //magcal.getExtremes(&magmin[0],&magmax[0]);
     //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]);
     //pc.printf("%f %f %f %f %f %f %f %f %f %f %f %f \r\n",acc_x,acc_y,acc_z,mag_x,mag_y,mag_z,accref[0],accref[1],accref[2],magref[0],magref[1],magref[2]);
-    sd.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
+    pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
     //pc.printf("%f %f %f %f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
     //pc.printf("%f %f %f %f : %f %f %f\r\n",float(obs_count)/100.0,th_mg,dynaccnorm,accnormerr,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
     //pc.printf("%d \r\n",userButton.read());
@@ -471,18 +464,11 @@
     
     pitchPID.setProcessValue(pitch);
     pitchratePID.setProcessValue(gyro_y);
-    rollPID.setProcessValue(roll);
-    rollratePID.setProcessValue(gyro_x);
     float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
-    float da = rollPID.compute()+rollratePID.compute()+rc[0];
-    float dT = rc[2];
     
-    scaledServoOut[0]=de+da;
-    scaledServoOut[1]=-de+da;
-    scaledMotorOut[0]= dT;
+    scaledServoOut[0]=de;
     
     float LP_servo = 0.2;
-    float LP_motor = 0.2;
     for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
         servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
         if(servoOut[i]<servoPwmMin) {
@@ -493,18 +479,7 @@
         };
     }
     
-    for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
-        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
-        if(motorOut[i]<motorPwmMin) {
-            motorOut[i] = motorPwmMin;
-        };
-        if(motorOut[i]>motorPwmMax) {
-            motorOut[i] = motorPwmMax;
-        };
-    }
-    servoRight.pulsewidth_us(servoOut[0]);
-    servoLeft.pulsewidth_us(servoOut[1]); 
-    servoThrust.pulsewidth_us(motorOut[0]);
+    servo.pulsewidth_us(servoOut[0]);
      
     //観測アップデート
     calcDynAcc();
@@ -541,31 +516,17 @@
 {    
     pitchPID.setSetPoint(0.0);
     pitchratePID.setSetPoint(0.0); 
-    rollPID.setSetPoint(0.0); 
-    rollratePID.setSetPoint(0.0); 
     pitchPID.setBias(0.0);
-    pitchratePID.setBias(0.0); 
-    rollPID.setBias(0.0); 
-    rollratePID.setBias(0.0); 
+    pitchratePID.setBias(0.0);  
     pitchPID.setOutputLimits(-1.0,1.0);
     pitchratePID.setOutputLimits(-1.0,1.0);
-    rollPID.setOutputLimits(-1.0,1.0); 
-    rollratePID.setOutputLimits(-1.0,1.0);
     pitchPID.setInputLimits(-pi,pi);
     pitchratePID.setInputLimits(-pi,pi);
-    rollPID.setInputLimits(-pi,pi); 
-    rollratePID.setInputLimits(-pi,pi);
     
-    servoRight.period_us(15000.0);
-    servoLeft.period_us(15000.0);
-    servoThrust.period_us(15000.0);
-    servoRight.pulsewidth_us(1500.0);
-    servoLeft.pulsewidth_us(1500.0); 
-    servoThrust.pulsewidth_us(1100.0);
+    servo.period_us(15000.0);
+    servo.pulsewidth_us(1500.0);
     
     pc.baud(57600);
-    sd.baud(57600);
-    sd.printf("\r\n Program Start \r\n");
     accelgyro.initialize();
     //加速度計のフルスケールレンジを設定
     accelgyro.setFullScaleAccelRange(ACCEL_FSR);