/********************************************** *******************************
* Routine name: empty project
* Note: New projects can directly copy empty projects to start programming.
* Wiring : ---
* Notice \t\t : ---
***************************************************** *******************************/
//--Include the header files you want to use--//
#include "config.h" //General configuration header file
#define LED_ON 0
#define LED_OFF 1
#define GYRO_OFFSET 13 //Gyroscope zero drift value
#define Car_speed 200 // car speed
#define VKp 8 //Car speed PID proportional coefficient
#define VKi 3 //Trolley speed PID integral coefficient
#define VKd 0 //Trolley speed PID differential coefficient
//--declare some constants to use--//
int Kp =1400; //The direction scale factor of the car's tracking is 1500
int Kd =15; //The directional damping coefficient of the car's tracking is 17
sbit LED0 = P4^4;
sbit LED1 = P5^4;
#define LED_DATA P6 //select digital tube segment
#define IR_LED P0 //IR sensor status
uchar IR_LR=0; //Infrared tube left and right sequence
uchar GX_n = 0; //crossing count variable=0
sbit W1 = P7^4; //digital tube selection control pin
sbit W2 = P7^5;
sbit W3 = P7^6;
sbit W4 = P7^7;
sbit KEY_UP = P4^0;
sbit KEY_DOWN = P4^1;
sbit KEY_LEFT = P4^2;
sbit KEY_RIGHT = P4^3;
sbit KEY = P3^2;
bit CAR_PWM_EN = 0; //PWM enable =0 not running =1 running
int L_speed =0; //wheel speed
int R_speed = 0;
int PWM_SPEED=0;//speed control PWM value
int PWM_DIR = 0;//direction control PWM value
int Set_Speed = Car_speed; //set speed
int Err_Speed = 0; //current deviation
int Err_Speed_Old = 0;//The last deviation
int Err_Speed_OldOld= 0;//The last 2 deviations
int L_PWM =0; //Define the PWM of the left and right wheels
int R_PWM = 0;
int Car_Err = 0 ; // car body deviation
int Car_Err_old = 0 ; //The last deviation of the car body
uint32 S_car =0; //The distance traveled by the car
uchar IRLED = 0;
uchar IRLED8[8] = {<!-- -->0};
uchar MPU6050_DATA[14];
float GYRO_Z;
// 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14
uchar DIS_LED[]={<!-- -->0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f,0x3e,0x38,0x73,0x49,0x78}; //segment code table
// 0 1 2 3 4 5 6 7 8 9 U L P S T
uchar DIS_LED_DATA[4] = {<!-- -->10,1,2,3}; //The data to be displayed by the four digital tubes
uchar KEY_OLD[5] = {<!-- -->0xff}; //The past state of the button
uchar L_n =0; //Ultrasonic data control bit
uchar L_data[3]; //ultrasonic data buffer
uint32 LL = 0; //The distance measured by ultrasonic wave
//--declare some variables that need to be used--//
uint T0_n =0; //timer 0 count variable
uint POWER_U_ADC; //battery voltage ADC value
uchar DIS_dat = 0; //DAT =0 - display voltage =1 - display speed =2 - display PWM value
uint CAR_SPEED = 123; //Define the car speed
void MOTOR_OUT(int Lpwm,int Rpwm); //motor PWM output function
void DIS_LEDS(void); // digital tube refresh function
void SCAN_KEY(void); //key scanning function
char Get_Err(uchar IR); //Get the car body deviation function
/**************************************************** *******************************
* Function name: main
* Function function: main function
* Input: None
* Output: none
***************************************************** *******************************/
void main(void)
{<!-- -->
int i=0;
GPIO_init_allpin(0); //Initialize all IOs as quasi-bidirectional
GPIO_init_pin(20,1); //Initialize the motor drive pin to strong push-pull mode
GPIO_init_pin(21,1);
GPIO_init_pin(22,1);
GPIO_init_pin(23,1);
GPIO_init_pin(55,1);
GPIO_init_8pin(6,1); //Initialize all IOs of P6 as strong push-pull
GPIO_init_pin(74,1); //Initialize the bit selection pin of the digital tube to strong push-pull mode
GPIO_init_pin(75,1);
GPIO_init_pin(76,1);
GPIO_init_pin(77,1);
GPIO_init_pin(17,2); //P17ADC pin high impedance mode
\t
UART_init(1,115200,1,1); //Initialize serial port 1, baud rate 115200bps, the first multiplex,
UART_init(3,9600,2,1); //Initialize the serial port 3, the baud rate is 9600bps, the second channel is multiplexed, and read the distance of the ultrasonic module
PIT_init_ms(2,10); //Initialize timer 2, 10ms interrupt
ADC_init(7,8,4); //Initialize ADC to measure battery voltage
PCA_Pwm_init(70,50,0); //Initialize the PCA module and generate 50HZPWM to control the steering gear
INT_init(2,0); //Initialize external interrupt 2.3
INT_init(3,0);
\t
InitMPU6050(); //Initialize the gyroscope module
\t
PWM_init(20,1000,0); //Initialize motor PWM pin
PWM_init(21,1000,0);
PWM_init(22,1000,0);
PWM_init(23,1000,0);
\t
BUZZ = 1; //buzzer sounds
Delay_X_mS(500);
BUZZ = 0;
\t
while (1)
{<!-- -->
SCAN_KEY(); //Scan keys
DIS_LEDS(); //Refresh digital tube
\t\t
// judge the starting line
if(S_car>2000) //Constraints for the first crossing line
{<!-- -->
if(LL > 50 & amp; & amp; LL < 300) // determine whether there is a roadblock
{<!-- -->
CAR_PWM_EN = 0;
while(LL < 300);
CAR_PWM_EN = 1;
PWM_SPEED = 0;
Err_Speed_Old=0;
Err_Speed=0;
}
\t\t\t
if(IR_LED == 0xFF & amp; & amp; GX_n == 0) //If the line is pressed
{<!-- -->
while(IR_LED == 0xFF); //wait for the line
GX_n = 1; //Enable cross-line variables
}
if(IR_LED == 0xFF & amp; & amp; GX_n != 0) //If the second press line
{<!-- -->
CAR_PWM_EN = 0;
}
}
}
}
void UART3_isr(void)
{<!-- -->
uchar DAT3;
\t
DAT3 = S3BUF; //Read the data received by serial port 3
\t
if(L_n <= 2) //save data
{<!-- -->
L_data[L_n] = DAT3;
L_n++;
}
\t
if(L_n >= 3) // when receiving is complete
{<!-- -->
LL = (L_data[0]*65536 + L_data[1]*256 + L_data[2])/1000; //Get the distance value
if(LL > 999) LL =999; // limit the distance
}
}
void INT2_isr(void) //collect the pulse of external interrupt 2 and judge the direction
{<!-- -->
if(P34) L_speed ++ ;
else L_speed--;
}
void INT3_isr(void) //collect the pulse of external interrupt 2 and judge the direction
{<!-- -->
if(P35) R_speed--;
else R_speed++;
}
void SCAN_KEY(void)
{<!-- -->
uchar key;
\t
KEY_OLD[0]<<=1; //Key state before iteration
KEY_OLD[1]<<=1;
KEY_OLD[2]<<=1;
KEY_OLD[3]<<=1;
KEY_OLD[4]<<=1;
key = KEY_UP;
KEY_OLD[0] + = key;
key = KEY_DOWN;
KEY_OLD[1] + = key;
key = KEY_LEFT;
KEY_OLD[2] + = key;
key = KEY_RIGHT;
KEY_OLD[3] + = key;
key = KEY;
KEY_OLD[4] + = key;
if(KEY_OLD[4] == 0x00) //Judge the middle button is pressed
{<!-- -->
while(!KEY); //wait for the button to be released
\t\t
switch(DIS_dat)
{<!-- -->
case 0:
{<!-- -->
CAR_PWM_EN =!CAR_PWM_EN; //Departure judgment
\t\t\t\t\t\t\t
S_car = 0;
PWM_SPEED = 0;
Err_Speed_Old=0;
Err_Speed=0;
\t\t\t\t\t\t\t
}break; //Interface 1
case 1: //Interface 2
{<!-- -->
}break;
case 2: //Interface 3
{<!-- -->
}break;
}
}
if(KEY_OLD[0] == 0x00) //Press when judging UP
{<!-- -->
while(!KEY_UP); //wait for the button to be released
\t\t
switch(DIS_dat)
{<!-- -->
case 0: //Interface 1
{<!-- -->
Set_Speed += 5;
\t\t\t\t\t\t\t
}break;
case 1: //Interface 2
{<!-- -->
}break;
case 2: //Interface 3
{<!-- -->
Kp + = 50;
}break;
case 3: //Interface 4
{<!-- -->
Kd + = 1;
}break;
}
}
if(KEY_OLD[1] == 0x00) //press when judging DOWN
{<!-- -->
while(!KEY_DOWN); //wait for the button to be released
\t\t
switch(DIS_dat)
{<!-- -->
case 0: //Interface 1
{<!-- -->
Set_Speed -= 5;
}break;
case 1:
{<!-- -->
}break;
case 2:
{<!-- -->
Kp -= 50;
}break;
case 3: //Interface 4
{<!-- -->
Kd -= 1;
}break;
}
}
if(KEY_OLD[2] == 0x00) //Press when judging LEFT
{<!-- -->
while(!KEY_LEFT); //Wait for the button to be released
\t\t
if(DIS_dat>0) DIS_dat-=1;
}
if(KEY_OLD[3] == 0x00) //Press when judging RIGHT
{<!-- -->
while(!KEY_RIGHT); //wait for the button to be released
\t\t
if(DIS_dat<4) DIS_dat + =1;
}
\t
}
char Get_Err(uchar IR)
{<!-- -->
char L,R,i;
\t
L = 0; //Look for rising edge from left to right
for(i=0;i<7;i ++ )
{<!-- -->
if(((IR & amp; 1<<(i + 1)) >> (i + 1))-((IR & amp; 1<<i) >> i) == 1) //left to right find rising edge
{<!-- -->
L = i;
break;
}
}
R = 7; //Right to left to find the rising edge
for(i=7;i>0;i--)
{<!-- -->
if(((IR & amp; 1<<(i)) >> (i))-((IR & amp; 1<<(i + 1)) >> (i + 1)) == 1) / /Left to right to find the rising edge
{<!-- -->
R = i;
break;
}
}
i = L + R - 7;
\t
return i;
}
void DIS_LEDS(void)
{<!-- -->
int i;
\t
switch(DIS_dat)
{<!-- -->
case 0: //display vehicle speed
{<!-- -->
DIS_LED_DATA[0] = 13; //S
DIS_LED_DATA[1] = Set_Speed/100;
DIS_LED_DATA[2] = Set_Speed/10;
DIS_LED_DATA[3] = Set_Speed/1;
break;
}
case 1: //display battery voltage
{<!-- -->
DIS_LED_DATA[0] = 10; //U
DIS_LED_DATA[1] = POWER_U_ADC/100;
DIS_LED_DATA[2] = POWER_U_ADC/10;
DIS_LED_DATA[3] = POWER_U_ADC/1;
break;
}
case 2: //Display KP
{<!-- -->
DIS_LED_DATA[0] = 12; //P
DIS_LED_DATA[1] = Kp/1000;
DIS_LED_DATA[2] = Kp/100;
DIS_LED_DATA[3] = Kp/10;
break;
}
case 3: //Display KD
{<!-- -->
DIS_LED_DATA[0] = 14; //d
DIS_LED_DATA[1] = Kd/100;
DIS_LED_DATA[2] = Kd/10;
DIS_LED_DATA[3] = Kd/1;
break;
}
case 4: //Display L
{<!-- -->
DIS_LED_DATA[0] = 11; //L
DIS_LED_DATA[1] = LL/100;
DIS_LED_DATA[2] = LL/10;
DIS_LED_DATA[3] = LL/1;
break;
}
}
\t
for(i=0;i<4;i ++ )
{<!-- -->
switch(i)
{<!-- -->
case 0:
{<!-- -->
W1 = 1;W2 = 0;W3 = 0;W4 = 0; //The first bit is on
LED_DATA = DIS_LED[DIS_LED_DATA[3]];
}break;
case 1:
{<!-- -->
W1 = 0;W2 = 1;W3 = 0;W4 = 0; //The second bit is on
LED_DATA = DIS_LED[DIS_LED_DATA[2]];
}break;
case 2:
{<!-- -->
W1 = 0;W2 = 0;W3 = 1;W4 = 0; //the third bit is on
if(DIS_dat == 1) //if display voltage + decimal point
{<!-- -->
LED_DATA = DIS_LED[DIS_LED_DATA[1]] | 0x80;
}
else
{<!-- -->
LED_DATA = DIS_LED[DIS_LED_DATA[1]];
}
}break;
case 3:
{<!-- -->
W1 = 0;W2 = 0;W3 = 0;W4 = 1; //The 4th bit is on
LED_DATA = DIS_LED[DIS_LED_DATA[0]];
}break;
}
Delay_X_mS(2);
LED_DATA = 0x00;
}
}
void MOTOR_OUT(int Lpwm, int Rpwm)
{<!-- -->
if(Lpwm>9600) Lpwm = 9600; //output PWM limiter
if(Rpwm>9600) Rpwm = 9600;
if(Lpwm<-9600) Lpwm = -9600;
if(Rpwm<-9600) Rpwm = -9600;
\t
if(Lpwm > 0) //judging the direction of the left wheel
{<!-- -->
PWM_change(20,abs(Lpwm)); //forward rotation
PWM_change(21,0);
}
else
{<!-- -->
PWM_change(20,0); //Reverse
PWM_change(21,abs(Lpwm));
}
\t
if(Rpwm > 0) //judging the direction of the right wheel
{<!-- -->
PWM_change(23,abs(Rpwm)); //forward rotation
PWM_change(22,0);
}
else
{<!-- -->
PWM_change(23,0); //Reverse
PWM_change(22,abs(Rpwm));
}
\t
}
void TM2_isr(void) //10ms one interrupt
{<!-- -->
\t
uchar a;
\t
if(T0_n<9999) T0_n ++ ; //Form a loop variable
else T0_n=0;
\t
\t
if(GX_n != 0 & amp; & amp; GX_n < 50)
{<!-- -->
GX_n ++ ; //The cross-line variable is added automatically
}
else if(GX_n == 50)
{<!-- -->
GX_n = 0;
}
\t\t
\t
Read_MPU6050(MPU6050_DATA); //Read gyroscope raw data
GYRO_Z = ((int)(MPU6050_DATA[12]<<8 | MPU6050_DATA[13]) + GYRO_OFFSET)/65.5; //Conversion data
\t
IR_LR = 0;
a= P07; //The rightmost infrared tube pin
IR_LR + = a*128;
a= P06;
IR_LR + = a*64;
a= P05;
IR_LR + = a*32;
a= P04;
IR_LR + = a*16;
a= P03;
IR_LR + = a*8;
a= P02;
IR_LR + = a*4;
a= P01;
IR_LR + = a*2;
a= P00; //The leftmost infrared tube pin
IR_LR + = a*1;
\t
Car_Err = Get_Err(IR_LR); //Read the status of the infrared tube and calculate the deviation p07 p06 p05 p04 - p00
\t
PWM_DIR = Car_Err * Kp + GYRO_Z * Kd ; //direction PID calculation
\t
\t
if(CAR_PWM_EN == 0 ) //Enable PWM output
{<!-- -->
Set_Speed = 0;
\t\t
//PWM_SPEED = 0;
//PWM_SPEED = 0;
}
else
{<!-- -->
Set_Speed = Car_speed;
}
\t
L_PWM = PWM_SPEED + PWM_DIR; //speed loop + direction loop
R_PWM = PWM_SPEED - PWM_DIR; //speed loop - direction loop
\t
Car_Err = IR_LR;
\t
if(Car_Err == 0x00) //Lost the line
{<!-- -->
if(Car_Err_old == 0x80)
{<!-- -->
MOTOR_OUT(5000,-1500); //motor open loop output control
while(!(P05)); //0010 0000
}
if(Car_Err_old == 0x01)
{<!-- -->
MOTOR_OUT(-1500,5000); //motor open loop output control
while(!(P02)); //0000 0100
}
}
Car_Err_old = Car_Err; //Iterate last state
\t
\t
\t
MOTOR_OUT(L_PWM,R_PWM); //motor open loop output control
if(T0_n % 5 == 0) //50ms speed control
{<!-- -->
Err_Speed = Set_Speed - (L_speed + R_speed); // get deviation speed
\t\t
S_car + = (L_speed + R_speed); //Integral car travel distance
\t\t
PWM_SPEED + = VKp * (Err_Speed - Err_Speed_Old) + VKi * (Err_Speed); //position type PID formula
\t\t
if(PWM_SPEED>5000) PWM_SPEED = 5000; //output limiter
if(PWM_SPEED<-5000) PWM_SPEED = -5000;
\t\t
Err_Speed_OldOld = Err_Speed_Old; //Deviation iteration
Err_Speed_Old = Err_Speed;
\t\t
L_speed = 0; //speed cleared to 0
R_speed = 0;
\t\t
if(CAR_PWM_EN == 1) //running
{<!-- -->
PCA_Pwm_change(0,780-(Get_Err(P0)*30)); //The servo looks ahead
}
else
{<!-- -->
PCA_Pwm_change(0,780); //Steering gear back to normal
}
}
\t\t
if(T0_n % == 0) //Execute once every 250ms
{<!-- -->
LED0=!LED0; //The light flashes
LED1=!LED1;
\t\t
\t\t
UART_Send_byte(3,0xA0); //Send start ranging data
L_n = 0;
\t\t
POWER_U_ADC = ADC_get(7); //Get the conversion result of ADC channel 7
POWER_U_ADC /= 4.09; //Convert the ADC value into battery voltage*100 times
}
}