logzhan
/
RobotHardware-UESTC
1
0
Fork 2
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity
RobotHardware-UESTC/Hardware/Firmware/GeekRobotTiny_Firmware v1.0/HARDWARE/MOTO/car.c

98 lines
3.2 KiB
C
Raw Normal View History

上传机器人主控板和直流电机驱动
2023-12-09 21:03:01 +08:00
/******************** (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
}
}