基于STM32F10x與VS1838紅外的自動航行小船源程序

ID:354602 · 發表于 2019-6-6 21:48

#include "control.h"
//#include "usart.h"
#include "delay.h"

typedef struct
{
      int set_angle;//設定角度
      int actual_angle;//實際角度
      int error;//偏差
      int error_next;//上一個偏差
      int error_last;//上上一個偏差
      float kp,ki,kd;//定義比例，積分，微分參數
}PID;

u8 hw0_state = 0,hw22_5_state = 0,hw45_state = 0,hw67_5_state = 0,hw90_state = 0;
u8 hw112_5_state = 0,hw135_state = 0,hw157_5_state = 0,hw180_state = 0;

extern u8  TIM2CH1_CAPTURE_STA;       //輸入捕獲狀態
extern u16       TIM2CH1_CAPTURE_VAL;       //輸入捕獲值
extern u8  TIM2CH2_CAPTURE_STA;       //輸入捕獲狀態
extern u16       TIM2CH2_CAPTURE_VAL;       //輸入捕獲值
extern u8  TIM2CH3_CAPTURE_STA;       //輸入捕獲狀態
extern u16       TIM2CH3_CAPTURE_VAL;       //輸入捕獲值
extern u8  TIM3CH1_CAPTURE_STA;       //輸入捕獲狀態
extern u16       TIM3CH1_CAPTURE_VAL;       //輸入捕獲值
extern u8  TIM3CH2_CAPTURE_STA;       //輸入捕獲狀態
extern u16       TIM3CH2_CAPTURE_VAL;       //輸入捕獲值
extern u8  TIM3CH3_CAPTURE_STA;       //輸入捕獲狀態
extern u16       TIM3CH3_CAPTURE_VAL;       //輸入捕獲值
extern u8  TIM3CH4_CAPTURE_STA;       //輸入捕獲狀態
extern u16       TIM3CH4_CAPTURE_VAL;       //輸入捕獲值
extern u8  TIM4CH3_CAPTURE_STA;       //輸入捕獲狀態
extern u16       TIM4CH3_CAPTURE_VAL;       //輸入捕獲值
extern u8  TIM4CH4_CAPTURE_STA;       //輸入捕獲狀態
extern u16       TIM4CH4_CAPTURE_VAL;       //輸入捕獲值

u16 N1=0,N2=0;//N1左馬達 N2右馬達
signed int turn_angle = 0;

PID altpid;

void driver_init(void)
{

//       GPIO_InitTypeDef GPIO_InitStructure2;
//       GPIO_InitTypeDef GPIO_InitStructure3;
//

//
//       GPIO_InitStructure1.GPIO_Mode=GPIO_Mode_IPD;//設置上拉輸入
//       GPIO_InitStructure1.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7;
//       GPIO_InitStructure1.GPIO_Speed=GPIO_Speed_50MHz;
//       GPIO_Init(GPIOA,&GPIO_InitStructure1);
//       GPIO_SetBits(GPIOA,GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7);       //初始化GPIOA1
//
//       GPIO_InitStructure2.GPIO_Mode=GPIO_Mode_IPD;//設置上拉輸入
//       GPIO_InitStructure2.GPIO_Pin=GPIO_Pin_15;
//       GPIO_InitStructure2.GPIO_Speed=GPIO_Speed_50MHz;
//       GPIO_Init(GPIOC,&GPIO_InitStructure2);
//       GPIO_SetBits(GPIOC,GPIO_Pin_15);
//
//       GPIO_InitStructure2.GPIO_Mode=GPIO_Mode_Out_PP;//設置上拉輸入
//       GPIO_InitStructure2.GPIO_Pin=GPIO_Pin_14;
//       GPIO_InitStructure2.GPIO_Speed=GPIO_Speed_50MHz;
//       GPIO_Init(GPIOC,&GPIO_InitStructure2);
//       GPIO_SetBits(GPIOC,GPIO_Pin_14);
      /*
PA 2345
*/
      GPIO_InitTypeDef GPIO_InitStructure;

      RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);

      GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;//設置推挽輸出
      GPIO_InitStructure.GPIO_Pin=GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5;
      GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;

      GPIO_Init(GPIOA,&GPIO_InitStructure);
      //GPIO_ResetBits(GPIOA,GPIO_Pin_7);//xia拉led
}

