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
servo.cpp
- Committer:
- osaka
- Date:
- 2021-11-12
- Revision:
- 109:eb255fc4462b
- Parent:
- 94:579e875a4244
- Child:
- 121:2523eef96b36
- Child:
- 123:b63d3524ffbc
File content as of revision 109:eb255fc4462b:
#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 } //姿勢角の所得 Matrix 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); if (true) { //level_flight(); //point_guide(); turning(); rollPID.setSetPoint(roll_obj); } //舵角計算 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; } dT = rc[2]; 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(); if(loop_count >= 5) { writeSDcard(); sendTelemetry(); loop_count = 1; } else { loop_count +=1; } }