电赛STM32F1红色小球识别的板球控制-ov7670程序 -

#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "lcd.h"
#include "usart.h"
#include "string.h"
#include "ov7670.h"
#include "tpad.h"
#include "timer.h"
#include "exti.h"
#include "usmart.h"
#include "EasyTracered.h"
#include "io_in_out.h"
#include "oled.h"
#include "gui.h"
#include "pid.h"
/************************************************
作者：TZT
************************************************/
void camera_refresh(void);
/*---------------------结构体全局定义-----------------*/
RESULT Resured;//判定为的目标条件
/*---------------------LCD显示-----------------*/
const u8*LMODE_TBL[5]={"Auto","Sunny","Cloudy","Office","Home"}; //5种光照模式
const u8*EFFECTS_TBL[7]={"Normal","Negative","B&W","Redish","Greenish","Bluish","Antique"}; //7种特效
extern u8 ov_sta; //在exit.c里面定义
extern u8 ov_frame; //在timer.c里面定义
/*---------------------------结构体变量-------------------*/
extern PIDTypdDef M1PID; //PID1
extern PIDTypdDef M2PID; //PID2
/*---------------------------x和y的误差-------------------*/
float x1_error;
float x2_error;
float x3_error;
float x4_error;
float y1_error;
float y2_error;
float y3_error;
float y4_error;
float x1_1error;
float x2_2error;
float x3_3error;
float x4_4error;
float y1_1error;
float y2_2error;
float y3_3error;
float y4_4error;
float sx_1error;
float sx_2error;
float sx_3error;
float sx_4error;
float sx_5error;
float sx_6error;
float sy_1error;
float sy_2error;
float sy_3error;
float sy_4error;
float sy_5error;
float sy_6error;
float sx_1need = 209.0;
float sx_2need = 254.0;
float sx_3need = 209.0;
float sx_4need = 163.0;
float sx_5need = 117.0;
float sx_6need = 70.0;
float sx_7need = 118.0;
float sy_1need = 71.0 ;
float sy_2need = 116.0;
float sy_3need = 161.0;
float sy_4need = 117.0;
float sy_5need = 71.0;
float sy_6need = 118.0;
float sy_7need = 162.0;
/*---------------------------菜单选择---------------------*/
extern unsigned char RWXZ; //#任务选择
int32_t PWM1; //#PWM1
int32_t PWM2; //#PWM2
extern float need_x1 = 209.0;
extern float need_x2 = 209.0;
extern float need_x3 = 118.0;
extern float need_x4 = 118.0;
extern float need_y1 = 182.0;
extern float need_y2 = 71.0;
extern float need_y3 = 71.0;
extern float need_y4 = 182.0;
float new_x1 ;
float new_x2 ;
float new_x3 ;
float new_x4 ;
float new_y1 ;
float new_y2 ;
float new_y3 ;
float new_y4 ;
extern int32_t SET_1;
extern int32_t SET_2;
extern int32_t SET_3;
extern int32_t SET_4;
/*---------------------位置和PWM变量-----------------*/
u16 r,y;
u16 s,z;
int32_t wz_x,wz_y; //显示坐标
#define M1_PWMMAX 180 //舵机一最大值
#define M2_PWMMAX 140 //舵机二最大值
void wkqwz(void)
{
if(PFin(7)==0)
{
delay_ms(10);
if(PFin(7)==0)
{
while(1)
{
camera_refresh();//更新显示
wz_x = Resured.x;
wz_y = Resured.y;
OLED_ShowNum(0,0,wz_x,4,16); //这句话把存进缓存的数显示出来
OLED_ShowNum(64,0,wz_y,4,16);
OLED_Refresh_Gram();
if(PFin(7)==1)
{
delay_ms(10);
if(PFin(7)==1)
{
break;
}
}
}
}
}
}
/*----------------控制舵机的转动角度----------*/
void kz_hs(int32_t pwm1,int32_t pwm2)
{
pwm1 = pwm1 + 1235; //目标121，158 实际219,260
pwm2 = 1275 - pwm2; //
TIM_SetCompare1(TIM4,pwm1);//前后
TIM_SetCompare4(TIM4,pwm2);//
}
/*----------------------任务------------------*/
void MODE1(void) //任务一
{
TIM_SetCompare1(TIM4,1235);//前后
TIM_SetCompare4(TIM4,1275);//左右
}
void MODE2(void) //任务二
{
// x = 163;
// y = 117;
const float priod = 2000.0; //周期(毫秒)
const uint32_t x_1 = 219;
const uint32_t y_1 = 260;
const uint32_t need_x = 121;
const uint32_t need_x_erro = 98;
const uint32_t need_y = 158;
const uint32_t need_y_erro = 102;
static uint32_t MoveTimeCnt = 0;
float Normalization = 0.0;
float set_x,set_y;
MoveTimeCnt += 5;
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 254.0 - (91.0 * Normalization) ;
set_y = 205.0 - (88.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.5);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.4);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
PWM1=PID_M1_PosLocCalc((float)Resured.x);
PWM2=PID_M2_PosLocCalc((float)Resured.y);
if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX;
if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
kz_hs(PWM1,PWM2);//控制函数
}
void MODE3(void) //任务三
{
// x1 = 163 , y1 = 207
// x2 = 163 , y2 = 117
const float priod = 1900.0; //周期(毫秒)
const uint32_t x_1 = 219;
const uint32_t y_1 = 260;
const uint32_t need_x = 121;
const uint32_t need_x_erro = 98;
const uint32_t need_y = 158;
const uint32_t need_y_erro = 102;
static uint32_t MoveTimeCnt = 0;
float Normalization = 0.0;
float set_x,set_y;
float hc_time = 600;
MoveTimeCnt += 5; //每5ms运算1次
if(MoveTimeCnt<=priod)
{
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 254.0 - (91.0 * Normalization);// x = 72 ,y = 28;
set_y = 205.0 - (-2.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.7);
PID_M1_SetKi(0.07);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.6);
PID_M2_SetKi(0.07);
PID_M2_SetKd(100);
}
else if(MoveTimeCnt>(priod+hc_time))
{
Normalization = (float)(MoveTimeCnt-(priod+hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (0.0 * Normalization);// x = 72 ,y = 28;
set_y = 207.0 - (90.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
PWM1=PID_M1_PosLocCalc((float)Resured.x);
PWM2=PID_M2_PosLocCalc((float)Resured.y);
if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX;
if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
kz_hs(PWM1,PWM2);//控制函数
}
void MODE4(void) //任务四
{
// x = 72; y = 28;
const float priod = 2000.0; //周期(毫秒)
const uint32_t x_1 = 219;
const uint32_t y_1 = 260;
const uint32_t need_x = 121;
const uint32_t need_x_erro = 98;
const uint32_t need_y = 158;
const uint32_t need_y_erro = 102;
static uint32_t MoveTimeCnt = 0;
float Normalization = 0.0;
float set_x,set_y;
float hc_time = 1000;
MoveTimeCnt += 5;
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
if(MoveTimeCnt<=priod)
{
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 254.0 - (66.0 * Normalization);// x = 72 ,y = 28;
set_y = 205.0 - (120.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.07);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.07);
PID_M2_SetKd(100);
}
/*---------------------------------------------*/
else if(MoveTimeCnt>priod)
{
Normalization = (float)(MoveTimeCnt-priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 188.0 - (116.0 * Normalization);// x = 72 ,y = 28;
set_y = 85.0 - (57.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.02);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.02);
PID_M2_SetKd(100);
}
PWM1=PID_M1_PosLocCalc((float)Resured.x);
PWM2=PID_M2_PosLocCalc((float)Resured.y);
if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX;
if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
kz_hs(PWM1,PWM2);//控制函数
}
void MODE5(void) //任务五
{
// x1 = 254; y1 = 205;
// x2 = 254; y2 = 116;
// x3 = 163; y3 = 25;
// x3 = 72 ; y4 = 28;
const float priod = 2000.0; //周期(毫秒)
const uint32_t x_1 = 219;
const uint32_t y_1 = 260;
const uint32_t need_x = 121;
const uint32_t need_x_erro = 98;
const uint32_t need_y = 158;
const uint32_t need_y_erro = 102;
static uint32_t MoveTimeCnt = 0;
float Normalization = 0.0;
float set_x,set_y;
float hc_time = 500;
MoveTimeCnt += 5; //每5ms运算1次
/*-----------------1 到 2 ----------------------------*/
if(MoveTimeCnt<=priod)
{
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 254.0 - (0.0 * Normalization);// x = 254 ,y = 116;
set_y = 205.0 - (89.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
/*-----------------2 到 6 ----------------------------*/
else if((MoveTimeCnt > (priod+hc_time))&&(MoveTimeCnt < (2.0 * priod + hc_time)))
{
Normalization = (float)(MoveTimeCnt-(priod+hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 254.0 - (91.0 * Normalization);// x = 72 ,y = 28;
set_y = 116.0 - (91.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*-----------------6 到 9 ----------------------------*/
else if(MoveTimeCnt>( 2.0 * priod + hc_time ))
{
Normalization = (float)(MoveTimeCnt-(2.0 * priod + hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (91.0 * Normalization);// x = 163 ,y = 25;
set_y = 25.0 - (0.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.02);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.02);
PID_M2_SetKd(100);
}
PWM1=PID_M1_PosLocCalc((float)Resured.x);
PWM2=PID_M2_PosLocCalc((float)Resured.y);
if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX;
if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
kz_hs(PWM1,PWM2);//控制函数
}
void MODE6(void) //任务六走四个点走不了对角线
{
const float priod = 1500.0; //周期(毫秒)
const uint32_t x_1 = 219;
const uint32_t y_1 = 260;
const uint32_t need_x = 121;
const uint32_t need_x_erro = 98;
const uint32_t need_y = 158;
const uint32_t need_y_erro = 102;
static uint32_t MoveTimeCnt = 0;
float Normalization = 0.0;
float set_x,set_y;
float hc_time = 100;
MoveTimeCnt += 5; //每5ms运算1次
/*----------------------180------------a到b-----------------------*/
if(((need_x1-need_x2)==180)||((need_x1-need_x2)==-180)||((need_y1-need_y2)==180)||((need_y1-need_y2)==-180))
{
if(MoveTimeCnt<=priod)
{
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
new_x1 = (need_x1 + need_x2)/2.0 ; //209-209
new_y1 = (need_y1 + need_y2)/2.0 ; //209-209
new_x1= (new_x1 + 163) /2.0; //209
new_y1 = (new_y1 + 117)/2.0 ; //117
x1_1error = new_x1 - need_x1; //163-250 = -87
y1_1error = new_y1 - need_y1; //72-28 = 44
set_x = need_x1 + (x1_1error * Normalization); //250 + -87 = 163
set_y = need_y1 + (y1_1error * Normalization) ;//26 + 44 = 70
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.02);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.02);
PID_M2_SetKd(100);
}
else if((MoveTimeCnt > (priod + hc_time) )&&( MoveTimeCnt < (2.0 * priod + hc_time)))
{
Normalization = (float)(MoveTimeCnt- priod ) / priod; //对单摆周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
x1_error = need_x2 - new_x1 ;
y1_error = need_y2 - new_y1;
set_x = new_x1 + (x1_error* Normalization);// x = 254 ,y = 116;
set_y = new_y1 + (y1_error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.02);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.02);
PID_M2_SetKd(100);
}
}
else if(MoveTimeCnt<= 2.0 * priod )
{
Normalization = (float)MoveTimeCnt / 2.0 * priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
x1_error = need_x1 - need_x2; //209-209
y1_error = need_y1 - need_y2; // 182-71
set_x = need_x1 - (x1_error * Normalization);// x = 254 ,y = 116;
set_y = need_y1 - (y1_error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(20.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(20.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*----------------------180------------b到c-----------------------*/
if(((need_x1-need_x2)==180)||((need_x1-need_x2)==-180)||((need_y1-need_y2)==180)||((need_y1-need_y2)==-180))
{
if((MoveTimeCnt > (2.0 * priod+ hc_time) )&&( MoveTimeCnt < (3.0 * priod+ hc_time)))
{
Normalization = (float)(MoveTimeCnt- (2.0 * priod- hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
new_x2 = (need_x2 + need_x3)/2.0 ; //209-209
new_y2 = (need_y2 + need_y3)/2.0 ; //209-209
new_x2= (new_x2 + 163) /2.0; //209
new_y2 = (new_y2 + 117)/2.0 ; //117
x2_2error = new_x2 - need_x2; //163-250 = -87
y2_2error = new_y2 - need_y2; //72-28 = 44
set_x = need_x2 + (x2_2error * Normalization); //250 + -87 = 163
set_y = need_y2 + (y2_2error * Normalization) ;//26 + 44 = 70
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.02);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.02);
PID_M2_SetKd(100);
}
else if((MoveTimeCnt > (3.0 * priod + hc_time))&&( MoveTimeCnt < (4.0 * priod+ hc_time)))
{
Normalization = (float)(MoveTimeCnt- (3.0*priod- hc_time) ) / priod; //对单摆周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
x2_error = need_x3 - new_x2 ;
y2_error = need_y3 - new_y2;
set_x = new_x2 + (x2_error* Normalization);// x = 254 ,y = 116;
set_y = new_y2 + (y2_error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.02);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.02);
PID_M2_SetKd(100);
}
}
else if((MoveTimeCnt > (2.0 * priod + hc_time))&&(MoveTimeCnt < (4.0 * priod + hc_time)))
{
Normalization = (float)(MoveTimeCnt-(2.0 * priod+ hc_time)) / 2.0 * priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
x2_error = need_x2 - need_x3; // 209- 118
y2_error = need_y2 - need_y3; // 71 -71
set_x = need_x2 - (x2_error * Normalization);// x = 72 ,y = 28;
set_y = need_y2 - (y2_error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(20.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(20.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*----------------------180------------c到d-----------------------*/
if(((need_x1-need_x2)==180)||((need_x1-need_x2)==-180)||((need_y1-need_y2)==180)||((need_y1-need_y2)==-180))
{
if((MoveTimeCnt > (4.0 * priod+ hc_time) )&&( MoveTimeCnt < (5.0 * priod+ hc_time)))
{
Normalization = (float)(MoveTimeCnt- (4.0 * priod- hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
new_x3 = (need_x3 + need_x4)/2.0 ; //209-209
new_y3 = (need_y3 + need_y4)/2.0 ; //209-209
new_x3= (new_x3 + 163) /2.0; //209
new_y3 = (new_y3 + 117)/2.0 ; //117
x3_3error = new_x3 - need_x3; //163-250 = -87
y3_3error = new_y3 - need_y3; //72-28 = 44
set_x = need_x3 + (x3_3error * Normalization); //250 + -87 = 163
set_y = need_y3 + (y3_3error * Normalization) ;//26 + 44 = 70
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.02);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.02);
PID_M2_SetKd(100);
}
else if((MoveTimeCnt > (5.0 * priod + hc_time))&&( MoveTimeCnt < (6.0 * priod+ hc_time)))
{
Normalization = (float)(MoveTimeCnt- (5.0 * priod - hc_time)) / priod; //对单摆周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
x3_error = need_x4 - new_x3 ;
y3_error = need_y4 - new_y3;
set_x = new_x3 + (x3_error* Normalization);// x = 254 ,y = 116;
set_y = new_y3 + (y3_error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.02);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.02);
PID_M2_SetKd(100);
}
}
else if(MoveTimeCnt> (6.0 * priod + hc_time ))
{
Normalization = (float)(MoveTimeCnt-(4.0 * priod+ hc_time) ) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
x3_error = need_x3 - need_x4; //118-118
y3_error = need_y3 - need_y4; //73 - 182
set_x = need_x3 - (x3_error * Normalization);// x = 163 ,y = 25;
set_y = need_y3 - (y3_error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(20.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(20.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
PWM1=PID_M1_PosLocCalc((float)Resured.x);
PWM2=PID_M2_PosLocCalc((float)Resured.y);
if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX;
if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
kz_hs(PWM1,PWM2);//控制函数
}
void MODE7(void) //任务七转 3 圈回 9
{
const float priod = 1000.0; //周期(毫秒)
const uint32_t x_1 = 219;
const uint32_t y_1 = 260;
const uint32_t need_x = 121;
const uint32_t need_x_erro = 98;
const uint32_t need_y = 158;
const uint32_t need_y_erro = 102;
static uint32_t MoveTimeCnt = 0;
float Normalization = 0.0;
float set_x,set_y;
float hc_time = 1500;
MoveTimeCnt += 5; //每5ms运算1次
/*--------------------4-------------------------*/
if(MoveTimeCnt<=priod)
{
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (0.0 * Normalization);// x = 72 ,y = 28;
set_y = 207.0 - (45.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.3);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.2);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R1_1-----------------------*/
else if((MoveTimeCnt>priod)&&(MoveTimeCnt<2.0*priod))
{
Normalization = (float)(MoveTimeCnt-priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (-46.0 * Normalization);// x = 72 ,y = 28;
set_y = 162.0 - (45.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R1_2-----------------------*/
else if((MoveTimeCnt>2.0*priod)&&(MoveTimeCnt<3.0*priod))
{
Normalization = (float)(MoveTimeCnt-2.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 209.0 - (46.0 * Normalization);// x = 72 ,y = 28;
set_y = 117.0 - (46.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R1_3-----------------------*/
else if((MoveTimeCnt>(4.0*priod))&&(MoveTimeCnt<(5.0*priod))) //4
{
Normalization = (float)(MoveTimeCnt-4.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (46.0 * Normalization);// x = 72 ,y = 28;
set_y = 71.0 - (-47.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R1_4-----------------------*/
else if((MoveTimeCnt>5.0*priod)&&(MoveTimeCnt<6.0*priod))
{
Normalization = (float)(MoveTimeCnt-5.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 117.0 - (-46.0 * Normalization);// x = 72 ,y = 28;
set_y = 118.0 - (-47.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R2_1-----------------------*/
else if((MoveTimeCnt>6.0*priod)&&(MoveTimeCnt<7.0*priod))
{
Normalization = (float)(MoveTimeCnt-6.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (-46.0 * Normalization);// x = 72 ,y = 28;
set_y = 162.0 - (45.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R2_2-----------------------*/
else if((MoveTimeCnt>7.0*priod)&&(MoveTimeCnt<8.0*priod))
{
Normalization = (float)(MoveTimeCnt-7.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 209.0 - (46.0 * Normalization);// x = 72 ,y = 28;
set_y = 117.0 - (46.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R2_3-----------------------*/
else if((MoveTimeCnt>(8.0*priod))&&(MoveTimeCnt<(9.0*priod))) //4
{
Normalization = (float)(MoveTimeCnt-8.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (46.0 * Normalization);// x = 72 ,y = 28;
set_y = 71.0 - (-47.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R2_4-----------------------*/
else if((MoveTimeCnt>9.0*priod)&&(MoveTimeCnt<10.0*priod))
{
Normalization = (float)(MoveTimeCnt-9.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 117.0 - (-46.0 * Normalization);// x = 72 ,y = 28;
set_y = 118.0 - (-47.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R3_1-----------------------*/
else if((MoveTimeCnt>10.0*priod)&&(MoveTimeCnt<11.0*priod))
{
Normalization = (float)(MoveTimeCnt-10.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (-46.0 * Normalization);// x = 72 ,y = 28;
set_y = 162.0 - (45.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R3_2-----------------------*/
else if((MoveTimeCnt>11.0*priod)&&(MoveTimeCnt<12.0*priod))
{
Normalization = (float)(MoveTimeCnt-11.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 209.0 - (46.0 * Normalization);// x = 72 ,y = 28;
set_y = 117.0 - (46.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R3_3-----------------------*/
else if((MoveTimeCnt>(12.0*priod))&&(MoveTimeCnt<(13.0*priod))) //4
{
Normalization = (float)(MoveTimeCnt-12.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (46.0 * Normalization);// x = 72 ,y = 28;
set_y = 71.0 - (-47.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R3_4-----------------------*/
else if((MoveTimeCnt>13.0*priod)&&(MoveTimeCnt<14.0*priod))
{
Normalization = (float)(MoveTimeCnt-13.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 117.0 - (-46.0 * Normalization);// x = 72 ,y = 28;
set_y = 118.0 - (-47.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R4_1-----------------------*/
else if((MoveTimeCnt>14.0*priod)&&(MoveTimeCnt<15.0*priod))
{
Normalization = (float)(MoveTimeCnt-14.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (-46.0 * Normalization);// x = 72 ,y = 28;
set_y = 162.0 - (45.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------R4_2-----------------------*/
else if((MoveTimeCnt>15.0*priod)&&(MoveTimeCnt<16.0*priod))
{
Normalization = (float)(MoveTimeCnt-15.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 209.0 - (46.0 * Normalization);// x = 72 ,y = 28;
set_y = 117.0 - (46.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
/*---------------------回九-----------------------*/
else if((MoveTimeCnt>16.0*priod)&&(MoveTimeCnt<17.0*priod))
{
Normalization = (float)(MoveTimeCnt-16.0*priod) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
set_x = 163.0 - (91.0 * Normalization);// x = 72 ,y = 28;
set_y = 71.0 - (43.0 * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(6.2);
PID_M1_SetKi(0.01);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(6.1);
PID_M2_SetKi(0.01);
PID_M2_SetKd(100);
}
PWM1=PID_M1_PosLocCalc((float)Resured.x);
PWM2=PID_M2_PosLocCalc((float)Resured.y);
if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX;
if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
kz_hs(PWM1,PWM2);
}
void MODE8(void) //发挥
{
// float sx_1need = 209.0;
// float sx_2need = 254.0;
// float sx_3need = 209.0;
// float sx_4need = 163.0;
// float sx_5need = 117.0;
// float sx_6need = 70.0;
// float sx_7need = 118.0;
//
// float sy_1need = 71.0 ;
// float sy_2need = 116.0;
// float sy_3need = 161.0;
// float sy_4need = 117.0;
// float sy_5need = 71.0;
// float sy_6need = 118.0;
// float sy_7need = 162.0;
const float priod = 2000.0; //周期(毫秒)
const uint32_t x_1 = 219;
const uint32_t y_1 = 260;
const uint32_t need_x = 121;
const uint32_t need_x_erro = 98;
const uint32_t need_y = 158;
const uint32_t need_y_erro = 102;
static uint32_t MoveTimeCnt = 0;
float Normalization = 0.0;
float set_x,set_y;
float hc_time = 500;
MoveTimeCnt += 5; //每5ms运算1次
/*-----------------A 到 B ----------------------------*/
if(MoveTimeCnt<=priod)
{
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
sx_1error = sx_1need - sx_2need; //209-209
sy_1error = sy_1need - sy_2need; // 182-71
set_x = sx_1need - (sx_1error * Normalization);// x = 254 ,y = 116;
set_y = sy_1need - (sy_1error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
/*-----------------B 到 C ----------------------------*/
else if((MoveTimeCnt > (priod+hc_time))&&(MoveTimeCnt < (2.0 * priod + hc_time)))
{
Normalization = (float)(MoveTimeCnt-(priod+hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
sx_2error = sx_2need - sx_3need; //209-209
sy_2error = sy_2need - sy_3need; // 182-71
set_x = sx_2need - (sx_2error * Normalization);// x = 254 ,y = 116;
set_y = sy_2need - (sy_2error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
/*-----------------C 到 D ----------------------------*/
else if((MoveTimeCnt > (2.0* priod+hc_time))&&(MoveTimeCnt < (3.0 * priod + hc_time)))
{
Normalization = (float)(MoveTimeCnt-(2.0 * priod + hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
sx_3error = sx_3need - sx_4need; //209-209
sy_3error = sy_3need - sy_4need; // 182-71
set_x = sx_3need - (sx_3error * Normalization);// x = 254 ,y = 116;
set_y = sy_3need - (sy_3error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
else if((MoveTimeCnt > (3.0* priod+hc_time))&&(MoveTimeCnt < (4.0 * priod + hc_time)))
{
Normalization = (float)(MoveTimeCnt-(3.0 * priod + hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
sx_4error = sx_4need - sx_5need; //209-209
sy_4error = sy_4need - sy_5need; // 182-71
set_x = sx_4need - (sx_4error * Normalization);// x = 254 ,y = 116;
set_y = sy_4need - (sy_4error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
else if((MoveTimeCnt > (4.0* priod+hc_time))&&(MoveTimeCnt < (5.0 * priod + hc_time)))
{
Normalization = (float)(MoveTimeCnt-(4.0 * priod + hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
sx_5error = sx_5need - sx_6need; //209-209
sy_5error = sy_5need - sy_6need; // 182-71
set_x = sx_5need - (sx_5error * Normalization);// x = 254 ,y = 116;
set_y = sy_5need - (sy_5error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
else if((MoveTimeCnt > (5.0* priod+hc_time))&&(MoveTimeCnt < (6.0 * priod + hc_time)))
{
Normalization = (float)(MoveTimeCnt-(5.0 * priod + hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
sx_6error = sx_6need - sx_7need; //209-209
sy_6error = sy_6need - sy_7need; // 182-71
set_x = sx_6need - (sx_6error * Normalization);// x = 254 ,y = 116;
set_y = sy_6need - (sy_6error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
PWM1=PID_M1_PosLocCalc((float)Resured.x);
PWM2=PID_M2_PosLocCalc((float)Resured.y);
if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX;
if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
kz_hs(PWM1,PWM2);
}
void MODE9(void) //附加简单任务六
{
const float priod = 2000.0; //周期(毫秒)
const uint32_t x_1 = 219;
const uint32_t y_1 = 260;
const uint32_t need_x = 121;
const uint32_t need_x_erro = 98;
const uint32_t need_y = 158;
const uint32_t need_y_erro = 102;
static uint32_t MoveTimeCnt = 0;
float Normalization = 0.0;
float set_x,set_y;
float hc_time = 100;
MoveTimeCnt += 5;
if(MoveTimeCnt<=priod)
{
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
x1_error = need_x1 - need_x2; //209-209
y1_error = need_y1 - need_y2; // 182-71
set_x = need_x1 - (x1_error * Normalization);// x = 254 ,y = 116;
set_y = need_y1 - (y1_error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
/*-----------------B 到 C ----------------------------*/
else if((MoveTimeCnt > (priod+hc_time))&&(MoveTimeCnt < (2.0 * priod + hc_time)))
{
Normalization = (float)(MoveTimeCnt-(priod+hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
x2_error = need_x2 - need_x3; // 209- 118
y2_error = need_y2 - need_y3; // 71 -71
set_x = need_x2 - (x2_error * Normalization);// x = 72 ,y = 28;
set_y = need_y2 - (y2_error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
/*-----------------C 到 D ----------------------------*/
else if(MoveTimeCnt>( 2.0 * priod + hc_time ))
{
Normalization = (float)(MoveTimeCnt-(2.0 * priod + hc_time)) / priod; //对板球周期归一化
if(Normalization>=1.0)
{
Normalization = 1.0;
}
x3_error = need_x3 - need_x4; //118-118
y3_error = need_y3 - need_y4; //73 - 182
set_x = need_x3 - (x3_error * Normalization);// x = 163 ,y = 25;
set_y = need_y3 - (y3_error * Normalization) ;
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.04);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.04);
PID_M2_SetKd(100);
}
PWM1=PID_M1_PosLocCalc((float)Resured.x);
PWM2=PID_M2_PosLocCalc((float)Resured.y);
if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX; //100
if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
kz_hs(PWM1,PWM2);//控制函数
}
void MODE10(void)//无效
{
const float priod = 50000.0; //周期(毫秒)
const uint32_t x_1 = 219;
const uint32_t y_1 = 260;
const uint32_t need_x = 121;
const uint32_t need_x_erro = 98;
const uint32_t need_y = 158;
const uint32_t need_y_erro = 102;
static uint32_t MoveTimeCnt = 0;
float Normalization = 0.0;
float A = 26.0;
float phase = 0.0;
float Omega = 0.0;
float set_x = 0.0;
float set_y = 0.0;
MoveTimeCnt += 5;
Normalization = (float)MoveTimeCnt / priod; //对板球周期归一化
Omega = 2.0*3.14159*Normalization; //对2π进行归一化处理
phase = 3.141592/2.0; //逆时针旋转相位差90°
set_x = A*sin(Omega) + 163.0; //计算出X方向当前摆角
set_y = A*sin(Omega+phase) + 117.0; //计算出Y方向当前摆角
PID_M1_SetPoint(set_x); //x的期望值
PID_M1_SetKp(8.2);
PID_M1_SetKi(0.02);
PID_M1_SetKd(100);
PID_M2_SetPoint(set_y); //y的期望值
PID_M2_SetKp(8.1);
PID_M2_SetKi(0.02);
PID_M2_SetKd(100);
PWM1=PID_M1_PosLocCalc((float)Resured.x);
PWM2=PID_M2_PosLocCalc((float)Resured.y);
if(PWM1 > M1_PWMMAX) PWM1=M1_PWMMAX; //100
if(PWM1 < -M1_PWMMAX) PWM1=-M1_PWMMAX;
if(PWM2 > M2_PWMMAX) PWM2=M2_PWMMAX;
if(PWM2< -M2_PWMMAX) PWM2=-M2_PWMMAX;
kz_hs(PWM1,PWM2);//控制函数
}
/*--------------全国电子设计竞赛显示---------------*/
void QGDZJS(void)
{
OLED_Init(); //初始化OLED
OLED_Showone16x16(0,0,1,0); //全
OLED_Showone16x16(16,0,1,1); //国
OLED_Showone16x16(32,0,1,2); //电
OLED_Showone16x16(48,0,1,3); //子
OLED_Showone16x16(64,0,1,4); //设
OLED_Showone16x16(80,0,1,5); //计
OLED_Showone16x16(96,0,1,6); //竞
OLED_Showone16x16(112,0,1,7); //赛
OLED_ShowChar(0,16,'X',16,1); //显示一个字符
OLED_ShowChar(0,32,'Y',16,1); //显示一个字符
OLED_Refresh_Gram(); //oled显示函数
}
/*---------------------摄像头初始化----------------*/
void show_7670(void)
{
u8 lightmode=0,saturation=2,contrast=2;
u8 effect=0;
POINT_COLOR=RED; //设置字体为红色
LCD_ShowString(30,50,200,16,16,"ELITE STM32F103 ^_^");
LCD_ShowString(30,210,200,16,16,"OV7670 Init...");
while(OV7670_Init())//初始化OV7670
{
LCD_ShowString(30,210,200,16,16,"OV7670 Error!!");
delay_ms(200);
LCD_Fill(30,210,239,246,WHITE);
delay_ms(200);
}
LCD_ShowString(30,210,200,16,16,"OV7670 Init OK");
delay_ms(1500);
OV7670_Light_Mode(lightmode);
OV7670_Color_Saturation(saturation);
OV7670_Contrast(contrast);
OV7670_Special_Effects(effect);
//TIM6_Int_Init(10000,7199); //10Khz计数频率,1秒钟中断
EXTI8_Init(); //使能定时器捕获
OV7670_Window_Set(12,176,240,320); //设置窗口
OV7670_CS=0;
LCD_Clear(BLACK);
}
/*---------------------更新LCD显示-----------------*/
void camera_refresh(void)
{
// TARGET_CONDI Conditionred={215,240,20,240,30,160,30,30,320,240}; //红色1 API参数 hsl的阈值，识别时用的
// unsigned char H_MIN;//目标最小色调
// unsigned char H_MAX;//目标最大色调
//
// unsigned char S_MIN;//目标最小饱和度
// unsigned char S_MAX;//目标最大饱和度
//
// unsigned char L_MIN;//目标最小亮度
// unsigned char L_MAX;//目标最大亮度
//
// unsigned int WIDTH_MIN;//目标最小宽度
// unsigned int HIGHT_MIN;//目标最小高度
// unsigned int WIDTH_MAX;//目标最大宽度
// unsigned int HIGHT_MAX;//目标最大高度
TARGET_CONDI Conditionred={215,240,20,240,30,160,15,15,320,240}; //红色1 API参数 hsl的阈值，识别时用的
u32 j;
u16 color;
if(ov_sta)//有帧中断更新？
{
LCD_Scan_Dir(U2D_L2R); //从上到下,从左到右
if(lcddev.id==0X1963)LCD_Set_Window((lcddev.width-240)/2,(lcddev.height-320)/2,240,320);//将显示区域设置到屏幕中央
else if(lcddev.id==0X5510||lcddev.id==0X5310)LCD_Set_Window((lcddev.width-320)/2,(lcddev.height-240)/2,320,240);//将显示区域设置到屏幕中央
LCD_WriteRAM_Prepare(); //开始写入GRAM
OV7670_RRST=0; //开始复位读指针
OV7670_RCK_L;
OV7670_RCK_H;
OV7670_RCK_L;
OV7670_RRST=1; //复位读指针结束
OV7670_RCK_H;
for(j=0;j<76800;j++)
{
OV7670_RCK_L;
color=GPIOC->IDR&0XFF; //读数据
OV7670_RCK_H;
color<<=8;
OV7670_RCK_L;
color|=GPIOC->IDR&0XFF; //读数据
OV7670_RCK_H;
LCD->LCD_RAM=color;
}
if(Trace(&Conditionred,&Resured) ) //API
{
LCD_Fillred(Resured.x-Resured.w/2,Resured.y-Resured.h/2,Resured.x+Resured.w/2,Resured.y-Resured.h/2+1,0xf800);//u16 x,u16 y,u16 width,u16 hight,u16 Color
LCD_Fillred(Resured.x-Resured.w/2,Resured.y-Resured.h/2,Resured.x-Resured.w/2+1,Resured.y+Resured.h/2,0xf800);
LCD_Fillred(Resured.x-Resured.w/2,Resured.y+Resured.h/2,Resured.x+Resured.w/2,Resured.y+Resured.h/2+1,0xf800);
LCD_Fillred(Resured.x+Resured.w/2,Resured.y-Resured.h/2,Resured.x+Resured.w/2+1,Resured.y+Resured.h/2,0xf800);
LCD_Fillred(Resured.x-2,Resured.y-2,Resured.x+2,Resured.y+2,0xf800);
r=Resured.x;
y=Resured.y;
}
//printf("%d %d\r\n ",r,y);
ov_sta=0; //清零帧中断标记
ov_frame++;
LCD_Scan_Dir(DFT_SCAN_DIR); //恢复默认扫描方向
}
}
/*---------------------主函数----------------------*/
int main(void)
{
u8 msgbuf[15]; //消息缓存区
u8 i=0;
u8 tm=0;
delay_init(); //延时函数初始化
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置中断优先级分组为组2：2位抢占优先级，2位响应优先级
uart_init(115200); //串口初始化为 115200
LED_Init(); //初始化与LED连接的硬件接口
LCD_Init(); //初始化LCD
io_in_out_Init(); // io口输入输出初始化
TIM4_PWM_Init(3599,49); //pwm 20khz
show_7670(); //7670和lcd的相关配置
QGDZJS(); //OLED初始化并且显示全国大学生电子设计竞赛字样
TIM_SetCompare1(TIM4,1235);// 控制前后舵机初始化角度
TIM_SetCompare4(TIM4,1275);// 控制左右舵机初始化角度
while(1)
{
/*--------------选择、修改、确定 ...都是通过 4个自锁开关和 4个按键来选择。------*/
PWMMENUS(); //修改 A B C D 四个区域出发到结束的位置
wkqwz(); //未启动位置作用：在还没启动任务的时候先观察小球在的位置是否满足要求
MENUS(); //总任务选择
camera_refresh();//更新显示
}
}
/*---------------------定时器二--------------------*/
void TIM2_IRQHandler(void) //TIM2中断 TIM2的初始化在总任务选择里
{
if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源
{
TIM_ClearITPendingBit(TIM2, TIM_IT_Update ); //清除TIMx的中断待处理位:TIM 中断源
if(((Resured.x>5)&&(Resured.x<315))&&((Resured.y>5)&&(Resured.y<235)))
{
switch(RWXZ)
{
case 1 : MODE1(); break;
case 2 : MODE2(); break;
case 3 : MODE3(); break;
case 4 : MODE4(); break;
case 5 : MODE5(); break;
case 6 : MODE6(); break;
case 7 : MODE7(); break;
case 8 : MODE8(); break;
case 9 : MODE9(); break;
case 10 : MODE10(); break;
}
}
}
}

复制代码