forked from logzhan/RobotHardware-UESTC
98 lines
3.2 KiB
C
98 lines
3.2 KiB
C
|
/******************** (C) COPYRIGHT 2023 GeekRebot *****************************
|
|||
|
|
* File Name : car.c
|
|||
|
|
* Current Version : V1.0 & ST 3.5.0
|
|||
|
|
* Author : zhanli 719901725@qq.com
|
|||
|
|
* Date of Issued : 2023.04.06 zhanli: Create
|
|||
|
|
* Comments : GeekRebot <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
********************************************************************************/
|
|||
|
|
|
|||
|
|
#include "car.h"
|
|||
|
|
#include "pid.h"
|
|||
|
|
|
|||
|
|
int Encoder_Left,Encoder_Right; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
int Moto_Left,Moto_Right;
|
|||
|
|
int para_L,para_R;//<2F><><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ú<EFBFBD><C3BA><EFBFBD>Ҫ
|
|||
|
|
int SetPoint=70;//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>ֵ<EFBFBD><D6B5>λRPM<50><4D><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> = setopint*<2A><><EFBFBD><EFBFBD><EFBFBD>ܳ<EFBFBD> = setpoint*3.14*0.065(<28><><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>65mm) <20><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>m/min һ<><D2BB>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>ٺ<EFBFBD><D9BA><EFBFBD><EFBFBD>ٵĹ<D9B5>ϵ
|
|||
|
|
|
|||
|
|
|
|||
|
|
//ʹ<>ü<EFBFBD><C3BC>ٱ<EFBFBD><D9B1><EFBFBD>1<EFBFBD><31>120<32>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD>䡣<EFBFBD><E4A1A3><EFBFBD><EFBFBD>ʵ<EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC>ٱȵ<D9B1><C8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><DEB8><EFBFBD><EFBFBD>²<EFBFBD><C2B2><EFBFBD>
|
|||
|
|
//6240=13*4*120<32><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>13<31>ߣ<EFBFBD>STM32<33><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ 4<><34>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31>120
|
|||
|
|
//2496=13*4*48<34><38><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>13<31>ߣ<EFBFBD>STM32<33><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ 4<><34>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31>48
|
|||
|
|
#define SetPointL SetPoint*2496/600 //<2F><><EFBFBD><EFBFBD><EFBFBD>ɱ<EFBFBD><C9B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>pid<69><64><EFBFBD>Ƶ<EFBFBD><C6B5>DZ<EFBFBD><C7B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
#define SetPointR SetPoint*2496/600 //<2F><><EFBFBD><EFBFBD><EFBFBD>ɱ<EFBFBD><C9B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>pid<69><64><EFBFBD>Ƶ<EFBFBD><C6B5>DZ<EFBFBD><C7B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>
|
|||
|
|
u32 temp1,temp2;
|
|||
|
|
float temp3,temp4;
|
|||
|
|
char set_speed[5];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾС<CABE><D0A1>
|
|||
|
|
char speed[5];//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾС<CABE><D0A1>
|
|||
|
|
//Time1<65><31>ʱ<EFBFBD><CAB1>1<EFBFBD>жϷ<D0B6><CFB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//100ms<6D><73>ʱ
|
|||
|
|
|
|||
|
|
|
|||
|
|
void TIM6_IRQHandler(void)
|
|||
|
|
{
|
|||
|
|
// <20><><EFBFBD>ﶨʱ<EFB6A8><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(TIM_GetFlagStatus(TIM6, TIM_IT_Update) != RESET)
|
|||
|
|
{
|
|||
|
|
TIM_ClearITPendingBit(TIM6, TIM_FLAG_Update); // <20><><EFBFBD>ж<EFBFBD>
|
|||
|
|
|
|||
|
|
LED_Flash(5); // 500ms<6D><73>˸һ<CBB8><D2BB>
|
|||
|
|
Encoder_Left = Read_Encoder(2) - 30000; // <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
Encoder_Right = Read_Encoder(4) - 30000; // <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
//С<><D0A1><EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
|
|||
|
|
//<2F><><EFBFBD>㳵<EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>ֺ<EFBFBD><D6BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȺͳ<C8BA><CDB3><EFBFBD>2 <20><>λ m/min
|
|||
|
|
temp3 = (Encoder_Right + Encoder_Left) / 2*600/2496 *0.065*3.1415;
|
|||
|
|
sprintf(speed,"%2.2f",temp3);
|
|||
|
|
OLED_ShowString(48,0, (u8*)speed,16); //OLED<45><44>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
if(Encoder_Left < 0)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
|
{
|
|||
|
|
Encoder_Left = -Encoder_Left;
|
|||
|
|
}
|
|||
|
|
if(Encoder_Right < 0)
|
|||
|
|
{
|
|||
|
|
Encoder_Right = -Encoder_Right;
|
|||
|
|
}
|
|||
|
|
para_L=PID_Calc_Left(Encoder_Left,SetPointL); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>ʽPID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
para_R=PID_Calc_Right(Encoder_Right,SetPointR); // <20>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>ʽPID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
|
|||
|
|
if((para_L<-3)||(para_L>3)) // <20><><EFBFBD><EFBFBD> PID <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>СʱƵ<CAB1><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
Moto_Left +=para_L;
|
|||
|
|
}
|
|||
|
|
if((para_R<-3)||(para_R>3)) // <20><><EFBFBD><EFBFBD> PID <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>СʱƵ<CAB1><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
Moto_Right += para_R;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(Moto_Left>3500)Moto_Left=3000; //<2F><EFBFBD> <20><>ֹ<EFBFBD>ջٵ<D5BB><D9B5><EFBFBD>
|
|||
|
|
if(Moto_Right>3500)Moto_Right=3000; //<2F><EFBFBD>
|
|||
|
|
|
|||
|
|
Moto_Left=1500;
|
|||
|
|
Moto_Right=1500;
|
|||
|
|
TIM3->CCR3=Moto_Left; //<2F><><EFBFBD><EFBFBD>pwm
|
|||
|
|
TIM3->CCR4=Moto_Right; //<2F><><EFBFBD><EFBFBD>pwm
|
|||
|
|
|
|||
|
|
//<2F><>ʾpid<69><64><EFBFBD><EFBFBD>
|
|||
|
|
showPID();
|
|||
|
|
//<2F><>ʾ<EFBFBD><CABE><EFBFBD>ֺ<EFBFBD><D6BA><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA> <20><>λRPM
|
|||
|
|
temp1 = Encoder_Left*600/2496;
|
|||
|
|
OLED_ShowNum(56,2,temp1,3,12); //<2F><><EFBFBD><EFBFBD>ʵ<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA> <20><>λRPM
|
|||
|
|
temp2 = Encoder_Right*600/2496;
|
|||
|
|
OLED_ShowNum(56,3,temp2,3,12); //<2F><><EFBFBD><EFBFBD>ʵ<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA> <20><>λRPM
|
|||
|
|
|
|||
|
|
//<2F>趨ֵ<E8B6A8><D6B5>ʾ
|
|||
|
|
OLED_ShowNum(48,6,SetPoint,3,12); //<2F><>ʾ<EFBFBD>趨<EFBFBD>ij<EFBFBD><C4B3><EFBFBD>ת<EFBFBD><D7AA> <20><>λRPM
|
|||
|
|
//temp4 = SetPoint*3.1415*0.065; //<2F><><EFBFBD><EFBFBD><EFBFBD>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
|
|||
|
|
//sprintf(set_speed,"%2.2f",temp4);
|
|||
|
|
//OLED_ShowString(48,7, set_speed,12); //<2F>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> <20><>λm/min
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|