solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 28:fc6c46c1db65
- Parent:
- 27:43bd0e444633
- Child:
- 29:34ee662f454e
diff -r 43bd0e444633 -r fc6c46c1db65 main.cpp --- a/main.cpp Tue Feb 16 02:56:16 2021 +0000 +++ b/main.cpp Tue Feb 16 03:24:20 2021 +0000 @@ -42,9 +42,14 @@ double acc_x,acc_y,acc_z; double gyro_x,gyro_y,gyro_z; int out1, out2; -float scaled_servo[3]; -int servo_out[3] = {0,0,0}; -int motor_out[1] = {0}; +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 = 1800; +int motorPwmMin = 1200; int offsetVal[6] = {0,0,0,0,0,0}; const double pitch_align = 0.0; @@ -64,12 +69,49 @@ // 割り込まれた時点での出力(computeの結果)を返す関数 void calcServoOut(){ - 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 - } + 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 } + } + + pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; + roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; + 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; + for(int i = 0;i<sizeof(servoOut);i++){ + servoOut[i] = int(map(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax)) + if(servoOut[i]<servoPwmMin) { + servoOut[i] = servoPwmMin; + }; + if(servoOut[i]>servoPwmMax) { + servoOut[i] = servoPwmMax; + }; + } + + for(int i = 0;i<sizeof(motorOut);i++){ + motorOut[i] = int(map(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax)) + if(motorOut[i]<motorPwmMin) { + motorOut[i] = motorPwmMin; + }; + if(motorOut[i]>motorPwmMax) { + motorOut[i] = motorPwmMax; + }; + } + + servoRight.pulsewidth_us(servoOut[0]); + //servoLeft.pulsewidth_us(servoOut[1]); + if(loop_count > 10) pushto_sdcard(); loop_count = 0; @@ -97,7 +139,7 @@ { LoopTicker sdcard_loop; LoopTicker PIDtick; - PIDtick.attach(PID_compute,PID_dt); + PIDtick.attach(calcServoOut,PID_dt); pc.baud(115200); sd.baud(115200); accelgyro.initialize(); @@ -107,53 +149,35 @@ accelgyro.setFullScaleGyroRange(GYRO_FSR); //MPU6050のLPFを設定 accelgyro.setDLPFMode(MPU6050_LPF); - + + 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); + pitchPID.setOutputLimits(-1.0,1.0); + pitchratePID.setOutputLimits(-1.0,1.0); + rollPID.setOutputLimits(-1.0,1.0); + rollratePID.setOutputLimits(-pi,pi); + pitchPID.setInputLimits(-pi,pi); + pitchratePID.setInputLimits(-pi,pi); + rollPID.setInputLimits(-pi,pi); + rollratePID.setInputLimits(-pi,pi); servoRight.period_us(20000); - MadgwickFilter.begin(100); //サンプリング周波数Hza t.start(); while(1) { tstart = t.read(); //姿勢角を更新 updateAttitude() - - pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; - roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; + PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する - //float LP_rc3 = 0.15f; - servoOut = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*roll_ac; - int pwmMax = 1800; - int pwmMin = 1200; - if(out1<pwmMin) { - out1 = pwmMin; - }; - if(out1>pwmMax) { - out1 = pwmMax; - }; - out2 = map((int)(rc[2]*1000.0f),-1000,1000,pwmMin,pwmMax); - if(out2<pwmMin) { - out2 = pwmMin; - }; - if(out2>pwmMax) { - out2 = pwmMax; - }; - pc.printf("roll%lf ", gyro_x); - //pc.printf("out1:%d\n ", out1); - //pc.printf("out2:%d ", out2); - wait(PID_dt - t.read() + tstart); // PIDのために、待ち時間調節(割り込みにするべき) float tend = t.read(); - MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza pc.printf("time%f \r\n", (tend-tstart)); - test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう - PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する - //sdcard_loop.loop(); - printf("scaled_servo[0]: %f",scaled_servo[0]); - servo_out[0] = map((int)(scaled_servo[0]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用 - for (int i = 0; i < 3 ; i ++){ - servo_out[i] = max(servo_out[i],pwmMin); - servo_out[i] = min(servo_out[i], pwmMax); - } - pc.printf("out1:%d ",servo_out[0]); - servoRight.pulsewidth_us(servo_out[0]); + MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hz + } } \ No newline at end of file