HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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@139:b378528c05f2, 2021-12-06 (annotated)

Committer:
cocorlow
Date:
Mon Dec 06 08:26:16 2021 +0000
Revision:
139:b378528c05f2
Parent:
137:240588882ae2
Child:
140:53dbdb207542
Eigen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 139:b378528c05f2 1 //#include "global.hpp"
cocorlow 139:b378528c05f2 2 //
cocorlow 139:b378528c05f2 3 //// 割り込まれた時点での出力(computeの結果）を返す関数
cocorlow 139:b378528c05f2 4 //void calcServoOut()
cocorlow 139:b378528c05f2 5 //{
cocorlow 139:b378528c05f2 6 // // sbusデータの読み込み
cocorlow 139:b378528c05f2 7 // for (int i =0 ; i < 16;i ++){
cocorlow 139:b378528c05f2 8 // rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
cocorlow 139:b378528c05f2 9 // }
cocorlow 139:b378528c05f2 10 //
cocorlow 139:b378528c05f2 11 //
cocorlow 139:b378528c05f2 12 // //姿勢角の所得
cocorlow 139:b378528c05f2 13 // euler = eskf.computeAngles();
cocorlow 139:b378528c05f2 14 // rpy.x = euler(1,1);
cocorlow 139:b378528c05f2 15 // rpy.y = euler(2,1);
cocorlow 139:b378528c05f2 16 // rpy.z = euler(3,1);
cocorlow 139:b378528c05f2 17 //
cocorlow 139:b378528c05f2 18 // //PIDへの状態量のセット
cocorlow 139:b378528c05f2 19 // pitchPID.setProcessValue(rpy.y);
cocorlow 139:b378528c05f2 20 // pitchratePID.setProcessValue(gyro.y);
cocorlow 139:b378528c05f2 21 // rollPID.setProcessValue(rpy.x);
cocorlow 139:b378528c05f2 22 // rollratePID.setProcessValue(gyro.x);
cocorlow 139:b378528c05f2 23 //
cocorlow 139:b378528c05f2 24 // dT = rc[2];
cocorlow 139:b378528c05f2 25 //
cocorlow 139:b378528c05f2 26 // if (rc[4]>-0.3f && rc[6] < -0.3f)
cocorlow 139:b378528c05f2 27 // {
cocorlow 139:b378528c05f2 28 // //level_flight();
cocorlow 139:b378528c05f2 29 // //point_guide();
cocorlow 139:b378528c05f2 30 // climb();
cocorlow 139:b378528c05f2 31 // rollPID.setSetPoint(roll_obj);
cocorlow 139:b378528c05f2 32 // pitchPID.setSetPoint(pitch_obj);
cocorlow 139:b378528c05f2 33 // dT += dT_obj;
cocorlow 139:b378528c05f2 34 // }else{
cocorlow 139:b378528c05f2 35 // rollPID.setSetPoint(0.0f);
cocorlow 139:b378528c05f2 36 // pitchPID.setSetPoint(0.0f);
cocorlow 139:b378528c05f2 37 // }
cocorlow 139:b378528c05f2 38 //
cocorlow 139:b378528c05f2 39 // //舵角計算
cocorlow 139:b378528c05f2 40 // if(rc[4]<-0.3f){
cocorlow 139:b378528c05f2 41 // de = (rc[0]-rc[1])/2.0f;
cocorlow 139:b378528c05f2 42 // da = (rc[0]+rc[1])/2.0f;
cocorlow 139:b378528c05f2 43 // }else{
cocorlow 139:b378528c05f2 44 // de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f;
cocorlow 139:b378528c05f2 45 // da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f;
cocorlow 139:b378528c05f2 46 // }
cocorlow 139:b378528c05f2 47 //
cocorlow 139:b378528c05f2 48 // scaledServoOut[0]=de+da;
cocorlow 139:b378528c05f2 49 // scaledServoOut[1]=-de+da;
cocorlow 139:b378528c05f2 50 // scaledMotorOut[0]= dT;
cocorlow 139:b378528c05f2 51 //
cocorlow 139:b378528c05f2 52 // float LP_servo = 0.2;
cocorlow 139:b378528c05f2 53 // float LP_motor = 0.2;
cocorlow 139:b378528c05f2 54 // for(int i = 0; i < sizeof(servoOut)/sizeof(servoOut[0]); i++)
cocorlow 139:b378528c05f2 55 // {
cocorlow 139:b378528c05f2 56 // servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
cocorlow 139:b378528c05f2 57 // if(servoOut[i]<servoPwmMin)
cocorlow 139:b378528c05f2 58 // {
cocorlow 139:b378528c05f2 59 // servoOut[i] = servoPwmMin;
cocorlow 139:b378528c05f2 60 // }
cocorlow 139:b378528c05f2 61 // if(servoOut[i]>servoPwmMax)
cocorlow 139:b378528c05f2 62 // {
cocorlow 139:b378528c05f2 63 // servoOut[i] = servoPwmMax;
cocorlow 139:b378528c05f2 64 // }
cocorlow 139:b378528c05f2 65 // }
cocorlow 139:b378528c05f2 66 //
cocorlow 139:b378528c05f2 67 // for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
cocorlow 139:b378528c05f2 68 // motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
cocorlow 139:b378528c05f2 69 // if(motorOut[i]<motorPwmMin) {
cocorlow 139:b378528c05f2 70 // motorOut[i] = motorPwmMin;
cocorlow 139:b378528c05f2 71 // };
cocorlow 139:b378528c05f2 72 // if(motorOut[i]>motorPwmMax) {
cocorlow 139:b378528c05f2 73 // motorOut[i] = motorPwmMax;
cocorlow 139:b378528c05f2 74 // };
cocorlow 139:b378528c05f2 75 // }
cocorlow 139:b378528c05f2 76 // servoRight.pulsewidth_us(servoOut[0]);
cocorlow 139:b378528c05f2 77 // servoLeft.pulsewidth_us(servoOut[1]);
cocorlow 139:b378528c05f2 78 // servoThrust.pulsewidth_us(motorOut[0]);
cocorlow 139:b378528c05f2 79 //
cocorlow 139:b378528c05f2 80 // sendData2PC();
cocorlow 139:b378528c05f2 81 // writeSDcard();
cocorlow 139:b378528c05f2 82 //
cocorlow 139:b378528c05f2 83 // if(loop_count >= 5)
cocorlow 139:b378528c05f2 84 // {
cocorlow 139:b378528c05f2 85 // sendTelemetry();
cocorlow 139:b378528c05f2 86 // loop_count = 1;
cocorlow 139:b378528c05f2 87 //
cocorlow 139:b378528c05f2 88 // }
cocorlow 139:b378528c05f2 89 // else
cocorlow 139:b378528c05f2 90 // {
cocorlow 139:b378528c05f2 91 // loop_count +=1;
cocorlow 139:b378528c05f2 92 // }
cocorlow 139:b378528c05f2 93 //}