629. K Inverse Pairs Array

本文探讨了如何计算由1到n构成的数组中恰好包含k个逆序对的不同数组的数量,并提供两种动态规划方法来解决该问题。一种是直接使用三维动态规划,另一种则是通过状态压缩优化算法。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

Given two integers n and k, find how many different arrays consist of numbers from 1 to n such that there are exactly k inverse pairs.

We define an inverse pair as following: For ith and jth element in the array, if i < j and a[i] > a[j] then it's an inverse pair; Otherwise, it's not.

Since the answer may very large, the answer should be modulo 109 + 7.

Example 1:

Input: n = 3, k = 0
Output: 1
Explanation: 
Only the array [1,2,3] which consists of numbers from 1 to 3 has exactly 0 inverse pair.

 

Example 2:

Input: n = 3, k = 1
Output: 2
Explanation: 
The array [1,3,2] and [2,1,3] have exactly 1 inverse pair.

 

Note:

  1. The integer n is in the range [1, 1000] and k is in the range [0, 1000].

 

思路:直觉是DP,第3层循环枚举最大值得位置

public class Solution {
    public int kInversePairs(int n, int k) {
        long[][] dp = new long[n+1][k+1];
        
        for(int i=0; i<=n; i++)
        	dp[i][0] = 1;
        
        for(int i=1; i<=n; i++)
        	for(int j=1; j<=k; j++) {
        		for(int q=0; q<=j; q++) {
        			if(i-1 >= j-q) {
        				dp[i][j] += dp[i-1][q];
        				dp[i][j] %= 1000000007;
        			}
        		}
        	}
        
        return (int) dp[n][k];
    }
}


TLE,原来dp过程有枚举过程,而且重复dp[i][j-1]的枚举过程,可以进行状态压缩

package l629;
/*
 * 自觉告诉我DP
 * 状态压缩,dp过程有枚举过程,而且重复dp[i][j-1]的枚举过程
 */
public class Solution {
    public int kInversePairs(int n, int k) {
        long[][] dp = new long[n+1][k+1];
        
        for(int i=0; i<=n; i++)
        	dp[i][0] = 1;
        
        for(int i=1; i<=n; i++)
        	for(int j=1; j<=k; j++) {
        		dp[i][j] = dp[i][j-1]+dp[i-1][j];
        		if(j-i >= 0) dp[i][j] -= dp[i-1][j-i];
        		dp[i][j] += 1000000007;
        		dp[i][j] %= 1000000007;
        	}
        
        return (int) dp[n][k];
    }
}

 

 

 

 

dp[n][k] denotes the number of arrays that have k inverse pairs for array composed of 1 to n
we can establish the recursive relationship between dp[n][k] and dp[n-1][i]:

 

if we put n as the last number then all the k inverse pair should come from the first n-1 numbers
if we put n as the second last number then there's 1 inverse pair involves n so the rest k-1 comes from the first n-1 numbers
...
if we put n as the first number then there's n-1 inverse pairs involve n so the rest k-(n-1) comes from the first n-1 numbers

 

dp[n][k] = dp[n-1][k]+dp[n-1][k-1]+dp[n-1][k-2]+...+dp[n-1][k+1-n+1]+dp[n-1][k-n+1]

 

It's possible that some where in the right hand side the second array index become negative, since we cannot generate negative inverse pairs we just treat them as 0, but still leave the item there as a place holder.

 

dp[n][k] = dp[n-1][k]+dp[n-1][k-1]+dp[n-1][k-2]+...+dp[n-1][k+1-n+1]+dp[n-1][k-n+1]
dp[n][k+1] = dp[n-1][k+1]+dp[n-1][k]+dp[n-1][k-1]+dp[n-1][k-2]+...+dp[n-1][k+1-n+1]

 

so by deducting the first line from the second line, we have

 

dp[n][k+1] = dp[n][k]+dp[n-1][k+1]-dp[n-1][k+1-n]

