Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: servo.cpp
- Revision:
- 139:b378528c05f2
- Parent:
- 137:240588882ae2
- Child:
- 140:53dbdb207542
--- a/servo.cpp Wed Dec 01 19:17:36 2021 +0000 +++ b/servo.cpp Mon Dec 06 08:26:16 2021 +0000 @@ -1,93 +1,93 @@ -#include "global.hpp" - -// 割り込まれた時点での出力(computeの結果)を返す関数 -void calcServoOut() -{ - // sbusデータの読み込み - for (int i =0 ; i < 16;i ++){ - rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input - } - - - //姿勢角の所得 - euler = eskf.computeAngles(); - rpy.x = euler(1,1); - rpy.y = euler(2,1); - rpy.z = euler(3,1); - - //PIDへの状態量のセット - pitchPID.setProcessValue(rpy.y); - pitchratePID.setProcessValue(gyro.y); - rollPID.setProcessValue(rpy.x); - rollratePID.setProcessValue(gyro.x); - - dT = rc[2]; - - if (rc[4]>-0.3f && rc[6] < -0.3f) - { - //level_flight(); - //point_guide(); - climb(); - rollPID.setSetPoint(roll_obj); - pitchPID.setSetPoint(pitch_obj); - dT += dT_obj; - }else{ - rollPID.setSetPoint(0.0f); - pitchPID.setSetPoint(0.0f); - } - - //舵角計算 - if(rc[4]<-0.3f){ - de = (rc[0]-rc[1])/2.0f; - da = (rc[0]+rc[1])/2.0f; - }else{ - de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f; - da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f; - } - - scaledServoOut[0]=de+da; - scaledServoOut[1]=-de+da; - scaledMotorOut[0]= dT; - - 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) - { - servoOut[i] = servoPwmMin; - } - if(servoOut[i]>servoPwmMax) - { - servoOut[i] = servoPwmMax; - } - } - - 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]); - - sendData2PC(); - writeSDcard(); - - if(loop_count >= 5) - { - sendTelemetry(); - loop_count = 1; - - } - else - { - loop_count +=1; - } -} \ No newline at end of file +//#include "global.hpp" +// +//// 割り込まれた時点での出力(computeの結果)を返す関数 +//void calcServoOut() +//{ +// // sbusデータの読み込み +// for (int i =0 ; i < 16;i ++){ +// rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input +// } +// +// +// //姿勢角の所得 +// euler = eskf.computeAngles(); +// rpy.x = euler(1,1); +// rpy.y = euler(2,1); +// rpy.z = euler(3,1); +// +// //PIDへの状態量のセット +// pitchPID.setProcessValue(rpy.y); +// pitchratePID.setProcessValue(gyro.y); +// rollPID.setProcessValue(rpy.x); +// rollratePID.setProcessValue(gyro.x); +// +// dT = rc[2]; +// +// if (rc[4]>-0.3f && rc[6] < -0.3f) +// { +// //level_flight(); +// //point_guide(); +// climb(); +// rollPID.setSetPoint(roll_obj); +// pitchPID.setSetPoint(pitch_obj); +// dT += dT_obj; +// }else{ +// rollPID.setSetPoint(0.0f); +// pitchPID.setSetPoint(0.0f); +// } +// +// //舵角計算 +// if(rc[4]<-0.3f){ +// de = (rc[0]-rc[1])/2.0f; +// da = (rc[0]+rc[1])/2.0f; +// }else{ +// de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f; +// da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f; +// } +// +// scaledServoOut[0]=de+da; +// scaledServoOut[1]=-de+da; +// scaledMotorOut[0]= dT; +// +// 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) +// { +// servoOut[i] = servoPwmMin; +// } +// if(servoOut[i]>servoPwmMax) +// { +// servoOut[i] = servoPwmMax; +// } +// } +// +// 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]); +// +// sendData2PC(); +// writeSDcard(); +// +// if(loop_count >= 5) +// { +// sendTelemetry(); +// loop_count = 1; +// +// } +// else +// { +// loop_count +=1; +// } +//} \ No newline at end of file