飞思卡尔光电管 舵机，电机控制程序 1

mtzs2010

先看一下吧。。。。

jys

谢谢！

shao1505

好      。

maninthemirror

转的还要分

jojocn

拿走了

pr1220

谢谢

pr1220

谢谢

小强小车

坑爹呢这是

小虾米

我这只有舵机，摄像头，和主程序，但是只是框架，要等调试。可以参考哈
#ifndef   _WATCH_H_ //头文件保护
#define   _WATCH_H_
#ifndef   ULONG
#define   ULONG  unsigned long
#define   UCHAR  unsigned char
#define   UINT  unsigned int
#endif
#define   ROAD_MAX 10
////////////////代设值
#define   MP0   //捕捉象素引脚
#define   HREF   //捕捉行中断引脚
//////////////////
static unsigned char Colors[10][48]; //象素数组
static unsigned char JudgeMax;  //最大象素
static unsigned char JudgeMin;  //最小象素
unsigned char   Over;   //中断函数处理完成
          //使用数组后要清零
        
struct   ROADS
{
unsigned char LeftRight;  //1 left, 0 right 2 表示没检测到黑线 4表示起始线
unsigned int road;   //转向率，0表示直线或则没检测到黑线
}Roads[10];      //没检测到黑线一般为前方有大于90度的弯，
        //行驶一会后数组会更新，不会因此丢掉跑道

unsigned long RoadLength；   //mm单位
void   GetRoads();    //取象素
void   ProcessRoads();   //取转向率
void   delay(UINT);   //延时函数
void   ColorToRoads(UINT);  //2值化与赋值
#endif   //_WATCH_H_
以下为实现文件：
//watch.c
#include <hidef.h>      /* common defines and macros */
#include <MC9S12XS128.h>     /* derivative information */
#include  "watch.h"

#pragma CODE_SEG NON_BANKED
#pragma TRAP_PROC
void GetRoads()      //场中断服务函数
{
UINT i;
UINT Href= 0;
delay(5);
while(HREF)
{
  if(Href>= 270)    //点读取完毕，退出循环
   break;
  if((270-Href)%21!= 0 || Href< 60)  //取60-270行中平均间隔的行数，共30行
  {
   Href++;
   while(HREF);   //等待此行扫描结束
   while(!HREF);   //等待新行开始扫描
   continue;
  }
  delay(...);     //根据调式结果具体设置
  for(i= 0; i< 48; i++)
  {
   Colors[((270-Href)%21)]= MP0;   //读取引脚数据，后来会根据串口还是并口读取做相应修改
   if(MP0> judgeMax)   //取最大最小值
    JudgeMax= MP0;
   if(MP0< JudgeMin)
    JudgeMin= MP0;
   delay(...);    //具体设置
  }
  Href++;
  while(HREF);
  while(!HREF);    //等待行中断结束
}
Over= 1;       //中断函数处理完成
}

#pragma CODE_SEG DEFAULT