#include "MyProject.h" #include "DRV8313.h" #include "BLDC_tim.h" #include "tim.h" /************************************************ main中调用的接口函数都在当前文件中 ================================================= 本程序仅供学习,引用代码请标明出处 使用教程:https://blog.csdn.net/loop222/article/details/120471390 创建日期:20210925 作 者:loop222 @郑州 ************************************************/ /******************************************************************************/ extern float target; /******************************************************************************/ long sensor_direction; float voltage_power_supply; float voltage_limit; float voltage_sensor_align; int pole_pairs; unsigned long open_loop_timestamp; float velocity_limit; /******************************************************************************/ int alignSensor(void); float velocityOpenloop(float target_velocity); float angleOpenloop(float target_angle); /******************************************************************************/ void Motor_init(void) { // printf("MOT: Init\r\n"); // new_voltage_limit = current_limit * phase_resistance; // voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit; if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; sensor_direction=UNKNOWN; //M1_Enable; // printf("MOT: Enable driver.\r\n"); } /******************************************************************************/ void Motor_initFOC(void) { alignSensor(); //检测零点偏移量和极对数 //added the shaft_angle update angle_prev=getAngle(); //getVelocity(),make sure velocity=0 after power on HAL_Delay(5); shaft_velocity = shaftVelocity(); //必须调用一次,进入主循环后速度为0 HAL_Delay(5); shaft_angle = shaftAngle();// shaft angle if(controller==Type_angle)target=shaft_angle;//角度模式,以当前的角度为目标角度,进入主循环后电机静止 HAL_Delay(200); } /******************************************************************************/ int alignSensor(void) { long i; float angle; float mid_angle,end_angle; float moved; // printf("MOT: Align sensor.\r\n"); // find natural direction // move one electrical revolution forward for(i=0; i<=500; i++) { angle = _3PI_2 + _2PI * i / 500.0; setPhaseVoltage(voltage_sensor_align, 0, angle); HAL_Delay(2); } mid_angle=getAngle(); for(i=500; i>=0; i--) { angle = _3PI_2 + _2PI * i / 500.0 ; setPhaseVoltage(voltage_sensor_align, 0, angle); HAL_Delay(2); } end_angle=getAngle(); setPhaseVoltage(0, 0, 0); HAL_Delay(200); // printf("mid_angle=%.4f\r\n",mid_angle); // printf("end_angle=%.4f\r\n",end_angle); moved = fabs(mid_angle - end_angle); if((mid_angle == end_angle)||(moved < 0.02)) //相等或者几乎没有动 { // printf("MOT: Failed to notice movement loop222.\r\n"); // M1_Disable; //电机检测不正常,关闭驱动 DRV8313_ENx_Off(); return 0; } else if(mid_angle < end_angle) { // printf("MOT: sensor_direction==CCW\r\n"); sensor_direction=CCW; } else { // printf("MOT: sensor_direction==CW\r\n"); sensor_direction=CW; } // printf("MOT: PP check: "); //计算Pole_Pairs if( fabs(moved*pole_pairs - _2PI) > 0.5 ) // 0.5 is arbitrary number it can be lower or higher! { // printf("fail - estimated pp:"); pole_pairs=_2PI/moved+0.5; //浮点数转整形,四舍五入 // printf("%d\r\n",pole_pairs); } else // printf("OK!\r\n"); setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); //计算零点偏移角度 HAL_Delay(700); zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*getAngle(), pole_pairs)); HAL_Delay(20); // printf("MOT: Zero elec. angle:"); // printf("%.4f\r\n",zero_electric_angle); setPhaseVoltage(0, 0, 0); // delay_ms(200); return 1; } /******************************************************************************/ void loopFOC(void) { if( controller==Type_angle_openloop || controller==Type_velocity_openloop ) return; shaft_angle = shaftAngle();// shaft angle electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first switch(torque_controller) { case Type_voltage: // no need to do anything really break; case Type_dc_current: break; case Type_foc_current: break; default: // printf("MOT: no torque control selected!"); break; } // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } /******************************************************************************/ void move(float new_target) { shaft_velocity = shaftVelocity(); switch(controller) { case Type_torque: if(torque_controller==Type_voltage)voltage.q = new_target; // if voltage torque control else current_sp = new_target; // if current/foc_current torque control break; case Type_angle: // angle set point shaft_angle_sp = new_target; // calculate velocity set point shaft_velocity_sp = PID_angle( shaft_angle_sp - shaft_angle ); // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control // if torque controlled through voltage if(torque_controller == Type_voltage) { voltage.q = current_sp; voltage.d = 0; } break; case Type_velocity: // velocity set point shaft_velocity_sp = new_target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control // if torque controlled through voltage control if(torque_controller == Type_voltage) { voltage.q = current_sp; // use voltage if phase-resistance not provided voltage.d = 0; } break; case Type_velocity_openloop: // velocity control in open loop shaft_velocity_sp = new_target; voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor voltage.d = 0; break; case Type_angle_openloop: // angle control in open loop shaft_angle_sp = new_target; voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor voltage.d = 0; break; } } /******************************************************************************/ void setPhaseVoltage(float Uq, float Ud, float angle_el) { float Uout; uint32_t sector; float T0,T1,T2; float Ta,Tb,Tc; if(Ud) // only if Ud and Uq set {// _sqrt is an approx of sqrt (3-4% error) Uout = _sqrt(Ud*Ud + Uq*Uq) / voltage_power_supply; // angle normalisation in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); } else {// only Uq available - no need for atan2 and sqrt Uout = Uq / voltage_power_supply; // angle normalisation in between 0 and 2pi // only necessary if using _sin and _cos - approximation functions angle_el = _normalizeAngle(angle_el + _PI_2); } if(Uout> 0.577)Uout= 0.577; if(Uout<-0.577)Uout=-0.577; sector = (angle_el / _PI_3) + 1; T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout; T0 = 1 - T1 - T2; // calculate the duty cycles(times) switch(sector) { case 1: Ta = T1 + T2 + T0/2; Tb = T2 + T0/2; Tc = T0/2; break; case 2: Ta = T1 + T0/2; Tb = T1 + T2 + T0/2; Tc = T0/2; break; case 3: Ta = T0/2; Tb = T1 + T2 + T0/2; Tc = T2 + T0/2; break; case 4: Ta = T0/2; Tb = T1+ T0/2; Tc = T1 + T2 + T0/2; break; case 5: Ta = T2 + T0/2; Tb = T0/2; Tc = T1 + T2 + T0/2; break; case 6: Ta = T1 + T2 + T0/2; Tb = T0/2; Tc = T1 + T0/2; break; default: // possible error state Ta = 0; Tb = 0; Tc = 0; } SetPWMDutyPersent(&htim3, TIM_CHANNEL_1, Ta*100); SetPWMDutyPersent(&htim3, TIM_CHANNEL_2, Tb*100); SetPWMDutyPersent(&htim3, TIM_CHANNEL_3, Tc*100); // TIM_SetCompare1(TIM2,Ta*PWM_Period); // TIM_SetCompare2(TIM2,Tb*PWM_Period); // TIM_SetCompare3(TIM2,Tc*PWM_Period); } /******************************************************************************/ float velocityOpenloop(float target_velocity) { unsigned long now_us; float Ts,Uq; now_us = SysTick->VAL; //_micros(); if(now_us<open_loop_timestamp)Ts = (float)(open_loop_timestamp - now_us)/9*1e-6; else Ts = (float)(0xFFFFFF - now_us + open_loop_timestamp)/9*1e-6; open_loop_timestamp=now_us; //save timestamp for next call // quick fix for strange cases (micros overflow) if(Ts == 0 || Ts > 0.5) Ts = 1e-3; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); Uq = voltage_limit; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); return Uq; } /******************************************************************************/ float angleOpenloop(float target_angle) { unsigned long now_us; float Ts,Uq; now_us = SysTick->VAL; //_micros(); if(now_us<open_loop_timestamp)Ts = (float)(open_loop_timestamp - now_us)/9*1e-6; else Ts = (float)(0xFFFFFF - now_us + open_loop_timestamp)/9*1e-6; open_loop_timestamp = now_us; //save timestamp for next call // quick fix for strange cases (micros overflow) if(Ts == 0 || Ts > 0.5) Ts = 1e-3; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) if(fabs( target_angle - shaft_angle ) > velocity_limit*Ts) { shaft_angle += _sign(target_angle - shaft_angle) * velocity_limit * Ts; //shaft_velocity = velocity_limit; } else { shaft_angle = target_angle; //shaft_velocity = 0; } Uq = voltage_limit; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); return Uq; } /******************************************************************************/ #include "MyProject.h" /***************************************************************************/ // int array instead of float array // 4x200 points per 360 deg // 2x storage save (int 2Byte float 4 Byte ) // sin*10000 const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000}; /***************************************************************************/ // function approximating the sine calculation by using fixed size array // ~40us (float array) // ~50us (int array) // precision +-0.005 // it has to receive an angle in between 0 and 2PI float _sin(float a){ if(a < _PI_2){ //return sine_array[(int)(199.0*( a / (_PI/2.0)))]; //return sine_array[(int)(126.6873* a)]; // float array optimized return 0.0001*sine_array[_round(126.6873* a)]; // int array optimized }else if(a < _PI){ // return sine_array[(int)(199.0*(1.0 - (a-_PI/2.0) / (_PI/2.0)))]; //return sine_array[398 - (int)(126.6873*a)]; // float array optimized return 0.0001*sine_array[398 - _round(126.6873*a)]; // int array optimized }else if(a < _3PI_2){ // return -sine_array[(int)(199.0*((a - _PI) / (_PI/2.0)))]; //return -sine_array[-398 + (int)(126.6873*a)]; // float array optimized return -0.0001*sine_array[-398 + _round(126.6873*a)]; // int array optimized } else { // return -sine_array[(int)(199.0*(1.0 - (a - 3*_PI/2) / (_PI/2.0)))]; //return -sine_array[796 - (int)(126.6873*a)]; // float array optimized return -0.0001*sine_array[796 - _round(126.6873*a)]; // int array optimized } } /***************************************************************************/ // function approximating cosine calculation by using fixed size array // ~55us (float array) // ~56us (int array) // precision +-0.005 // it has to receive an angle in between 0 and 2PI float _cos(float a){ float a_sin = a + _PI_2; a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin; return _sin(a_sin); } /***************************************************************************/ // normalizing radian angle to [0,2PI] float _normalizeAngle(float angle){ float a = fmod(angle, _2PI); return a >= 0 ? a : (a + _2PI); } /***************************************************************************/ // Electrical angle calculation float _electricalAngle(float shaft_angle, int pole_pairs) { return (shaft_angle * pole_pairs); } /***************************************************************************/ // square root approximation function using // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root float _sqrtApprox(float number) {//low in fat long i; float y; // float x; // const float f = 1.5F; // better precision // x = number * 0.5F; y = number; i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); y = * ( float * ) &i; // y = y * ( f - ( x * y * y ) ); // better precision return number * y; } /***************************************************************************/ #include "MyProject.h" /******************************************************************************/ float shaft_angle;//!< current motor angle float electrical_angle; float shaft_velocity; float current_sp; float shaft_velocity_sp; float shaft_angle_sp; DQVoltage_s voltage; DQCurrent_s current; TorqueControlType torque_controller; MotionControlType controller; float sensor_offset=0; float zero_electric_angle; /******************************************************************************/ // shaft angle calculation float shaftAngle(void) { // if no sensor linked return previous value ( for open loop ) //if(!sensor) return shaft_angle; return sensor_direction*getAngle() - sensor_offset; } // shaft velocity calculation float shaftVelocity(void) { // if no sensor linked return previous value ( for open loop ) //if(!sensor) return shaft_velocity; return sensor_direction*LPF_velocity(getVelocity()); } /******************************************************************************/ float electricalAngle(void) { return _normalizeAngle((shaft_angle + sensor_offset) * pole_pairs - zero_electric_angle); } /******************************************************************************/ #include "MyProject.h" /******************************************************************************/ #define DEF_CURR_FILTER_Tf 0.005 //!< default currnet filter time constant #define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant /******************************************************************************/ //unsigned long lpf_vel_timestamp; float y_vel_prev=0; /******************************************************************************/ /* float LPF_velocity(float x) { unsigned long now_us; float Ts, alpha, y; now_us = SysTick->VAL; if(now_us<lpf_vel_timestamp)Ts = (float)(lpf_vel_timestamp - now_us)/9*1e-6f; else Ts = (float)(0xFFFFFF - now_us + lpf_vel_timestamp)/9*1e-6f; lpf_vel_timestamp = now_us; if(Ts == 0 || Ts > 0.5) Ts = 1e-3f; alpha = DEF_VEL_FILTER_Tf/(DEF_VEL_FILTER_Tf + Ts); y = alpha*y_prev + (1.0f - alpha)*x; y_prev = y; return y; } */ float LPF_velocity(float x) { float y = 0.9*y_vel_prev + 0.1*x; y_vel_prev=y; return y; } /******************************************************************************/ #include "MyProject.h" #include "AS5600.h" #define AS5600_RAW_ANGLE_REG 0x0C #define AS5600_ANGLE_REG 0x0E /************************************************ 本程序仅供学习,引用代码请标明出处 使用教程:https://blog.csdn.net/loop222/article/details/120471390 创建日期:20210925 作 者:loop222 @郑州 ************************************************/ /******************************************************************************/ long cpr=4096; float full_rotation_offset; long angle_data_prev; unsigned long velocity_calc_timestamp; float angle_prev; /******************************************************************************/ /******************************************************************************/ void MagneticSensor_Init(void) { angle_data_prev = AS5600_Read_uint16(AS5600_ANGLE_REG); full_rotation_offset = 0; velocity_calc_timestamp=0; } /******************************************************************************/ float getAngle(void) { float angle_data,d_angle; //angle_data = I2C_getRawCount(I2C1); angle_data = AS5600_Read_uint16(AS5600_ANGLE_REG); // tracking the number of rotations // in order to expand angle range form [0,2PI] to basically infinity d_angle = angle_data - angle_data_prev; // if overflow happened track it as full rotation if(fabs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; // save the current angle value for the next steps // in order to know if overflow happened angle_data_prev = angle_data; // return the full angle // (number of full rotations)*2PI + current sensor angle return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; /* AS5600_Refresh_Angle(&has5600); return has5600.angle;*/ } /******************************************************************************/ // Shaft velocity calculation float getVelocity(void) { unsigned long now_us; float Ts, angle_c, vel; // calculate sample time now_us = SysTick->VAL; //_micros(); if(now_us<velocity_calc_timestamp)Ts = (float)(velocity_calc_timestamp - now_us)/9*1e-6; else Ts = (float)(0xFFFFFF - now_us + velocity_calc_timestamp)/9*1e-6; // quick fix for strange cases (micros overflow) if(Ts == 0 || Ts > 0.5) Ts = 1e-3; // current angle angle_c = getAngle(); // velocity calculation vel = (angle_c - angle_prev)/Ts; // save variables for future pass angle_prev = angle_c; velocity_calc_timestamp = now_us; return vel; } /******************************************************************************/ #include "MyProject.h" /************************************************ 本程序仅供学习,引用代码请标明出处 使用教程:https://blog.csdn.net/loop222/article/details/120471390 创建日期:20210925 作 者:loop222 @郑州 ************************************************/ /******************************************************************************/ float pid_vel_P, pid_ang_P; float pid_vel_I, pid_ang_D; float integral_vel_prev; float error_vel_prev, error_ang_prev; float output_vel_ramp; float output_vel_prev; unsigned long pid_vel_timestamp, pid_ang_timestamp; /******************************************************************************/ void PID_init(void) { pid_vel_P=0.1; //0.1 pid_vel_I=2; //2 output_vel_ramp=100; //output derivative limit [volts/second] integral_vel_prev=0; error_vel_prev=0; output_vel_prev=0; pid_vel_timestamp=SysTick->VAL; pid_ang_P=10; pid_ang_D=0.5; error_ang_prev=0; pid_ang_timestamp=SysTick->VAL; } /******************************************************************************/ //just P&I is enough,no need D float PID_velocity(float error) { unsigned long now_us; float Ts; float proportional,integral,output; float output_rate; now_us = SysTick->VAL; if(now_us<pid_vel_timestamp)Ts = (float)(pid_vel_timestamp - now_us)/9*1e-6f; else Ts = (float)(0xFFFFFF - now_us + pid_vel_timestamp)/9*1e-6f; pid_vel_timestamp = now_us; if(Ts == 0 || Ts > 0.5) Ts = 1e-3f; // u(s) = (P + I/s + Ds)e(s) // Discrete implementations // proportional part // u_p = P *e(k) proportional = pid_vel_P * error; // Tustin transform of the integral part // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) integral = integral_vel_prev + pid_vel_I*Ts*0.5*(error + error_vel_prev); // antiwindup - limit the output integral = _constrain(integral, -voltage_limit, voltage_limit); // sum all the components output = proportional + integral; // antiwindup - limit the output variable output = _constrain(output, -voltage_limit, voltage_limit); // limit the acceleration by ramping the output output_rate = (output - output_vel_prev)/Ts; if(output_rate > output_vel_ramp)output = output_vel_prev + output_vel_ramp*Ts; else if(output_rate < -output_vel_ramp)output = output_vel_prev - output_vel_ramp*Ts; // saving for the next pass integral_vel_prev = integral; output_vel_prev = output; error_vel_prev = error; return output; } /******************************************************************************/ //P&D for angle_PID float PID_angle(float error) { unsigned long now_us; float Ts; float proportional,derivative,output; //float output_rate; now_us = SysTick->VAL; if(now_us<pid_ang_timestamp)Ts = (float)(pid_ang_timestamp - now_us)/9*1e-6f; else Ts = (float)(0xFFFFFF - now_us + pid_ang_timestamp)/9*1e-6f; pid_ang_timestamp = now_us; if(Ts == 0 || Ts > 0.5) Ts = 1e-3f; // u(s) = (P + I/s + Ds)e(s) // Discrete implementations // proportional part // u_p = P *e(k) proportional = pid_ang_P * error; // u_dk = D(ek - ek_1)/Ts derivative = pid_ang_D*(error - error_ang_prev)/Ts; output = proportional + derivative; output = _constrain(output, -velocity_limit, velocity_limit); // limit the acceleration by ramping the output // output_rate = (output - output_ang_prev)/Ts; // if(output_rate > output_ang_ramp)output = output_ang_prev + output_ang_ramp*Ts; // else if(output_rate < -output_ang_ramp)output = output_ang_prev - output_ang_ramp*Ts; // saving for the next pass // output_ang_prev = output; error_ang_prev = error; return output; } /******************************************************************************/ /******************************************************************************/ 逐行解释
最新发布
07-23
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值