pid.h摄像头 循迹舵机

95 篇文章 5 订阅
/*
 * PID.c
 *
 *  Created on: 2021��12��10��
 *      Author: yue
 */

#include "PID.h"
#include "Filter.h" //�˲��㷨
#include "Steer.h"  //���
#include "Motor.h"  //���
#include "SEEKFREE_18TFT.h"

SteerPID SteerK;            //���PID����
MotorPID MotorK_L,MotorK_R; //���PID����

/********************************************************************************************
 ** ��������: ����PID�����ĸ�ֵ��ʼ��
 ** ��    ��: SteerPID *SteerK
 **           MotorPID *MotorK
 ** �� �� ֵ: ��
 ** ��    ��: LJF
 *********************************************************************************************/
void PID_init(SteerPID *SteerK,MotorPID *MotorK_L,MotorPID *MotorK_R)
{
    SteerK->P=16.25;SteerK->I=0;SteerK->D=37.5;         //��ʼ�������PID����   //У������19.25 5   //����������14.25 30
    MotorK_L->P=180;MotorK_L->I=0.45;MotorK_L->D=0;     //��ʼ�������PID����   //У������80 0.5    //����������180 0.45
    MotorK_R->P=180;MotorK_R->I=0.45;MotorK_R->D=0;     //��ʼ�������PID����   //У������80 0.5    //����������180 0.45
}

/*
 *******************************************************************************************
 ** ��������: ����ƫ��������PWM
 ** ��    ��: float SlopeBias:    ��С�ع鷽�̵ó�����ƫ��
 **           SteerPID K:         ����PID�Ķ��PID����
 ** �� �� ֵ: �������PWM
 ** ��    ��: LJF
 ** ע    �⣺ƫ���Ǵ��������Ǵ��������߽����ٸ����Ǹ�������ƫ�
 **           ���س�ȥ��PWM���ܺ�С�������������PWM�������Ż��ǰ������ȥ����?
 **           Bias�����������ﴦ�����Ǵ�����֮ǰ������������һ�������й�����
 ********************************************************************************************
 */
uint32 Steer_Position_PID(float bias,float slope,SteerPID K)
{
    static float last_bias_slope;
    float bias_slope=0;
    int PWM;
    //�����ں�
    slope=FirstOrderLagFilter_Slope(slope); //��б�ʵ�ͨ�˲�
//    bias_slope=BIAS_SLOPE_P*bias+(1-BIAS_SLOPE_P)*(3*slope);
    bias_slope=0.9*bias+slope;
//    printf("%f,%f\n",bias,slope);
    //PID����
    PWM=(int)(K.P*bias_slope+K.D*(bias_slope-last_bias_slope));
    //������һ��ƫ��
    last_bias_slope=FirstOrderLagFilter(bias_slope);   //����һ�ε�ƫ���ͨ�˲�

    return STEER_MID+PWM;
}

/*
 *******************************************************************************************
 ** ��������: �������ʽ�ٶ�PI�����������ݱ�������ֵ�����Ͻ���������ٶ�   //����
 ** ��    ��: left_encoder��������������ֵ
 **           left_target ������Ŀ���ٶ�
 **           K           : ���PID����
 ** �� �� ֵ: �������PWM
 ** ��    ��: WBN
 ** ע    �⣺ʹ������ʽPI���Ƶ��ŵ㣺
 **           1.��ʽ��û���ۼӵĹ��̣�������ֻ������β���ֵ�йأ������ײ���������
 **           2.����������������仯���������и��õ��ݴ�
 ********************************************************************************************
 */
int Speed_PI_Left(int16 left_encoder,int16 left_target,MotorPID K)
{
    static int bias,last_bias,PWM;

    bias=left_target-left_encoder;              //�����ǰƫ��
    PWM+=(int)(K.P*(bias-last_bias)+K.I*bias);  //����ʽPID�����ѽ��ֱ�ӵ�������һ�ε�PWM��
    last_bias=bias;           //������һ��ƫ��

    //PID����޷�����ֹ���ڵ����PID������ͬ�����µ�������ջٵ�����
    //ע�⣺��PID����޷���ᵼ����Ӧ����
//    if(PWM>MOTOR_PWM_MAX)
//    {
//        PWM=MOTOR_PWM_MAX;
//    }
//    else if(PWM<-MOTOR_PWM_MAX)
//    {
//        PWM=-MOTOR_PWM_MAX;
//    }

    return PWM;         //���ؿ���ֱ�Ӹ�ֵ�������PWM
}

