第七届工训赛物流组小车电控程序 -

#include "stm32f10x.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "timer.h"
#include "encoder.h"
#include "motor.h"
#include "dataprocess.h"
#include "steer.h"
#include "show.h"
#include "oled.h"
#include "exti.h"
#include "DataScope_DP.h"
#include "adc.h"
#include "dma.h"
#include "steercontrol.h"
u8 Mode_All=1; //总模式标志，0，控制电机，1，控制舵机
u16 Time_Count=0; //时间计数变量
u16 Distance_F,Distance_L; //前方距离和左边距离
u16 LocationInit_X,LocationInit_Y; //初始坐标
int Location_X,Location_Y; //当前坐标
float Yaw ; //航向角
int Encoder_Left,Encoder_Right;
int Distance_Left=0,Distance_Right=0; //用于更新坐标（单位时间内【50ms】左轮前进距离和右轮前进距离）
u8 state=1; //mode=0,停止；mode=1,直行；mode=2,右转90度；mode =3,左转90度；mode=4，左转180度
u16 Voltage; //电池电压
u16 AD_Value[2]; //电池电压ADC转换的结果
u8 Turn_Flag; //转向标志，如果某一次转向完成，则置1，反之，置0
u8 Task=2; //通过扫描二维码得到的任务
unsigned char FSM_Status = 0; //状态机的状态变量
float Target1=750,Target2=750,Target3=750,Target4=750,Target5=835,Target6=750; //电机目标值
float TargetX=0.2,TargetY=0,Target_Beta=-10,Target_Alpha=0,Target_Gamma=-5; //姿态目标值，Target_Beta 1号舵机旋转角度，Target_Alpha，云台角度，Target_Gamma，6号舵机角度
u16 Angle1=90,Angle2=90,Angle3=90,Angle4=90,Angle5=90,Angle6=105;
int main(void)
{
u16 i;
delay_init(); //延时函数初始化
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
LED_Init(); //LED初始化
OLED_Init(); //OLED初始化
uart_init(128000); //串口一初始化
// uart4_init(9600); //测距串口初始化
// uart5_init(115200); //openmv串口初始化
// AdcInit(); //电池电压采集
// MYDMA_Config(); //DMA1采集ADC
// IIC_Init(); //=====模拟IIC初始化
// MPU6050_initialize(); //=====MPU6050初始化
// DMP_Init(); //=====初始化DMP
Encoder_Init_TIM2(); //编码器初始化，右轮，为正值
Encoder_Init_TIM3(); //编码器初始化，左轮，为负值
Motor_Init(); //电机控制IO口初始化
Motor_PWM_Init(7199,0); //电机pwm初始化，10KHz
TIM1_PWM_Init(9999,144); //舵机PWM初始化，50Hz,T=20ms
TIM8_PWM_Init(9999,144); //舵机PWM初始化，50Hz,T=20ms
// TIM6_Int_Init(499,7200); //50ms定时中断
TIM7_Int_Init(99,7200); //10ms定时中断
EXTIX_Init(); //按键外部中断初始化
// EXTI_PC5_Init(); //mpu6050INT引脚中断初始化
// SetSteer(95,90,90,90,90,105);
// delay_ms(1000);
// delay_ms(1000);
// SetSteer_PID(95,155,90,180,90,130);
while(1)
{
oled_show();
delay_ms(50);
}
while(1)
{
state =1;//直行
oled_show();
if(Location_X == 0 && Location_Y >= 200)
{
state=0;//停
break;
}
}
for(i=0;i<20;i++)
{
oled_show();
delay_ms(50);
}
while(1)
{
state = 2; //右转90 90
oled_show();
if(Turn_Flag == 1)//转向完成
{
state = 0; //停
Turn_Flag =0;
break;
}
}
delay_ms(300);
while(1)
{
state = 1;//直行
oled_show();
if(Location_Y >= 200 && Location_X >= 1800)
{
state =0;
break;
}
}
delay_ms(300);
while(1)
{
state = 2; //右转90 180
oled_show();
if(Turn_Flag == 1)//转向完成
{
state = 0; //停
Turn_Flag =0;
break;
}
}
delay_ms(300);
while(1)
{
state = 2; //右转90 -90
oled_show();
if(Turn_Flag == 1)//转向完成
{
state = 0; //停
Turn_Flag =0;
break;
}
}
delay_ms(300);
while(1)
{
state = 1;//直行
oled_show();
if(Location_Y >= 200 && Location_X <= 900)
{
state =0;
break;
}
}
while(1)
{
state = 2; //右转90 0
oled_show();
if(Turn_Flag == 1)//转向完成
{
state = 0; //停
Turn_Flag =0;
break;
}
}
delay_ms(300);
while(1)
{
state = 4;//左转180 -180
oled_show();
if(Turn_Flag == 1)//转向完成
{
state = 0; //停
Turn_Flag =0;
break;
}
}
delay_ms(300);
while(1)
{
state = 2; //右转90 -90
oled_show();
if(Turn_Flag == 1)//转向完成
{
state = 0; //停
Turn_Flag =0;
break;
}
}
delay_ms(300);
while(1)
{
state = 1;//直行
oled_show();
if(Location_Y >= 200 && Location_X <= 0)
{
state =0;
break;
}
}
while(1)
{
state = 3; //左转90 -180
oled_show();
if(Turn_Flag == 1)//转向完成
{
state = 0; //停
Turn_Flag =0;
break;
}
}
delay_ms(300);
while(1)
{
state = 1;//直行
oled_show();
if(Location_Y <= 0 && Location_X <= 0)
{
state =0;
break;
}
}
while(1)
{
Led_Flash(50000);
oled_show();
delay_ms(50);
}
}

复制代码