飞思卡尔电磁组程序

电磁组

// 控制策略car.cpp : Defines the initialization routines for the DLL.
//
#include <math.h>
#include "stdafx.h"
#include "控制策略car.h"
//#include "Judgement.H"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
extern "C" _declspec(dllexport)
void Judge( unsigned short Speed,
unsigned char* SensorData, unsigned short SensorCount,
unsigned char* CCDData, int CCDWidth, int CCDHeight,
unsigned short* MotorPWM, unsigned short* SteerPWM);

extern "C" _declspec(dllexport)
void Reset();


void Init_ECT(void);
void Init_PWMout(void);
void Init_SCI(void);
void Transmit_Sci(unsigned int);
unsigned char Receive_Sci(void);
void SignalProcess();
void PWMout(int, int,int);
void Memoryroad(void);
void Roadplay(void);
void PIDinit(void);
void delay(int cnt);
void SteerContral(void);
void Circles(void);
int Runroad_time=0/*第一圈时间*/,Roadtime,Runroad_time2=0/*第二圈时间*/;
int Inbentcount=0,Outbentcount=0; //弯道数目
int Speedout,Steerout,Speedfout,allspeed;
int AD,cnt,flag_delay=0;
int f0=0,f1=0,f2=0,f3=0,f4=0,f5=0,f6=0,f7=0,f8=0,f9=0,f10=0;
unsigned char ch_0,ch_1,ch_2,ch_3,ch_4,ch_5,ch_6,ch_7,ch_8,ch_9,ch_10,ch_11,ch_12;
static int oldDirection,Direction, Velocity,VelocityFz; //方向速度变量16位
int flag0,flag1,flag2,flag3,flag4,flag5,flag6,flag7,flag8,flag9,flag10,Flagstart;
unsigned char Jump=0,Jump0=0,Jump1=0,Jump2=0,Jump3=0,Jump4=0,Jump5=0,Jump6=0,Jump7=0,Jump8=0,Jump9=0;
unsigned char PulseCnt,PORTB;
unsigned int sudu,Turn=11,bjqushu,oldbjqushu,qushu;
unsigned int jstime,SYSCLOCK=0,sctime=0;
int Startline,Circle=0,line=0,line0=0,line2=0,line22=0,line3=0,line33=0,aa0=0,aa1=0,aa2=0;
int Zhuanjiao[]={-250,-235,-215,-195,-175,-150,-120,-90,-60,-30,0,30,60,90,120,150,175,195,215,235,250};
int HiShuruSudu[]={15,20,20,20,22,22,25,25,30,30,40,30,30,25,25,22,22,20,20,20,15};
int ShuruSudu[]={15,20,20,20,22,22,25,25,25,30,30,30,25,25,25,22,22,20,20,20,15};
int ss=0,insstime[50],outsstime[50],oldsstime=0,ssflag=0,oldssflag=0,sstime_flag2=0,sstime_flag1=0;
int oldss;
/***************************PID*************************/
//速度PID
typedef struct {
int NextPoint;
int ThisPoint; //设定目标 Desired value
int Kp; //比例常数 Proportional Const
int Ki; //积分常数 Integral Const
int Kd; //微分常数 Derivative Const
int LastError; //Error[-1]
int PrevError; //Error[-2]
int SumError; //Sums of Errors
} speedPID ;
speedPID sPID={0,0,1500,600,60,0,0,0}; /* *pp=&sPID; */
//角度PID
typedef struct {
int NextPoint;
int ThisPoint; //设定目标 Desired v
alue
int Kp; //比例常数 Proportional Const
int Ki; //积分常数 Integral Const
int Kd; //微分常数 Derivative Const

电磁组

int LastError; //Error[-1]
int PrevError; //Error[-2]
int SumError; //Sums of Errors
} steerPID ;
steerPID DjPID={0,0,1,0,0,0,0,0}; /*, *pp=&sPID; */
int speedPIDCalc (speedPID *pp);
//速度、舵机PID初始化:void PIDinit(void)
/*sPID.ThisPoint=0; //设定目标 Desired value
sPID.Kp=1500; //比例常数 Proportional Const
sPID.Ki=600; //积分常数 Integral Const
sPID.Kd=60; //微分常数 Derivative Const
http://www.77cn.com.cnstError=0; //Error[-1]
sPID.PrevError=0; //Error[-2]
sPID.SumError=0; //Sums of Errors
DjPID.ThisPoint=0; //设定目标 Desired value
DjPID.Kp=1; //比例常数 Proportional Const
DjPID.Ki=0; //积分常数 Integral Const
DjPID.Kd=0; //微分常数 Derivative Const
http://www.77cn.com.cnstError=0; //Error[-1]
DjPID.PrevError=0; //Error[-2]
DjPID.SumError=0; //Sums of Errors
/*************************PID初始化**********************/
unsigned short PWM;
//Flagstart=0;
extern "C" _declspec(dllexport)
void Judge( unsigned short Speed,
unsigned char* SensorData, unsigned short SensorCount,
unsigned char* CCDData, int CCDWidth, int CCDHeight,
unsigned short* MotorPWM, unsigned short* SteerPWM
)
{ SensorCount=11;
sudu=Speed;
ch_0=SensorData[0];
ch_1=SensorData[1];
ch_2=SensorData[2];
ch_3=SensorData[3];
ch_4=SensorData[4];
ch_5=SensorData[5];
ch_6=SensorData[6];
ch_7=SensorData[7];
ch_8=SensorData[8];
ch_9=SensorData[9];
ch_10=SensorData[10];
//ch_11=SensorData[11];
//ch_12=SensorData[12];
if(ch_0<255){
flag0=1;f0=1;
}
if(ch_0>=255){
flag0=0; f0=0;
}
if(ch_1<255){
flag1=300;f1=1;
}
if(ch_1>=255){
flag1=0; f1=0;
}
if(ch_2<255){
flag2=5; f2=1;
}
if(ch_2>=255){
flag2=0; f2=0;
}
if(ch_3<255){
flag3=700;f3=1;
}
if(ch_3>=255){
flag3=0; f3=0;
}
if(ch_4<255){
flag4=9; f4=1;
}
if(ch_4>=255){
flag4=0; f4=0;
}
if(ch_5<255){
flag5=1100;f5=1;
}
if(ch_5>=255){
flag5=0; f5=0;
}
if(ch_6<255){
flag6=13; f6=1;
}
if(ch_6>=255){
flag6=0; f6=0;
}
if(ch_7<255){
flag7=2550;f7=1;
}
if(ch_7>=255){
flag7=0; f7=0;
}
if(ch_8<255){
flag8=17; f8=1;
}
if(ch_8>=255){
flag8=0; f8=0;
}
if(ch_9<255){
flag9=1900;f9=1;
}
if(ch_9>=255){
…… 此处隐藏：6146字，全部文档内容请下载后查看。喜欢就下载吧 ……