void ColorToRoads(UINT Href)
{
UINT i;
UINT temp;      //用于起始线判断
UINT temp2;     
UINT temp3;
UCHAR Judge;      //平均厥值
UINT j= 0;
UINT m_nCount= 0;

Judge= (JudgeMax+ JydgeMin)/2;
Roads[Href].road= 0;
Roads[Href].LeftRight= 0;
for(i= 0; i<48; i+= 3)
{
  if(Colors<= Judge||
     Colors[i+ 1]<= Judge||
     Colors[i+ 2]<= Judge)
  {
   Roads[Href].road&= (0x8000>> (UCHAR)j); //相应位赋值为1
   m_nCount++;
  }
  j++;
}
if(Roads[Href].road== 0x00)
{
  Roads[Href].LeftRight= 0x02;   //没有捕捉到黑线设置2
}
if(m_nCount> 1)       //若黑点数大于1，从左向右数出连续的点中的中间点
{
  j= 0;
  i= 0;
  while(!(Roads[Href].road& (0x8000>>(UCHAR)i)))
  {
   i++;
  }
  j= i;
  while((Roads[Href].road& (0x8000>>(UCHAR)i))
  {
   i++;
   if(i> 15)
    break;
  }
  if(i< 15)      //起始线判断
  {
   temp= i;
   while(!(Roads[Href].road& (0x8000>>(UCHAR)temp)))
   {
    temp++;
    temp2= temp;
   }
   if(temp< 15)
   {
    while((Roads[Href].road& (0x8000>>(UCHAR)temp))
    {
     temp++;
     temp3= temp;
     if(temp> 15)
      break;
    }
    if(temp< 15)
    {
     while(!(Roads[Href].road& (0x8000>>(UCHAR)temp)))
     {
      temp++;
     }
     if(temp< 15)
     {
      Roads[Href].LeftRight= 0x04;
      Roads[Href].road&= 0x8000>>(UCHAR)((temp2+ temp3)/2);
      return;
     }
    }
   }
  }
  Roads[Href].road&= 0x8000>>(UCHAR)((i+j)/2);
}
}

void ProcessRoads()      //路径处理函数，在主函数中调用
{
int  i;
for (i= 0;i< 10; i++)
{
  ColorToRoads(i);
}
}

void delay(UINT m)      //延时函数根据调试结果相应做改动
{
UINT i;
UINT j;
for(i= 0; i< m; i++)
{
  for(j= 0; j< 200; j++)
}
}

再下面就是主函数实现文件：

#include <hidef.h>      /* common defines and macros */
#include <MC9S12XS128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"

#include "watch.h"  //摄像头
#include "TurnAround.h"   //舵机
//#include "..."   //直流电机
//#include "..."   //测速模块

#ifndef TRUE
#define TRUE 1
#define FLASE 0
#endif

#define ULONG unsigned long
#define UCHAR unsigned char
#define UINT  unsigned int

#define LITTLE
#define LARGE
//////////////////////全局变量
UCHAR m_nCount;    //圈数计算   
UCHAR m_nCount2;

////////////////////////////////函数定义
void init();       //初始化函数 待修改
UCHAR CheckRoad(UCHAR*，UCHAR*);  //第1参数返回第几个元素开始
          //第2个参数返回第几个元素结束
          //0 直线， 0x11有小弯道， 0x21大弯道， 0x31终点

void CarRun(UCHAR, UCHAR, UCHAR); //小车行驶函数   

#pragma CODE_SEG NON_BANKED
#pragma TRAP_PROC
void Int_TimerOverFlow(void)  //32MHz TCNT 50000--200ms
{
static int m= 0;
if(m== 15)   //3秒
{
  m_nCount2= 0;
  TSCR1_TEN= 0; //关定时器
  TFLG2_TOF  = 1;
}
m++;
}

#pragma CODE_SEG DEFAULT


void TimerInit()
{
    TIOS= 0x00;
    TSCR2_PR= 7;   //默认情况下是32MHz bus clock,所以分频后主时钟为 0.25MHz
    TSCR2_TOI= 1;   //OverFlow方式
    TCNT= 65535- 50000;
    TSCR1_TEN= 1;   //开定时器
}

////////////////////////////////
void main()      //主程序
{
UCHAR ret;
UCHAR number1= 0;
UCAHR number2= 0;
init();
while(1)
{
  while(over);    //等待扫描完毕
  over= 0;
  IRQCR_IRQEN= 0;    //关外部中断IRQ
  ProcessRoads();    //interface of watch.h
  ret= CheckRoad(&number1, &number2);  //检测路径
  if(ret== 0x31)
   break;
  CarRun(number1, number2, ret);
}
PWME_PWME1= 0;     //关闭PWM
while(1);
}

void init()
{
IRQCR_IRQE= 1;    //下降沿触发IRQ
PE1= 1;
EnableInterrupts;   //开总中断
INTCR_IRQEN= 1;
PWMInit();     //舵机初始化
Speed(MAX);
}

UCHAR CheckRoad(UCHAR* number1, UCHAR number2)
{
int i;
int sub= 0;
int psub= 0;
int temp;
bool flag;
UCHAR ret;
i= 0;
m_nCount2++;
while(!Roads.road)
{
  i++;
}
*number1= i;
if(Roads.road>= Roads[i+ 1].road)
{
  flag= TRUE;
  psub= Roads[0].road- Roads[1].road;
}
else
{
  flag= FALSE;
  psub= Roads[1].road- Roads[0].road;
}
for(; i< 9; i++)
{
  if(Roads.LeftRight== 0x04)
  {
   if(m_nCount2== 0)
   {
    m_nCount2= 1;
    m_nCount++;
    TimerInit();
   }
  }
  if(m_nCount== 3)
  {
   Speed(0);
   ret= 0x31;
   goto _RET;
  }
  if(Roads[i+ 1].road== 0)
  {
   break;
  }
  if(flag)
  {
   sub= Roads.road- Roads[i+ 1].road;
  }
  else
  {
   sub= Roads[i+ 1].road- Roads.road;
  }
  sub>>= (UCHAR)i;
  if(sub>= psub)      //取差值
   temp= sub- psub;
  else
   temp= psub- sub;
  if(temp> LARGE)      //大弯道
  {
   ret= 0x21;
   while(Roads.road!= 0)
   {
    i++;
    *number2= i;
    if(i> 9)
     break;
   }
   goto _RET;
  }
  if(temp> LITTLE)     //小弯道
  {
   ret= 0x11;
   while(Roads.road!= 0)
   {
    i++;
    *number2= i;
    if(i> 9)
     break;
   }
   goto _RET;
  }
  *number2= i;
  psub= sub;
}
_RET:
return ret;
}

void CarRun(UCHAR number1, UCHAR number2, UCHAR ret)
{
int end= Roads[number2].road;
Turn(number2- number1, end- 0x100, Roads[number2].LeftRight, ret);  //舵机函数
IRQCR_IRQEN= 1;     //开IRQ
}
下面为舵机头文件：
//TurnAround.h
#ifndef _TURNAROUND_H_ //头文件保护
#define _TURNAROUND_H_

void Turn(unsigned char, unsigned int, unsigned char, unsigned char); //转角函数
void PWMInit();        //优先初始化

#endif

再以下为实现文件：(表内容待实验后求得)
//TurnAround.c
#include <hidef.h>        /* common defines and macros */
#include <MC9S12XS128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
#include "TurnAround.h"
#include "..."    //直流电机

#ifndef ULONG
#define ULONG unsigned long
#define UCHAR unsigned char
#define UINT unsigned int
#endif

/////////////////////////转角定义
#define RIGHT60  1083
#define RIGHT45  1000
#define RIGHT30  917
#define RIGHT15  833
#define RIGHT5  778

#define MIDDLE  750

#define LEFT5  722
#define LEFT15  667
#define LEFT30  583
#define LEFT45  500
#define LEFT60  417

/////////////////////////速度定义
#define SPEED0     //直线速度
#define SPEED5
#define SPEED15
#define SPEED30
#define SPEED45
#define SPEED60

//PWM查询表 7*10*2
static UINT PWMTable[]=
{
//Left
LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60,
LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60,
LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60,
LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60,
LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60,
LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60,
LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60, LEFT60,
//Right
RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60,
RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60,
RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60,
RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60,
RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60,
RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60,
RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60, RIGHT60
};

小虾米

小虾米 发表于 2012-3-18 00:01
我这只有舵机，摄像头，和主程序，但是只是框架，要等调试。可以参考哈
#ifndef   _WATCH_H_ //头文件保护 ...

我不厚道啊