float Fabs(float j)
{
      float a;

      if(j<0)
                     a = -j;
      else
            a = j;
      return a;
}

float limit(float value,float min,float max)
{
      float value_new;

      if(value > max)
            value_new = max;
      else if(value < min)
            value_new = min;
      else
            value_new = value;

      return value_new;
}

void sys_init()
{
//       hw_high_GPIOA;
//       hw_high_GPIOC;//紅外上電默認不接收到信號

      ENA_ENABLE;
      ENB_ENABLE;//默認使能

      LEFT_MOTOR_1_HIGH;
      LEFT_MOTOR_2_LOW;

      RIGHT_MOTOR_1_HIGH;
      RIGHT_MOTOR_2_LOW;

}
void left_right_pwm(u16 n1,u16 n2)
{
      LEFT_MOTOR_1_HIGH;
      LEFT_MOTOR_2_LOW;

      RIGHT_MOTOR_1_HIGH;
      RIGHT_MOTOR_2_LOW;

      TIM_SetCompare1(TIM4,n1);//left
      TIM_SetCompare2(TIM4,n2);
}

void pid_realise_init(float k_p,float k_i,float k_d)//增量式PID初始化
{
altpid.set_angle = 0;
altpid.actual_angle = 0;
altpid.error = 0;
altpid.error_next = 0;
altpid.error_last = 0;
altpid.kp = k_p;
altpid.ki = k_i;
altpid.kd = k_d;
}

/*
PA 0/1  PB 0/1/4/5/8/9/10
*/
void Catch_hw_state()
{
      if(TIM2CH1_CAPTURE_STA&0x80)//捕獲到一個下降沿、、
            {

                     hw0_state = 1;//說明0度紅外收到信號，便于給權重
                     TIM2CH1_CAPTURE_STA=0;//開啟下一次捕獲
            }
            else hw0_state = 0;

            if(TIM2CH2_CAPTURE_STA&0x80)//捕獲到一個下降沿、、
            {

                     hw22_5_state = 1;//說明0度紅外收到信號，便于給權重
                     TIM2CH2_CAPTURE_STA=0;//開啟下一次捕獲
            }
            else hw22_5_state = 0;

            if(TIM3CH3_CAPTURE_STA&0x80)//捕獲到一個下降沿
            {
                     hw45_state = 1;//說明0度紅外收到信號，便于給權重
                     TIM3CH3_CAPTURE_STA=0;//開啟下一次捕獲
            }
            else hw45_state = 0;

            if(TIM3CH4_CAPTURE_STA&0x80)//捕獲到一個下降沿
            {
                     hw67_5_state = 1;//說明0度紅外收到信號，便于給權重
                     TIM3CH4_CAPTURE_STA=0;//開啟下一次捕獲
            }
            else hw67_5_state = 0;

            if(TIM3CH1_CAPTURE_STA&0x80)//捕獲到一個下降沿
            {
                     hw90_state = 1;//說明0度紅外收到信號，便于給權重
                     TIM3CH1_CAPTURE_STA=0;//開啟下一次捕獲
            }
            else hw90_state = 0;

            if(TIM3CH2_CAPTURE_STA&0x80)//捕獲到一個下降沿、、
            {
                     hw112_5_state = 1;//說明0度紅外收到信號，便于給權重
                     TIM3CH2_CAPTURE_STA=0;//開啟下一次捕獲
            }
            else hw112_5_state = 0;

            if(TIM4CH3_CAPTURE_STA&0x80)//捕獲到一個下降沿、、
            {
                     hw135_state = 1;//說明0度紅外收到信號，便于給權重
                     TIM4CH3_CAPTURE_STA=0;//開啟下一次捕獲
            }
            else hw135_state = 0;

            if(TIM4CH4_CAPTURE_STA&0x80)//捕獲到一個下降沿
            {
                     hw157_5_state = 1;//說明0度紅外收到信號，便于給權重
                     TIM4CH4_CAPTURE_STA=0;//開啟下一次捕獲
            }
            else hw157_5_state = 0;

            if(TIM2CH3_CAPTURE_STA&0x80)//捕獲到一個下降沿、、
            {
                     hw180_state = 1;//說明0度紅外收到信號，便于給權重
                     TIM2CH3_CAPTURE_STA=0;//開啟下一次捕獲
            }
            else hw180_state = 0;
}

