{"id":228,"date":"2025-05-23T12:23:53","date_gmt":"2025-05-23T04:23:53","guid":{"rendered":"http:\/\/47.111.133.139\/?p=228"},"modified":"2025-05-26T13:08:29","modified_gmt":"2025-05-26T05:08:29","slug":"%e9%80%90%e9%a3%9e%e7%a7%91%e6%8a%80-tc264-pid","status":"publish","type":"post","link":"https:\/\/yin.nnneri.me\/?p=228","title":{"rendered":"\u9010\u98de\u79d1\u6280 TC264 PID"},"content":{"rendered":"\n

\u7a0b\u5e8f<\/h2>\n\n\n\n

c<\/h3>\n\n\n\n
C<\/span><\/path><\/path><\/svg><\/span>
#include<\/span> <\/span>"<\/span>zf_common_headfile.h<\/span>"<\/span><\/span>\n#include<\/span> <\/span>"<\/span>isr_config.h<\/span>"<\/span><\/span>\n<\/span>\n<\/span>\n\/* \u4f7f\u7528\u7c7b\u578bA\u7684\u7535\u673a\u63a7\u5236\u5668\u8bf7\u6ce8\u91ca\u6389\u4e0b\u8ff0\u4ee3\u7801 *\/<\/span><\/span>\n#define<\/span> <\/span>MOTOR_CONTROLLER_TYPE_B<\/span><\/span>\n<\/span>\n\/* \u672a\u5b9e\u73b0 *\/<\/span><\/span>\n#define<\/span> <\/span>MOTOR_L_ROTATION<\/span> <\/span>1<\/span><\/span>\n#define<\/span> <\/span>MOTOR_R_ROTATION<\/span> <\/span>-<\/span>1<\/span><\/span>\n<\/span>\n#define<\/span> <\/span>SERVO_MOTOR<\/span>          (ATOM1_CH1_P33_9)<\/span>    \/\/\u8235\u673aPWM\u901a\u9053<\/span><\/span>\n#define<\/span> <\/span>SERVO_MOTOR_FREQ<\/span>     (<\/span>100<\/span>)<\/span>                \/\/\u8235\u673aPWM\u9891\u7387<\/span><\/span>\n#define<\/span> <\/span>SERVO_MOTOR_MAX<\/span>      (<\/span>110<\/span>)<\/span>                \/\/\u8235\u673a\u6700\u5927\u7edd\u5bf9\u504f\u8f6c\u89d2<\/span><\/span>\n#define<\/span> <\/span>SERVO_MOTOR_MIN<\/span>      (<\/span>80<\/span>)<\/span>                 \/\/\u8235\u673a\u6700\u5c0f\u7edd\u5bf9\u504f\u8f6c\u89d2<\/span><\/span>\n#define<\/span> <\/span>SERVO_MOTOR_MID<\/span>      (<\/span>95<\/span>)<\/span>                 \/\/\u8235\u673a\u4e2d\u5fc3\u7edd\u5bf9\u504f\u8f6c\u89d2<\/span><\/span>\n#define<\/span> <\/span>SERVO_MOTOR_DUTY<\/span>(<\/span>x<\/span>)  ((<\/span>double<\/span>)PWM_DUTY_MAX<\/span>\/<\/span>(<\/span>1000.0<\/span>\/<\/span>(<\/span>double<\/span>)SERVO_MOTOR_FREQ)<\/span>*<\/span>(<\/span>0.5<\/span>+<\/span>(<\/span>double<\/span>)(x)<\/span>\/<\/span>90.0<\/span>))<\/span><\/span>\n<\/span>\n#define<\/span> <\/span>PIT0<\/span>                 (CCU60_CH0)<\/span>          \/\/\u4e2d\u65ad\u6e90<\/span><\/span>\n#define<\/span> <\/span>GEAR_RATIO<\/span>           (<\/span>2320<\/span>)<\/span>               \/\/\u8f6e\u901f\u7f16\u7801\u5668\u6bd4<\/span><\/span>\n#define<\/span> <\/span>PID_MS<\/span>               (<\/span>1<\/span>)<\/span>                  \/\/\u4e24\u8f6e\u8fd0\u7b97\u65f6\u95f4\u5dee<\/span><\/span>\n#define<\/span> <\/span>MAX_RPM<\/span>              (<\/span>100<\/span>)<\/span><\/span>\n<\/span>\n#define<\/span> <\/span>WHEEL_CIRCUMFERENCE<\/span>  (<\/span>6.7<\/span> <\/span>*<\/span> <\/span>3.14<\/span> <\/span>*<\/span> <\/span>0.01<\/span>)<\/span>  \/\/ \u8f6e\u5b50\u5468\u957f<\/span><\/span>\n#define<\/span> <\/span>FRONT_WHEEL_DISTANCE<\/span> (<\/span>0.152<\/span>)<\/span>              \/\/ \u524d\u8f6e\u8f74\u8ddd<\/span><\/span>\n#define<\/span> <\/span>BACK_WHEEL_DISTANCE<\/span>  (<\/span>0.152<\/span>)<\/span>              \/\/ \u540e\u8f6e\u8f74\u8ddd<\/span><\/span>\n#define<\/span> <\/span>WHEELBASE<\/span>            (<\/span>0.20<\/span>)<\/span>               \/\/ \u524d\u540e\u8f6e\u8ddd\u79bb<\/span><\/span>\n#define<\/span> <\/span>MOTOR_MAX<\/span>            (<\/span>60<\/span>)<\/span>                 \/\/ \u6700\u5927PWM<\/span><\/span>\n\/* \u63a5\u53e3\u4e0e\u901a\u9053\u5b9a\u4e49 *\/<\/span><\/span>\n#define<\/span> <\/span>MOTORL_1<\/span>             (ATOM0_CH4_P02_4)<\/span><\/span>\n#define<\/span> <\/span>MOTORL_2<\/span>             (ATOM0_CH5_P02_5)<\/span><\/span>\n#define<\/span> <\/span>MOTORR_2<\/span>             (ATOM1_CH6_P02_6)<\/span><\/span>\n#define<\/span> <\/span>MOTORR_1<\/span>             (ATOM1_CH7_P02_7)<\/span><\/span>\n#define<\/span> <\/span>MOTOR_L<\/span>              (ATOM1_CH7_P02_7)<\/span><\/span>\n#define<\/span> <\/span>MOTOR_R<\/span>              (ATOM0_CH5_P02_5)<\/span><\/span>\n#define<\/span> <\/span>MOTOR_L_DIRECTION<\/span>    (P02_6)<\/span><\/span>\n#define<\/span> <\/span>MOTOR_R_DIRECTION<\/span>    (P02_4)<\/span><\/span>\n<\/span>\n#define<\/span> <\/span>ENCODER1_DIR<\/span>         (TIM5_ENCODER)<\/span><\/span>\n#define<\/span> <\/span>ENCODER1_DIR_PULSE<\/span>   (TIM5_ENCODER_CH1_P10_3)<\/span><\/span>\n#define<\/span> <\/span>ENCODER1_DIR_DIR<\/span>     (TIM5_ENCODER_CH2_P10_1)<\/span><\/span>\n<\/span>\n#define<\/span> <\/span>ENCODER2_DIR<\/span>         (TIM6_ENCODER)<\/span><\/span>\n#define<\/span> <\/span>ENCODER2_DIR_PULSE<\/span>   (TIM6_ENCODER_CH1_P20_3)<\/span><\/span>\n#define<\/span> <\/span>ENCODER2_DIR_DIR<\/span>     (TIM6_ENCODER_CH2_P20_0)<\/span><\/span>\n<\/span>\nint32 Encoder1Count <\/span>=<\/span> <\/span>0<\/span>;<\/span><\/span>\nint32 Encoder2Count <\/span>=<\/span> <\/span>0<\/span>;<\/span><\/span>\n<\/span>\ndouble<\/span> Encoder1RPM <\/span>=<\/span> <\/span>0<\/span>;<\/span><\/span>\ndouble<\/span> Encoder2RPM <\/span>=<\/span> <\/span>0<\/span>;<\/span><\/span>\n<\/span>\ntypedef<\/span> <\/span>struct<\/span> {<\/span><\/span>\n        <\/span>double<\/span> SetSpeed;<\/span><\/span>\n        <\/span>double<\/span> ActualSpeed;<\/span><\/span>\n        <\/span>double<\/span> CurrSpeed;<\/span><\/span>\n        <\/span>double<\/span> Kp;<\/span><\/span>\n        <\/span>double<\/span> Ki;<\/span><\/span>\n        <\/span>double<\/span> Kd;<\/span><\/span>\n        <\/span>double<\/span> Err;<\/span><\/span>\n        <\/span>double<\/span> ErrLast;<\/span><\/span>\n        <\/span>double<\/span> ErrPrev;<\/span><\/span>\n}<\/span>pid_config_t<\/span>;<\/span><\/span>\n<\/span>\npid_config_t<\/span> MotorPid1 <\/span>=<\/span> {<\/span><\/span>\n        .Kp <\/span>=<\/span> <\/span>0.8<\/span>,<\/span><\/span>\n        .Ki <\/span>=<\/span> <\/span>0<\/span>,<\/span><\/span>\n        .Kd <\/span>=<\/span> <\/span>2.5<\/span>,<\/span><\/span>\n};<\/span><\/span>\n<\/span>\npid_config_t<\/span> MotorPid2 <\/span>=<\/span> {<\/span><\/span>\n        .Kp <\/span>=<\/span> <\/span>0.8<\/span>,<\/span><\/span>\n        .Ki <\/span>=<\/span> <\/span>0.0<\/span>,<\/span><\/span>\n        .Kd <\/span>=<\/span> <\/span>2.5<\/span>,<\/span><\/span>\n};<\/span><\/span>\n<\/span>\ndouble<\/span> speed_test <\/span>=<\/span> <\/span>0.3<\/span>;<\/span><\/span>\ndouble<\/span> distance_test<\/span>=<\/span> <\/span>0.45<\/span>;<\/span><\/span>\ndouble<\/span> offset_test <\/span>=<\/span> <\/span>0<\/span>;<\/span><\/span>\n<\/span>\ndouble<\/span> L <\/span>=<\/span> <\/span>0<\/span>;<\/span><\/span>\ndouble<\/span> R <\/span>=<\/span> <\/span>0<\/span>;<\/span><\/span>\nvoid<\/span> <\/span>movControl<\/span>(<\/span>double<\/span> <\/span>speed<\/span>, <\/span>double<\/span> <\/span>distance<\/span>, <\/span>double<\/span> <\/span>offset<\/span>);<\/span><\/span>\nvoid<\/span> <\/span>motorPwmExpect<\/span> (<\/span>double<\/span> <\/span>LPwm<\/span>,<\/span>double<\/span> <\/span>RPwm<\/span>);<\/span><\/span>\nvoid<\/span> <\/span>motorPwmSet<\/span>(<\/span>double<\/span> <\/span>LPwm<\/span>,<\/span>double<\/span> <\/span>RPwm<\/span>);<\/span><\/span>\nvoid<\/span> <\/span>servoAngleSet<\/span>(<\/span>double<\/span> <\/span>angle<\/span>);<\/span><\/span>\nvoid<\/span> <\/span>motorPwmInit<\/span>() {<\/span><\/span>\n    <\/span>pwm_init<\/span>(SERVO_MOTOR, SERVO_MOTOR_FREQ, <\/span>0<\/span>);<\/span><\/span>\n<\/span>\n#ifdef<\/span> <\/span>MOTOR_CONTROLLER_TYPE_B<\/span><\/span>\n    <\/span>pwm_init<\/span>(MOTOR_L, <\/span>17000<\/span>, <\/span>0<\/span>);<\/span><\/span>\n    <\/span>pwm_init<\/span>(MOTOR_R, <\/span>17000<\/span>, <\/span>0<\/span>);<\/span><\/span>\n    <\/span>gpio_init<\/span>(MOTOR_R_DIRECTION, GPO, GPIO_LOW, GPO_PUSH_PULL);<\/span><\/span>\n    <\/span>gpio_init<\/span>(MOTOR_L_DIRECTION, GPO, GPIO_LOW, GPO_PUSH_PULL);<\/span><\/span>\n#else<\/span><\/span>\n    <\/span>pwm_init<\/span>(MOTORL_1, <\/span>17000<\/span>, <\/span>0<\/span>);<\/span><\/span>\n    <\/span>pwm_init<\/span>(MOTORL_2, <\/span>17000<\/span>, <\/span>0<\/span>);<\/span><\/span>\n    <\/span>pwm_init<\/span>(MOTORR_1, <\/span>17000<\/span>, <\/span>0<\/span>);<\/span><\/span>\n    <\/span>pwm_init<\/span>(MOTORR_2, <\/span>17000<\/span>, <\/span>0<\/span>);<\/span><\/span>\n#endif<\/span><\/span>\n    <\/span>encoder_dir_init<\/span>(ENCODER1_DIR, ENCODER1_DIR_PULSE, ENCODER1_DIR_DIR);<\/span><\/span>\n    <\/span>encoder_dir_init<\/span>(ENCODER2_DIR, ENCODER2_DIR_PULSE, ENCODER2_DIR_DIR);<\/span><\/span>\n    <\/span>motorPwmExpect<\/span>(<\/span>0<\/span>,<\/span>8<\/span>);<\/span><\/span>\n    <\/span>pit_ms_init<\/span>(PIT0, PID_MS);<\/span><\/span>\n    <\/span>while<\/span> (<\/span>TRUE<\/span>) {<\/span><\/span>\n        <\/span>printf<\/span>(<\/span>"<\/span>1:<\/span>%f<\/span>\\n<\/span>"<\/span>,Encoder1RPM);<\/span><\/span>\n        <\/span>printf<\/span>(<\/span>"<\/span>2:<\/span>%f<\/span>\\n<\/span>"<\/span>,Encoder2RPM);<\/span><\/span>\n        <\/span>\/\/ motorPwmSet(L,R);<\/span><\/span>\n        <\/span>movControl<\/span>(speed_test, distance_test, offset_test);<\/span><\/span>\n        <\/span>system_delay_ms<\/span>(<\/span>100<\/span>);<\/span><\/span>\n    }<\/span><\/span>\n<\/span>\n}<\/span><\/span>\n<\/span>\n\/* \u7b97\u6cd5\u5b58\u5728\u6f5c\u5728\u95ee\u9898\uff0c\u5df2\u5e9f\u5f03 *\/<\/span><\/span>\n\/\/void movControl(double speed, double distance, double offset) {<\/span><\/span>\n\/\/    if (offset == 0) {<\/span><\/span>\n\/\/        servoAngleSet (0);<\/span><\/span>\n\/\/        motorPwmExpect (speed \/ WHEEL_CIRCUMFERENCE,1.0 \/ WHEEL_CIRCUMFERENCE * speed);<\/span><\/span>\n\/\/        return;<\/span><\/span>\n\/\/    }<\/span><\/span>\n\/\/    double Radius = sqrt(distance * distance + offset * offset) \/ 2 * 1.0 \/ cos (atan (distance \/ offset));<\/span><\/span>\n\/\/    double Delta = (atan (WHEELBASE \/ Radius)) * ((offset > 0) ? -1.0 : 1.0);<\/span><\/span>\n\/\/    Delta *= 180;<\/span><\/span>\n\/\/    Delta \/= 3.1415926;<\/span><\/span>\n\/\/    servoAngleSet (Delta);<\/span><\/span>\n\/\/    double DeltaVPrime = speed * BACK_WHEEL_DISTANCE \/ Radius;<\/span><\/span>\n\/\/    DeltaVPrime \/= 2;<\/span><\/span>\n\/\/    double LeftWheelRPM = (speed + ((offset > 0) ? DeltaVPrime : -DeltaVPrime)) \/ WHEEL_CIRCUMFERENCE;<\/span><\/span>\n\/\/    double RightWheelRPM = 1.0 \/ WHEEL_CIRCUMFERENCE * (speed + ((offset < 0) ? DeltaVPrime : -DeltaVPrime));<\/span><\/span>\n\/\/    motorPwmExpect (LeftWheelRPM,RightWheelRPM);<\/span><\/span>\n\/\/}<\/span><\/span>\n<\/span>\nvoid<\/span> <\/span>movControl<\/span>(<\/span>double<\/span> <\/span>speed<\/span>, <\/span>double<\/span> <\/span>distance<\/span>, <\/span>double<\/span> <\/span>offset<\/span>)<\/span><\/span>\n{<\/span><\/span>\n    <\/span>if<\/span> (offset <\/span>==<\/span> <\/span>0<\/span>) {<\/span><\/span>\n        <\/span>servoAngleSet<\/span> (<\/span>0<\/span>);<\/span><\/span>\n        <\/span>motorPwmExpect<\/span> (speed <\/span>\/<\/span> WHEEL_CIRCUMFERENCE,<\/span>1.0<\/span> <\/span>\/<\/span> WHEEL_CIRCUMFERENCE <\/span>*<\/span> speed);<\/span><\/span>\n        <\/span>return<\/span>;<\/span><\/span>\n    }<\/span><\/span>\n    <\/span>double<\/span> Radius <\/span>=<\/span> (distance <\/span>*<\/span> distance <\/span>+<\/span> offset <\/span>*<\/span> offset) <\/span>\/<\/span> (<\/span>2<\/span> <\/span>*<\/span> offset);<\/span><\/span>\n    <\/span>double<\/span> Delta <\/span>=<\/span> <\/span>atan<\/span>(WHEELBASE <\/span>\/<\/span> Radius);<\/span><\/span>\n    <\/span>servoAngleSet<\/span> (Delta <\/span>*<\/span> <\/span>-<\/span>180.0<\/span> <\/span>\/<\/span> <\/span>3.1415926<\/span>);<\/span><\/span>\n    <\/span>double<\/span> DeltaSpeed <\/span>=<\/span> speed <\/span>*<\/span> BACK_WHEEL_DISTANCE <\/span>\/<\/span> Radius <\/span>\/<\/span> <\/span>2.0<\/span>;<\/span><\/span>\n    <\/span>double<\/span> LeftWheelRPM <\/span>=<\/span> (speed <\/span>+<\/span> DeltaSpeed) <\/span>\/<\/span> WHEEL_CIRCUMFERENCE;<\/span><\/span>\n    <\/span>double<\/span> RightWheelRPM <\/span>=<\/span> <\/span>1.0<\/span> <\/span>\/<\/span> WHEEL_CIRCUMFERENCE <\/span>*<\/span> (speed <\/span>-<\/span> DeltaSpeed);<\/span><\/span>\n    <\/span>motorPwmExpect<\/span> (LeftWheelRPM,RightWheelRPM);<\/span><\/span>\n}<\/span><\/span>\n<\/span>\nvoid<\/span> <\/span>servoAngleSet<\/span>(<\/span>double<\/span> <\/span>angle<\/span>) {<\/span><\/span>\n    angle <\/span>\/=<\/span> <\/span>3<\/span>;<\/span><\/span>\n    angle <\/span>+=<\/span> SERVO_MOTOR_MID;<\/span><\/span>\n    <\/span>double<\/span> angle1 <\/span>=<\/span> angle;<\/span><\/span>\n    <\/span>if<\/span> (angle1 <\/span>><\/span> SERVO_MOTOR_MAX) angle1 <\/span>=<\/span> SERVO_MOTOR_MAX;<\/span><\/span>\n    <\/span>if<\/span> (angle1 <\/span><<\/span> SERVO_MOTOR_MIN) angle1 <\/span>=<\/span> SERVO_MOTOR_MIN;<\/span><\/span>\n    <\/span>pwm_set_duty<\/span>(SERVO_MOTOR, <\/span>SERVO_MOTOR_DUTY<\/span>(angle1));<\/span><\/span>\n}<\/span><\/span>\n<\/span>\nvoid<\/span> <\/span>motorPwmExpect<\/span> (<\/span>double<\/span> <\/span>LPwm<\/span>,<\/span>double<\/span> <\/span>RPwm<\/span>) {<\/span><\/span>\n    MotorPid1.SetSpeed <\/span>=<\/span> LPwm;<\/span><\/span>\n    MotorPid2.SetSpeed <\/span>=<\/span> RPwm;<\/span><\/span>\n}<\/span><\/span>\n<\/span>\nvoid<\/span> <\/span>motorPwmSet<\/span>(<\/span>double<\/span> <\/span>LPwm<\/span>,<\/span>double<\/span> <\/span>RPwm<\/span>) {<\/span><\/span>\n    LPwm <\/span>=<\/span> (LPwm <\/span>><\/span> MOTOR_MAX) <\/span>?<\/span> MOTOR_MAX <\/span>:<\/span> LPwm ;<\/span><\/span>\n    LPwm <\/span>=<\/span> (LPwm <\/span><<\/span> <\/span>-<\/span>MOTOR_MAX) <\/span>?<\/span> <\/span>-<\/span>MOTOR_MAX <\/span>:<\/span> LPwm ;<\/span><\/span>\n    RPwm <\/span>=<\/span> (RPwm <\/span>><\/span> MOTOR_MAX) <\/span>?<\/span> MOTOR_MAX <\/span>:<\/span> RPwm ;<\/span><\/span>\n    RPwm <\/span>=<\/span> (RPwm <\/span><<\/span> <\/span>-<\/span>MOTOR_MAX) <\/span>?<\/span> <\/span>-<\/span>MOTOR_MAX <\/span>:<\/span> RPwm ;<\/span><\/span>\n<\/span>\n#ifdef<\/span> <\/span>MOTOR_CONTROLLER_TYPE_B<\/span><\/span>\n    <\/span>pwm_set_duty<\/span>(MOTOR_L, ((LPwm <\/span>>=<\/span> <\/span>0<\/span>) <\/span>?<\/span> LPwm <\/span>:<\/span> <\/span>-<\/span>LPwm) <\/span>*<\/span> <\/span>100<\/span>);<\/span><\/span>\n    <\/span>pwm_set_duty<\/span>(MOTOR_R, ((RPwm <\/span>>=<\/span> <\/span>0<\/span>) <\/span>?<\/span> RPwm <\/span>:<\/span> <\/span>-<\/span>RPwm) <\/span>*<\/span> <\/span>100<\/span>);<\/span><\/span>\n    <\/span>gpio_set_level<\/span>(MOTOR_L_DIRECTION, (LPwm <\/span>>=<\/span> <\/span>0<\/span>) <\/span>?<\/span> <\/span>0<\/span> <\/span>:<\/span> <\/span>1<\/span>);<\/span><\/span>\n    <\/span>gpio_set_level<\/span>(MOTOR_R_DIRECTION, (RPwm <\/span>>=<\/span> <\/span>0<\/span>) <\/span>?<\/span> <\/span>1<\/span> <\/span>:<\/span> <\/span>0<\/span>);<\/span><\/span>\n#else<\/span><\/span>\n    <\/span>if<\/span> (LPwm <\/span>>=<\/span> <\/span>0<\/span>) {<\/span><\/span>\n            <\/span>pwm_set_duty<\/span>(MOTORL_1, LPwm <\/span>*<\/span> <\/span>100<\/span>);<\/span><\/span>\n            <\/span>pwm_set_duty<\/span>(MOTORL_2, <\/span>0<\/span>);<\/span><\/span>\n        } <\/span>else<\/span> {<\/span><\/span>\n            <\/span>pwm_set_duty<\/span>(MOTORL_1, <\/span>0<\/span>);<\/span><\/span>\n            <\/span>pwm_set_duty<\/span>(MOTORL_2, <\/span>-<\/span>LPwm <\/span>*<\/span> <\/span>100<\/span>);<\/span><\/span>\n        }<\/span><\/span>\n        <\/span>if<\/span> (RPwm <\/span>>=<\/span> <\/span>0<\/span>) {<\/span><\/span>\n            <\/span>pwm_set_duty<\/span>(MOTORR_1, RPwm <\/span>*<\/span> <\/span>100<\/span>);<\/span><\/span>\n            <\/span>pwm_set_duty<\/span>(MOTORR_2, <\/span>0<\/span>);<\/span><\/span>\n        } <\/span>else<\/span> {<\/span><\/span>\n            <\/span>pwm_set_duty<\/span>(MOTORR_1, <\/span>0<\/span>);<\/span><\/span>\n            <\/span>pwm_set_duty<\/span>(MOTORR_2, <\/span>-<\/span>RPwm <\/span>*<\/span> <\/span>100<\/span>);<\/span><\/span>\n        }<\/span><\/span>\n#endif<\/span><\/span>\n}<\/span><\/span>\n<\/span>\n<\/span>\nvoid<\/span> <\/span>pidCalc<\/span>(<\/span>pid_config_t<\/span>*<\/span> <\/span>Pid<\/span>) {<\/span><\/span>\n    Pid<\/span>-><\/span>ErrPrev <\/span>=<\/span> Pid<\/span>-><\/span>ErrLast;<\/span><\/span>\n    Pid<\/span>-><\/span>ErrLast <\/span>=<\/span> Pid<\/span>-><\/span>Err;<\/span><\/span>\n    Pid<\/span>-><\/span>Err <\/span>=<\/span> Pid<\/span>-><\/span>SetSpeed <\/span>-<\/span> Pid<\/span>-><\/span>CurrSpeed;<\/span><\/span>\n    <\/span>double<\/span> Delta <\/span>=<\/span> Pid<\/span>-><\/span>Kd <\/span>*<\/span> (Pid<\/span>-><\/span>Err <\/span>-<\/span> Pid<\/span>-><\/span>ErrLast)<\/span><\/span>\n                   <\/span>+<\/span> Pid<\/span>-><\/span>Kp <\/span>*<\/span> Pid<\/span>-><\/span>Err<\/span><\/span>\n                   <\/span>+<\/span> Pid<\/span>-><\/span>Ki <\/span>*<\/span> (Pid<\/span>-><\/span>Err <\/span>-<\/span> <\/span>2<\/span> <\/span>*<\/span> Pid<\/span>-><\/span>ErrLast <\/span>+<\/span> Pid<\/span>-><\/span>ErrPrev);<\/span><\/span>\n    Pid<\/span>-><\/span>ActualSpeed <\/span>+=<\/span> Delta;<\/span><\/span>\n    <\/span>if<\/span> (Pid<\/span>-><\/span>ActualSpeed <\/span>><\/span> MAX_RPM)<\/span><\/span>\n        Pid<\/span>-><\/span>ActualSpeed <\/span>=<\/span> MAX_RPM;<\/span><\/span>\n    <\/span>if<\/span> (Pid<\/span>-><\/span>ActualSpeed <\/span><<\/span> <\/span>-<\/span>MAX_RPM)<\/span><\/span>\n        Pid<\/span>-><\/span>ActualSpeed <\/span>=<\/span> <\/span>-<\/span>MAX_RPM;<\/span><\/span>\n}<\/span><\/span>\n<\/span>\nIFX_INTERRUPT<\/span>(cc60_pit_ch0_isr, <\/span>0<\/span>, CCU6_0_CH0_ISR_PRIORITY) {<\/span><\/span>\n    <\/span>interrupt_global_enable<\/span>(<\/span>0<\/span>);<\/span><\/span>\n    <\/span>pit_clear_flag<\/span>(CCU60_CH0);<\/span><\/span>\n<\/span>\n    Encoder1Count <\/span>=<\/span> <\/span>encoder_get_count<\/span>(ENCODER1_DIR);<\/span><\/span>\n    Encoder2Count <\/span>=<\/span> <\/span>encoder_get_count<\/span>(ENCODER2_DIR);<\/span><\/span>\n<\/span>\n    <\/span>encoder_clear_count<\/span>(ENCODER1_DIR);<\/span><\/span>\n    <\/span>encoder_clear_count<\/span>(ENCODER2_DIR);<\/span><\/span>\n<\/span>\n    Encoder1RPM <\/span>=<\/span> <\/span>1.0<\/span> <\/span>*<\/span> Encoder1Count <\/span>\/<\/span> GEAR_RATIO <\/span>*<\/span> (<\/span>1000<\/span>\/<\/span>PID_MS);<\/span><\/span>\n    Encoder2RPM <\/span>=<\/span> <\/span>1.0<\/span> <\/span>*<\/span> Encoder2Count <\/span>\/<\/span> GEAR_RATIO <\/span>*<\/span> (<\/span>1000<\/span>\/<\/span>PID_MS);<\/span><\/span>\n<\/span>\n    MotorPid1.CurrSpeed <\/span>=<\/span> Encoder1RPM;<\/span><\/span>\n    MotorPid2.CurrSpeed <\/span>=<\/span> <\/span>-<\/span>Encoder2RPM;<\/span><\/span>\n<\/span>\n    <\/span>pidCalc<\/span>(<\/span>&<\/span>MotorPid1);<\/span><\/span>\n    <\/span>pidCalc<\/span>(<\/span>&<\/span>MotorPid2);<\/span><\/span>\n<\/span>\n    <\/span>motorPwmSet<\/span>(MotorPid1.ActualSpeed,MotorPid2.ActualSpeed);<\/span><\/span>\n}<\/span><\/span>\n<\/span><\/code><\/pre><\/div>\n\n\n\n
h<\/h3>\n\n\n\n
C<\/span><\/path><\/path><\/svg><\/span>
#ifndef<\/span> <\/span>MOTOR_H<\/span><\/span>\n#define<\/span> <\/span>MOTOR_H<\/span><\/span>\n<\/span>\nvoid<\/span> <\/span>motorPwmInit<\/span>();<\/span><\/span>\nvoid<\/span> <\/span>movControl<\/span>(<\/span>double<\/span> <\/span>speed<\/span>, <\/span>double<\/span> <\/span>distance<\/span>, <\/span>double<\/span> <\/span>offset<\/span>);<\/span><\/span>\n<\/span>\n#endif<\/span><\/span>\n<\/span><\/code><\/pre><\/div>\n","protected":false},"excerpt":{"rendered":"
\u7a0b\u5e8f c h<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"emotion":"","emotion_color":"","title_style":"","license":"","footnotes":""},"categories":[3],"tags":[],"class_list":["post-228","post","type-post","status-publish","format-standard","hentry","category-code"],"_links":{"self":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts\/228","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=228"}],"version-history":[{"count":3,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts\/228\/revisions"}],"predecessor-version":[{"id":339,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=\/wp\/v2\/posts\/228\/revisions\/339"}],"wp:attachment":[{"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=228"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=228"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/yin.nnneri.me\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=228"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}