/*
 *******************************************************************************************
 ** ��������: �������ʽ�ٶ�PI�����������ݱ�������ֵ�����Ͻ���������ٶ�   //�ҵ��
 ** ��    ��: left_encoder��������������ֵ
 **           left_target ������Ŀ���ٶ�
 **           K           : ���PID����
 ** �� �� ֵ: �������PWM
 ** ��    ��: WBN
 ** ע    �⣺ʹ������ʽPI���Ƶ��ŵ㣺
 **           1.��ʽ��û���ۼӵĹ��̣�������ֻ������β���ֵ�йأ������ײ���������
 **           2.����������������仯���������и��õ��ݴ�
 ********************************************************************************************
 */
int Speed_PI_Right(int16 right_encoder,int16 right_target,MotorPID K)
{
    static int bias,last_bias,PWM;

    bias=right_target-right_encoder;              //�����ǰƫ��
    PWM+=(int)(K.P*(bias-last_bias)+K.I*bias);  //����ʽPID�����ѽ��ֱ�ӵ�������һ�ε�PWM��
    last_bias=bias;           //������һ��ƫ��

    //PID����޷�����ֹ���ڵ����PID������ͬ�����µ�������ջٵ�����
    //ע�⣺��PID����޷���ᵼ����Ӧ����
//    if(PWM>MOTOR_PWM_MAX)
//    {
//        PWM=MOTOR_PWM_MAX;
//    }
//    else if(PWM<-MOTOR_PWM_MAX)
//    {
//        PWM=-MOTOR_PWM_MAX;
//    }

    return PWM;         //���ؿ���ֱ�Ӹ�ֵ�������PWM
}

pid.h

/*
 * PID.h
 *
 *  Created on: 2021��12��10��
 *      Author: yue
 */

#ifndef CODE_PID_H_
#define CODE_PID_H_

#include "common.h" //������������

#define BIAS_SLOPE_P 0.7

//���PID���ƵĽṹ�壬���ڷֶο���PID��ͼ��ʶ������·��֮��PID�IJ����ͷ��ͱ仯
typedef struct SteerPID
{
    float P;
    float I;
    float D;
}SteerPID;
//���PID���ƵĽṹ��
typedef struct MotorPID
{
    float P;
    float I;
    float D;
}MotorPID;

extern SteerPID SteerK;     //�����ȫ�ֱ���
extern MotorPID MotorK_L,MotorK_R;     //�����ȫ�ֱ���
extern int16 base_speed;    //�����ٶȵ�ȫ�ֱ���
extern uint32 SteerPWM;     //���PWM

//���ﻹҪд��PID��ʼ���ĺ����������������
void PID_init(SteerPID *SteerK,MotorPID *MotorK_L,MotorPID *MotorK_R);    //����PID�����ĸ�ֵ��ʼ��
uint32 Steer_Position_PID(float bias,float slope,SteerPID K);                    //���λ��ʽPID���ƣ����÷ֶ�ʽPID����
int Speed_PI_Left(int16 left_encoder,int16 left_target,MotorPID K);       //�������ʽPI���� //����
int Speed_PI_Right(int16 right_encoder,int16 right_target,MotorPID K);    //�������ʽPI����  //�ҵ��

#endif /* CODE_PID_H_ */

在IDEA控制台中,"------欢迎使用------请选择需要操作的功能"这行输出是由代码中的日志语句所产生的。根据引用中的代码片段,当点击按钮2时,会执行一个循环读取输入流的操作,并将读取到的字节转换成字符串。具体来说,代码中的inputStream表示输入流,而read方法则用于读取字节,并返回读取的字节数。如果返回值不等于-1,说明还有字节可读,此时将读取到的字节通过new String方法转换成字符串,并指定字符集为gbk。最后,通过Log.i方法将字符串打印出来。因此,"------欢迎使用------请选择需要操作的功能"这行输出很有可能是代码中的一个提示信息。至于1.登录和2.退出选项,用户可以根据需要选择相应的功能。<span class="em">1</span><span class="em">2</span><span class="em">3</span> #### 引用[.reference_title] - *1* *2* [Java-原生IO通览](https://blog.csdn.net/qq_42711920/article/details/110913607)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v93^chatsearchT3_1"}}] [.reference_item style="max-width: 50%"] - *3* [解决IO流获取数据为中文时,为����乱码](https://blog.csdn.net/LiuxXn/article/details/103164301)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra":{"utm_source":"vip_chatgpt_common_search_pc_result","utm_medium":"distribute.pc_search_result.none-task-cask-2~all~insert_cask~default-1-null.142^v93^chatsearchT3_1"}}] [.reference_item style="max-width: 50%"] [ .reference_list ]
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值