pwm.c

机器人小车控制

#include <stdio.h>
#include <ctype.h>
#include "sst89x5x4.h"
#include<math.h> 

#define uchar unsigned char 
#define uint unsigned int 

sbit en1=P1^3; /* L298的Enable A */ 
sbit en2=P1^4; /* L298的Enable B */ 

sbit s1=P1^0; /* L298的Input 1 */ 
sbit s2=P1^1; /* L298的Input 2 */ 
sbit s3=P1^6; /* L298的Input 3 */ 
sbit s4=P3^7; /* L298的Input 4 */ 

sbit sensleft1=P3^2;/*Left1 sensor*/
sbit sensleft2=P3^4;/*Left2 sensor*/
sbit sensleft3=P3^5;/*Left3 sensor*/
sbit sensleft4=P3^3;/*Left4 sensor*/

sbit encode_l=P1^5;/*Left encode*/
sbit encode_r=P1^7;/*Right encode*/

sbit crash=P3^6;/*碰撞开关*/

sbit Start=P2^7;/*启动开关*/

uchar m1=255; /* 电机1速度值 */ 
uchar m2=255; /* 电机2速度值 */ 

sfr16 CCAP4=0xee;/*Right encode*/
sfr16 CCAP2=0xec;/*Left encode*/

xdata uint pcacap1;/*右轮脉冲周期*/
xdata uint pcacap2;/*左轮脉冲周期*/

uint LastCCAP4;
uint LastCCAP2;
uint flag;
uint DeltaPulse;
uint CountRP=0;
uint CountLP=0;
/* 电机控制函数 index-电机号(1,2); speed-电机速度(0-255) */ 
void motor(uchar index, uint speed,int direct) 
{ 

if(index==1) /* 电机1的处理 */ 
{ 

//CR=0;
CL=0x00;
CH=0x00;
CCAPM0=0x42;
CCAPM1=0x42;


CCAP0L=abs(speed); // Set the initial value same as CCAP0H
CCAP0H=abs(speed);
//CR=1; // Start PCA Timer.

if(direct<0) /* 正转 */ 
{ 
s1=0; 
s2=1; 
} 
else /* 不为负数则反转 */ 
{ 
s2=0; 
s1=1; 

} 
} 
if(index==2) /* 电机2的处理 */ 
{ 
//CR=0;
CL=0x00;
CH=0x00;
CCAPM0=0x42;
CCAPM1=0x42;


CCAP1L=abs(speed); // Set the initial value same as CCAP1H
CCAP1H=abs(speed);
//CR=1; // Start PCA Timer.

if(direct<0) /* 电机2的方向控制 */ 
{ 
s3=0; 
s4=1; 
} 
else 
{ 
s4=0;
s3=1; 
 
} 
} 
} 


void delay(uint j) /* 简易延时函数 */ 
{ 
for(j;j>0;j--); 
} 


// 程序通过比较CL寄存器和CCPA0L的值产生脉冲。
// 如果CL >= CCAP0L输出为高，如果CL < CCAP0L输出为低。
// 占空比由CCAP0H的值决定.


void forward(uint lspeed,uint rspeed)
{

CMOD=0x02; // Setup PCA timer
CR=0;
motor(1,rspeed,-1);   //控制右轮
motor(2,lspeed,-1);   //控制左轮
CR=1;
}


void Stop(uint Index)
{
	if(Index==1){         //左电机停
	s3=1;
	s4=1;
	}
    if(Index==2){         //右电机停
	s1=1;
	s2=1;
	}
}

void pca4ini(void)
{
CMOD=0x01;//0000 0000,系统的12分频
CCAPM4=0x21;//0010 0001正边缘捕捉，初始化时打开PCA中断
CCAP4L=0x00;
CCAP4H=0x00;
}

void pca2ini(void)
{
CMOD=0x01;//0000 0000,系统的12分频
CCAPM2=0x21;//0010 0001正边缘捕捉，初始化时打开PCA中断
CCAP2L=0x00;
CCAP2H=0x00;
}


void TurnRight(uint PulseCount)
{
Stop(1);//右电机停
motor(1,1,-1);
flag=1;
LastCCAP4=CountRP;
	while(flag)
	{DeltaPulse=CountRP-LastCCAP4;
     if(DeltaPulse>PulseCount)
       {
        flag=0;
       }
	
	}
Stop(2);
}



void TurnLeft(uint PulseCount)
{
Stop(2);//左电机停
motor(2,1,-1);
flag=1;
LastCCAP2=CountLP;
	while(flag)
	{DeltaPulse=CountLP-LastCCAP2;
     if(DeltaPulse>PulseCount)
       {
        flag=0;
       }
	
	}
Stop(1);
}



void TurnBack(uint PulseCount)
{
flag=1;
motor(1,1,1);
motor(2,1,-1);
LastCCAP4=CountRP;
	while(flag)
	{DeltaPulse=CountRP-LastCCAP4;
     if(DeltaPulse>PulseCount)
       {
        flag=0;
       }
	
	}
Stop(1);
Stop(2);
}

void main()
{
while(Start==1)
{
}

    pca4ini();
    pca2ini();
    EA=1;//全局中断打开
    EC=1;//PCA中断打开
while(1)
{
forward(1,1);
delay(50000);
Stop(1);
Stop(2);
delay(50000);
TurnLeft(30);
delay(50000);
TurnRight(30);
delay(50000);
forward(1,1);
TurnBack(30);
P1=1;

	/*********
    if(sensleft2==1&sensleft3==1)//全白
	{
	forward(255,255);
	delay(5000);
	}
	if(sensleft2==0&sensleft3==1)//偏右
	{
	forward(100,1);
	delay(250);
	 }   
	if(sensleft2==1&sensleft3==0)//偏左
	{
	forward(1,100);
	delay(250);
	}
	if(sensleft2==0&sensleft3==0)//全黑
	{
	forward(1,1);
	delay(250);
	}
*//////////////////////////////
}
}




void PCA_ISR(void) interrupt 6 using 1
{
static xdata uint tmpcnt1=0;
static xdata uint tmpcnt2=0;	
    EC=0;//关闭PCA中断
	
		if(CCF4){
          CountRP++;
		  pcacap1=CCAP4-tmpcnt1;
		  tmpcnt1=CCAP4;
		  CCF4=0;
		}
       
        if(CCF2){
          CountLP++;
		  pcacap2=CCAP2-tmpcnt2;
		  tmpcnt2=CCAP2;
		  CCF2=0;
		}
    EC=1;//打开PCA中断
}