2022_gorobo_Ateam / 2022_Ateam_MOTORprogram

足回り用プログラム(修正版)

Dependencies:   SBDBT arrc_mbed BNO055

PIDco.hpp

Committer:
kazumayamanaka
Date:
2022-01-28
Revision:
5:e189d55ef292
Parent:
0:a1238c4cd105

File content as of revision 5:e189d55ef292:

#ifndef PID_CO_H
#define PID_CO_H
#include "mbed.h"

#define Kp 0.0001
#define Ki 0.0000014
#define Kd 0.0000001
#define co (108 * 3.1415926535897) / 500


class PIDco{
    
    public://use in main.cpp
        PIDco();
        double obt_spd();
        void pass_val(double PULSE,double TARGET);
        void wheel_ctl(PinName PIN_A,PinName PIN_B);
    
    private:
        void cal_spd();
        void cal_Error();
        void renew_Error();
        void cal_Pval();
        void cal_Ival();
        void cal_Dval();
        void cal_pwm();
        
        double spd;
        double Target,Error_a,Error_b;
        double Pval,Ival,Dval,Pwm;
        double Integral,dt;
        double diff,pulse_a,pulse_b;
        double v1p_set,v1m_set;
};

#endif