uva12538 Version Controlled IDE(可持续化Treap * 模板, STL ext/rope(块状链表))

本文介绍了一种名为可持续化Treap的数据结构及其应用,并提供了详细的代码实现。此外,还介绍了另一种高效的数据结构——STL绳子(rope),并给出了具体的使用案例。

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

uva12538 Version Controlled IDE(可持续化Treap * 模板, STL ext/rope(块状链表))

学习:http://www.2cto.com/kf/201401/273863.html

本题是可持续化Treap 的基本题目,虽然可持续化的Treap基本操作不难,但是还没什么深研究,留作模板

可持续化数据结构还是用不好,待深入学习:范浩强谈数据结构

, CLJ的可持久化数据结构研究???

写下注意事项和理解:

(1)指针的使用,在使用之前必须先new()

(2)实现可持续化是Copy()中new()后在复制,若不是先可持续化在可直接复制

(3)注意:Split和Merge的使用


持续更新。。。

本题代码:

#include <iostream>
#include <cstdio>
#include <cstring>
#include <string>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <vector>
#include <set>

using namespace std;
const double eps = 1e-10;
const int maxn = 1100000;
const int M = 51000;

struct node {
    node* ch[2];
    char c;
    int sz, r;
    void up()
    {
        sz = ch[0]->sz + ch[1]->sz + 1;
    }
}*null;
//node t[maxn];
node *root[M];

char s[1100];
int op, pos, num, version;
int tot;
int m, subt;
int step;

void newnode(node* &x, char cc = 0)
{
    x = new node();
    x->c = cc;
    x->sz = 1;
    x->r = rand();
    x->ch[0] = x->ch[1] = null;
}

void copy(node* &x, node* y)
{
    if (y == null) x = y;
    else newnode(x), *x = *y;///可持久化时,修改操作经过的路径要新建节点
}

void merge(node* &o, node* a, node* b)
{
    if (a == null) copy(o, b);
    else if (b == null) copy(o, a);
    else if (a->r < b->r)
    {
        copy(o, a);
        merge(o->ch[1], a->ch[1], b);
        o->up();
    }
    else
    {
        copy(o, b);
        merge(o->ch[0], a, b->ch[0]);
        o->up();
    }
}
void split(node* o, node* &a, node* &b, int k)
{
    if (!k) {
        copy(b, o);
        a = null;
    }
    else if (o->sz <= k){
       copy(a, o);
       b = null;
    }
    else if (o->ch[0]->sz >= k)
    {
        copy(b, o);
        split(o->ch[0], a, b->ch[0], k);
        b->up();
    }
    else
    {
        copy(a, o);
        split(o->ch[1], a->ch[1], b, k - o->ch[0]->sz - 1);
        a->up();
    }
}

void build(node* &o, int l, int r)
{
    if (l > r) return ;
    int m = (l + r) >> 1;
    newnode(o, s[m - 1]);
    build(o->ch[0], l, m - 1);
    build(o->ch[1], m + 1, r);
    o->up();
}


void out(node *o)
{
    if (o->ch[0] != null)
        out(o->ch[0]);
    printf("%c", o->c);
    if (o->c == 'c') subt++;
    if (o->ch[1] != null)
        out(o->ch[1]);
}

void solve_insert(node* &o, int pos, char s[])
{
    int n = strlen(s);
    node *a, *b, *c, *d;
    build(a, 1, n);
    split(root[step - 1], b, c, pos);
    merge(d, b, a);
    merge(o, d, c);

}
void solve_delet(node* &o, int pos, int num)
{
    node *a, *b, *c, *d;
    split(root[step - 1], a, b, pos - 1);
    split(b, b, c, num);///可同时使用b b???
    merge(o, a, c);
}

void solve_query(int version, int pos, int num)
{
    node *a, *b, *c, *d;

    split(root[version], a, b, pos - 1);
    split(b, c, d, num);
    out(c);
    puts("");
}


void Init()
{
    tot = 0;
    newnode(null);
    null->sz = 0;

    for (int i = 0; i <= m; i++)
        root[i] = null;
}

int main()
{
    scanf("%d", &m);
    Init();
    subt = 0;
    step = 0;
    while (m--)
    {
        scanf("%d", &op);
        if (op == 1)
        {
            scanf("%d", &pos);
            scanf("%s", s);
            solve_insert(root[++step], pos - subt, s);
        }
        else if (op == 2)
        {
            scanf("%d%d", &pos, &num);
            solve_delet(root[++step], pos - subt, num - subt);
        }
        else if (op == 3)
        {
            scanf("%d%d%d", &version, &pos, &num);
            solve_query(version - subt, pos - subt, num - subt);
        }

    }

}


还有一种STL的解法:

#include <ext/rope>

using namespace __gnu_cxx;

crope t, trp[12], tmp;

具体使用见代码。。。

此STL实现的是块状链表,效率和上面的几乎一样,具体是n*(n^0.5)参考来源:http://www.cnblogs.com/zhsl/archive/2013/05/06/3062000.html

STL。。。

//#pragma warning (disable: 4786)
//#pragma comment (linker, "/STACK:16777216")
//HEAD
#include <cstdio>
#include <ctime>
#include <cstdlib>
#include <cstring>
#include <queue>
#include <string>
#include <set>
#include <stack>
#include <map>
#include <cmath>
#include <vector>
#include <iostream>
#include <algorithm>

#include <ext/rope>///!!
using namespace __gnu_cxx;///!!
using namespace std;


typedef long long LL;
crope t, trp[50005], tmp;///!!
char s[205];
int n, nowv, subt, version;
int op, pos, num;

int main ()
{
    scanf("%d", &n);
    subt = 0;
    nowv = 0;
    while (n--)
    {
        scanf("%d", &op);
        if (op == 1) {
            scanf("%d%s", &pos, s);
            pos -= subt;
            t.insert(pos, s);///在pos后插入串s
            trp[++nowv] = t;
        }
        else if (op == 2) {
            scanf("%d%d", &pos, &num);
            pos -= subt; num -= subt;
            t.erase(pos - 1, num);///删除pos-1后面num个字母
            trp[++nowv] = t;
        }
        else {
            scanf("%d%d%d", &version, &pos, &num);
            version -= subt; pos -= subt; num -= subt;
            tmp = trp[version].substr(pos - 1, num);///取出pos - 1后面的num个字母
            subt += count(tmp.begin(), tmp.end(), 'c');///统计c的个数
            cout << tmp << endl;///输出tmp
        }
    }
    return 0;
}



#include "MyProject.h" #include "DRV8313.h" #include "BLDC_tim.h" #include "tim.h" /************************************************ main中调用的接口函数都在当前文件中 ================================================= 本程序仅供学习,引用代码请标明出处 使用教程:https://blog.youkuaiyun.com/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.youkuaiyun.com/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.youkuaiyun.com/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、付费专栏及课程。

余额充值