int Calculate(void)
{
      unsigned int  weight_num = 0;//記錄紅外接收到的個數
      static int times = 0,turn_angle_num;//濾波
      //Catch_hw_state();// CATCH AGAIN
      weight_num =  hw0_state
                                                      + hw22_5_state
                                                      + hw45_state
                                                      + hw67_5_state
                                                      + hw90_state
                                                      + hw112_5_state
                                                      + hw135_state
                                                      + hw157_5_state
                                                      + hw180_state;
      if(weight_num==0)//全沒接收到
      {
            times = 0;
            turn_angle_num = 0;
      }
      else
      {
            turn_angle = (  hw0_weight*hw0_state    //50
                                                                     + hw22_5_weight*hw22_5_state//20*
                                                                     + hw45_weight*hw45_state//30*
                                                                     + hw67_5_weight*hw67_5_state//40*
                                                                     + hw90_weight*hw90_state//50*
                                                                     + hw112_5_weight*hw112_5_state//60*
                                                                     + hw135_weight*hw135_state//70*
                                                                     + hw157_5_weight*hw157_5_state//80*
                                                                     + hw180_weight*hw180_state
                                                                     )/weight_num-50;
            turn_angle_num+=turn_angle;//50
            times++;
            if(times==5)
            {
                     times = 0;
                     turn_angle = turn_angle_num/5;
                     turn_angle_num=0;
            }
      }
      return turn_angle;
}

void pid_realise_control(int angle)       // 增量式
{
            float increment_speed = 0.0;//increment_speed_abs;//增量
            u16 increment_speed_fab;
            //Catch_hw_state();// CATCH AGAIN
            altpid.actual_angle = Calculate();//反饋值

altpid.set_angle = angle;//設置目標速度
altpid.error = altpid.set_angle - altpid.actual_angle;//set 0 error>0 left
                                                                     //  0.2                0.01    0.2
increment_speed = altpid.kp*(altpid.error-altpid.error_next)+altpid.ki*altpid.error+altpid.kd*(altpid.error-2*altpid.error_next+altpid.error_last);//增量計算公式
altpid.error_last = altpid.error_next;//上上次誤差
altpid.error_next = altpid.error;//上一次誤差

            increment_speed_fab =  100*Fabs(limit(increment_speed,-10,10));//限幅

      if(altpid.error > 0)//turn left
      {
            N1 = angle_turn_N1 + increment_speed_fab;
            N2 = angle_turn_N2;
            //left_right_pwm(N1,N2);
            printf("N1=%d\r\n",N1);
            printf("N2=%d\r\n",N2);
      }
      if(altpid.error < 0)//turn right
      {
            N1 = angle_turn_N1;
            N2 = angle_turn_N2 + increment_speed_fab;
            //left_right_pwm(N1,N2);
            printf("N1=%d\r\n",N1);
            printf("N2=%d\r\n",N2);
      }
      else
      {
            N1 = angle_turn_N1;
            N2 = angle_turn_N2;
      }




//
      printf("altpid.actual_angle=%d\r\n",altpid.actual_angle);
      printf("altpid.error=%d\r\n",altpid.error);
      printf("increment_speed=%f\r\n",increment_speed);
      printf("altpid.error_last=%d\r\n",altpid.error_last);
      printf("altpid.error_next=%d\r\n",altpid.error_next);
      printf("increment_speed_fab=%d\r\n",increment_speed_fab);
}

//void pid_control()
//{
//       pid_realise_control(0);
//       left_right_pwm(N1,N2);
//       delay_ms(200);
//
//}

全部程序51hei下載地址(缺少原理圖)：

32_boat.7z (200.45 KB, 下載次數: 12)

ID:1 · 發表于 2019-6-7 02:03

本帖需要重新編輯補全電路原理圖,源碼,詳細說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)

ID:516571 · 發表于 2019-6-8 09:13

可以分享下原理圖和工程文件嗎，非常感謝

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

基于STM32F10x與VS1838紅外的自動航行小船源程序