lihang/Car
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Car/my_lib/roadway_check.c

539 lines
10 KiB
C
Raw Normal View History

first git
2025-04-18 16:22:24 +08:00
#include "stm32f4xx.h"
#include "CanP_Hostcom.h"
#include "delay.h"
#include "roadway_check.h"
#include "cba.h"
#include "Timer.h"
#include "Rc522.h"
#include "drive.h"
uint8_t wheel_L_Flag =0,wheel_R_Flag = 0;
uint8_t wheel_Nav_Flag = 0;
uint8_t Go_Flag = 0,Back_Flag = 0;
uint8_t Track_Flag = 0;
uint8_t RFID_Flag = 0; ////RFID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ʮ<><CAAE>·<EFBFBD><C2B7>
uint8_t SZLK_Flag = 0;//RFID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ʮ<><CAAE>·<EFBFBD><C2B7>
uint8_t Trackk_Flag = 0;////////////
uint8_t rfidxh_Flag = 0;////////////
uint8_t Line_Flag = 0;
uint16_t count = 0;
uint8_t Stop_Flag = 0;
int LSpeed = 0,RSpeed = 0;
int Car_Spend = 0;
uint16_t temp_MP = 0;
uint16_t temp_Nav = 0;
uint32_t Wheel_flag = 0;
//void Trackk_Correct(uint8_t gd);///////////////////////////
void Track_Correct(uint8_t gd);
void Back_Track(uint8_t gd);
//_________________________________________________________
int16_t Roadway_cmp;
extern int16_t CanHost_Mp;
/*
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD><EFBFBD>
**/
void Roadway_mp_syn(void)
{
Roadway_cmp = CanHost_Mp;
}
/*
<EFBFBD><EFBFBD><EFBFBD>̻<EFBFBD>ȡ
**/
uint16_t Roadway_mp_Get(void)
{
uint32_t ct;
if(CanHost_Mp > Roadway_cmp)
ct = CanHost_Mp - Roadway_cmp;
else
ct = Roadway_cmp - CanHost_Mp;
if(ct > 0x8000)
ct = 0xffff - ct;
return ct;
}
uint16_t Roadway_Navig;
extern uint16_t CanHost_Navig;
/*
<EFBFBD>Ƕ<EFBFBD>ͬ<EFBFBD><EFBFBD>
**/
void Roadway_nav_syn(void)
{
Roadway_Navig = CanHost_Navig;
}
/*
<EFBFBD><EFBFBD>ȡ<EFBFBD>ǶȲ<EFBFBD>ֵ
**/
uint16_t Roadway_nav_Get(void)
{
uint16_t ct;
if(CanHost_Navig > Roadway_Navig)
ct = CanHost_Navig - Roadway_Navig;
else
ct = Roadway_Navig - CanHost_Navig;
while(ct >= 36000)
ct -= 36000;
return ct;
}
//_______________________________________________________________
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־λ<D6BE><CEBB><EFBFBD><EFBFBD>*/
void Roadway_Flag_clean(void)
{ wheel_L_Flag =0;wheel_R_Flag = 0;
Go_Flag = 0;Back_Flag = 0;
Track_Flag = 0;
Stop_Flag = 0;
SZLK_Flag = 0;
//Trackk_Flag = 0;
// temp_MP = 0;
}
/**
ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
void Go_and_Back_Check(void)
{
if(Go_Flag == 1)
{
if(temp_MP <= Roadway_mp_Get())
{
Go_Flag = 0;
Stop_Flag = 3;
Send_UpMotor(0,0); // ֹͣ
}
}
else if(Back_Flag == 1)
{
if(temp_MP <= Roadway_mp_Get())
{
Back_Flag = 0;
Stop_Flag=3;
Send_UpMotor(0,0);
}
}
}
uint8_t Roadway_GoBack_Check(void)
{
return ((Go_Flag == 0)&&(Back_Flag == 0)&&(Track_Flag == 0)&&(wheel_L_Flag == 0)&&(wheel_R_Flag == 0))? 1:0;
}
/**
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
*/
void wheel_Nav_check(void)
{
uint16_t Mp_Value = 0;
if(wheel_Nav_Flag)
{
Mp_Value = Roadway_mp_Get();
if(Mp_Value >= temp_Nav)
{
wheel_Nav_Flag = 0;
Stop_Flag = 2;
Send_UpMotor(0,0);
}
}
}
/**
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>
*/
uint32_t Mp_Value = 0;
void wheel_Track_check(void)
{
uint16_t Track_Value = 0;
if(wheel_L_Flag == 1)
{
Track_Value = Get_Host_UpTrack(TRACK_H8);
delay_ms(3);
if(!(Track_Value & 0X10)) //0x10 <20>ҵ<EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD>ߣ<EFBFBD>ֹͣ
{
if(Wheel_flag > 50)//ԭ50
{
wheel_L_Flag = 0;
Wheel_flag=0;
Stop_Flag=2;
Send_UpMotor(0,0);
}
}
else if(Track_Value == 0Xff) // ѭ<><D1AD><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB>
{
Wheel_flag++;
}
}
else if(wheel_R_Flag == 1)
{
Track_Value = Get_Host_UpTrack(TRACK_H8);
delay_ms(3);
if(!(Track_Value & 0X08)) //0x08<30>ҵ<EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD>ߣ<EFBFBD>ֹͣ
{
if(Wheel_flag > 50)
{
wheel_R_Flag=0;
Wheel_flag=0;
Stop_Flag=2;
Send_UpMotor(0,0);
}
}
else if( Track_Value == 0Xff)
{
Wheel_flag++;
}
}
}
/**
ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
void Track_Check()
{
if(Track_Flag == 1)
{
Track_Correct(Get_Host_UpTrack(TRACK_H8));
}
}
///**
// ѭ<><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//*/
//void Trackk_Check()
//{
// if(Trackk_Flag == 1)
// {
// Trackk_Correct(Get_Host_UpTrack(TRACK_H8));
// }
//}
void Roadway_Check(void)
{
Go_and_Back_Check();
wheel_Track_check();
wheel_Nav_check();
Track_Check();
}
/***************************************************************
** <EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƺ<EFBFBD><EFBFBD><EFBFBD>
** <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> L_Spend<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
** R_Spend<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
** <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>
****************************************************************/
void Control(int L_Spend,int R_Spend)
{
if(L_Spend>=0)
{
if(L_Spend>100)L_Spend=100;if(L_Spend<5)L_Spend=5; //<2F><><EFBFBD><EFBFBD><EFBFBD>ٶȲ<D9B6><C8B2><EFBFBD>
}
else
{
if(L_Spend<-100)L_Spend= -100;if(L_Spend>-5)L_Spend= -5; //<2F><><EFBFBD><EFBFBD><EFBFBD>ٶȲ<D9B6><C8B2><EFBFBD>
}
if(R_Spend>=0)
{
if(R_Spend>100)R_Spend=100;if(R_Spend<5)R_Spend=5; //<2F><><EFBFBD><EFBFBD><EFBFBD>ٶȲ<D9B6><C8B2><EFBFBD>
}
else
{
if(R_Spend<-100)R_Spend= -100;if(R_Spend>-5)R_Spend= -5; //<2F><><EFBFBD><EFBFBD><EFBFBD>ٶȲ<D9B6><C8B2><EFBFBD>
}
Send_UpMotor(L_Spend ,R_Spend);
}
extern uint8_t RFID_Flag;
extern uint8_t Terrain_Flag;
/***************************************************************
** <EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD> ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
** <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD>޲<EFBFBD><EFBFBD><EFBFBD>
** <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>
****************************************************************/
void Track_Correct(uint8_t gd)
{
if((gd == 0x00)) // ѭ<><D1AD><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB> ֹͣ
{
Track_Flag = 0;
Stop_Flag = 1;
Send_UpMotor(0,0);
}
else if(gd==0xE7)
{
LSpeed=Car_Spend;
RSpeed=Car_Spend;
} else if((gd==0xF7) || (gd==0XF3))
{
LSpeed=Car_Spend+20;
RSpeed=Car_Spend-40;
} else if((gd==0XFB) || (gd==0XF9))
{
LSpeed=Car_Spend+40;
RSpeed=Car_Spend-60;
} else if((gd==0XFD) || (gd==0XFC))
{
LSpeed=Car_Spend+60;
RSpeed=Car_Spend-90;
} else if(gd==0XFE)
{
LSpeed=Car_Spend+80;
RSpeed=Car_Spend-120;
} else if((gd==0XEF) || (gd==0XCF))
{
RSpeed = Car_Spend+20;
LSpeed = Car_Spend-40;
} else if((gd==0XDF) || (gd==0X9F))
{
RSpeed = Car_Spend+40;
LSpeed = Car_Spend-60;
} else if((gd==0XBF) || (gd==0X3F))
{
RSpeed = Car_Spend+60;
LSpeed = Car_Spend-90;
} else if(gd==0X7F)
{
RSpeed = Car_Spend+80;
LSpeed = Car_Spend-120;
} else
{
LSpeed = Car_Spend;
RSpeed = Car_Spend;
}
if(Trackk_Flag == 1)
{
if( (gd==0X7E)||(gd==0X3E)||(gd==0X7C)||(gd==0XC3)||(gd==0X78)||(gd==0XE1E))//01111110.00111110.01111100.00111100.01111000.00011110
{
Track_Flag = 0;
Stop_Flag = 1;
SZLK_Flag = 1;
Send_UpMotor(0,0);
}
if((gd&0x18)==0x18&&((gd&0x80)==0x00||(gd&0x01)==0x00)) //0001 1100 0001 1000
{
Track_Flag = 0;
Stop_Flag = 1;
SZLK_Flag = 1;
Send_UpMotor(0,0);
}
}
if(gd==0xFF) //ѭ<><D1AD><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB>
{
LSpeed = Car_Spend;
RSpeed = Car_Spend;
if(count > 1200)
{
count=0;
Send_UpMotor(0,0);
Track_Flag=0;
Stop_Flag = 4;
}
else {
count++;
}
}
else
{
count=0;
}
if(Track_Flag != 0)
{
Control(LSpeed,RSpeed);
}
}
/***************************************************************
** <EFBFBD><EFBFBD><EFBFBD>ܣ<EFBFBD> ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
** <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD>޲<EFBFBD><EFBFBD><EFBFBD>
** <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD> <EFBFBD><EFBFBD>
****************************************************************/
void Track_Roadway(uint8_t gd)
{
if(gd == 0x00) // ѭ<><D1AD><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB> ֹͣ
{
Track_Flag = 0;
Stop_Flag = 1;
Send_UpMotor(0,0);
} else if(gd==0x18)
{
LSpeed=Car_Spend;
RSpeed=Car_Spend;
} else if((gd==0x08) || (gd==0x0C))
{
LSpeed=Car_Spend+20;
RSpeed=Car_Spend-40;
} else if((gd==0x04) || (gd==0x06))
{
LSpeed=Car_Spend+40;
RSpeed=Car_Spend-60;
} else if((gd==0x02) || (gd==0x03))
{
LSpeed=Car_Spend+60;
RSpeed=Car_Spend-90;
} else if(gd==0x01)
{
LSpeed=Car_Spend+80;
RSpeed=Car_Spend-120;
} else if((gd==0x10) || (gd==0x30))
{
RSpeed = Car_Spend+20;
LSpeed = Car_Spend-40;
} else if((gd==0x20) || (gd==0x60))
{
RSpeed = Car_Spend+40;
LSpeed = Car_Spend-60;
} else if((gd==0x40) || (gd==0xC0))
{
RSpeed = Car_Spend+60;
LSpeed = Car_Spend-90;
} else if(gd==0X80)
{
RSpeed = Car_Spend+80;
LSpeed = Car_Spend-120;
} else
{
LSpeed=Car_Spend;
RSpeed=Car_Spend;
}
if(gd==0xFF) //ѭ<><D1AD><EFBFBD><EFBFBD>ȫ<EFBFBD><C8AB>
{
if(count > 1000)
{
count=0;
Send_UpMotor(0,0);
Track_Flag=0;
Stop_Flag = 4;
}
else {
count++;
}
}
else
{
count=0;
}
if(Track_Flag != 0)
{
Control(LSpeed,RSpeed);
}
}
void roadway_check_TimInit(uint16_t arr,uint16_t psc)
{
TIM_TimeBaseInitTypeDef TIM_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9,ENABLE);
TIM_InitStructure.TIM_Period = arr;
TIM_InitStructure.TIM_Prescaler = psc;
TIM_InitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_InitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_InitStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM9,&TIM_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = TIM1_BRK_TIM9_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 5;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_ITConfig(TIM9,TIM_IT_Update,ENABLE);
TIM_Cmd(TIM9, ENABLE);
}
void TIM1_BRK_TIM9_IRQHandler(void)
{
if(TIM_GetITStatus(TIM9,TIM_IT_Update) == SET)
{
Roadway_Check(); // ·<><C2B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
TIM_ClearITPendingBit(TIM9,TIM_IT_Update);
}
/*ʮ<><CAAE>·<EFBFBD><C2B7>Ѱ<EFBFBD><D1B0>*/
void Car_Track_szlkRFID(uint8_t speed,uint8_t chunk) // <20><><EFBFBD><EFBFBD>ʮ<EFBFBD><CAAE>·<EFBFBD><C2B7>ѭ<EFBFBD><D1AD>Ѱ<EFBFBD><D1B0> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȡ<D9B6><C8A1><EFBFBD><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD>ַ
{
Stop_Flag = 0; // <20><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>־λ
Go_Flag = 0; // ǰ<><C7B0><EFBFBD><EFBFBD>־λ
wheel_L_Flag = 0; // <20><>ת<EFBFBD><D7AA>־λ
wheel_R_Flag = 0; // <20><>ת<EFBFBD><D7AA>־λ
wheel_Nav_Flag = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>־λ
Back_Flag = 0; // <20><><EFBFBD>˱<EFBFBD>־λ
Track_Flag = 1; // ѭ<><D1AD><EFBFBD><EFBFBD>־λ
SZLK_Flag = 0; // ʮ<><CAAE>·<EFBFBD><C2B7>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD>־λ
Trackk_Flag = 1;
rfidxh_Flag = 0;
Car_Spend = speed; // <20>ٶ<EFBFBD>ֵ
Control(Car_Spend, Car_Spend); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> *
while(rfidxh_Flag != 0x01)//(gd==0X81)||(gd==0XC1)||(gd==0X83)||(gd==0XC3)||(gd==0X87)||(gd==0XE11)
{
//if((gd & 0x18)==0x18&&((gd & 0x80)==0x00||(gd & 0x01)==0x00)) // Ѱ<><D1B0><EFBFBD>ɹ<EFBFBD>
if (SZLK_Flag == 1)
{
Car_Go(40,170);
RC522(chunk,RFID_Read); //RC522(6,RFID_Read);
RC522(chunk,RFID_Read); //RC522(6,RFID_Read);
Cba_Beep(1);
Car_Go(40,90);
Stop_Flag = 1;
Track_Flag = 0;
rfidxh_Flag = 1;/////
Send_UpMotor(0,0);
}else if((Trackk_Flag == 1)&(Track_Flag == 0)) //0001 1100 0001 1000 (gd == 0x00)||
{
Car_Go(40,260);
Stop_Flag = 1;
rfidxh_Flag = 1;/////
Send_UpMotor(0,0);
}else if(PcdRequest(PICC_REQALL,CT) == MI_OK) // Ѱ<><D1B0><EFBFBD>ɹ<EFBFBD>
{
Track_Flag = 0;
Car_Stop();
RC522(chunk,RFID_Read); //RC522(6,RFID_Read);
RC522(chunk,RFID_Read); //RC522(6,RFID_Read);
Cba_Beep(1);
Car_Go(speed,50);
Track_Flag = 1;
Control(Car_Spend, Car_Spend);
}
}
}
//end file
Reference in New Issue Copy Permalink