📄 ctr.c
字号:
//**************************************//
// 源 文 件:Ctr.c //
// 描 述:控制物体作多种运动代码 //
// 主要功能:定点运动 //
// 圆周运动 //
// 沿黑线运动 //
// 编写作者:by Gholt(林青) //
// 编写日期:2005-09-07 //
// 整理日期:2005-10-30 //
//**************************************//
//================================
// IO端口分配:
// L1_IO----IOB0 左光感
// L2_IO----IOB1 上光感
// L3_IO----IOB8 右光感
// L4_IO----IOB10 下光感
// L5_IO----IOB2 中光感
//================================
#include "SPCE061V004.H"
#include "math.h"
#define L1_IO 0x0001
#define L2_IO 0x0002
#define L3_IO 0x0100
#define L4_IO 0x0400
#define L5_IO 0x0004
extern unsigned int DeCnt1,DeCnt2; //电机1、2脉冲延迟计数变量
extern unsigned int M1_Over,M2_Over; //电机1、2停转标志变量
extern unsigned N_step1,N_step2; //电机1、2步进数(1.8度/步进)
int Min_DC=2000; //电机驱动脉冲延迟计数2000（转速较慢）
//画圆、沿黑线运动时:Min_DC=1000（慢速）
//定点运动时:Min_DC=500（快速）
double C1=6.26,C2=6.24; //绕线轴1、2的周长（cm）
double X_Now,Y_Now; //目前物体位置坐标
double X_Set,Y_Set; //设定物体位置坐标
double Now_L1,Now_L2; //目前左、右线长（从物体到滑轮cm）
double Set_L1,Set_L2; //设定左、右线长
double D_L1,D_L2; //左、右线需要改变的长度
//沿黑线移动控制变量
unsigned int Angl_Now=90; //物体以Angl_Now度角移动
unsigned int D_Ang=30; //每次都以+或-30度改变方向
double R_Run_Back=0.3; //步进距离为0.3
double L=10;
int Out=0; //物体偏离黑线标志变量(也是移动方向校正次数)
int ActOver = 0; //要求的移动是否完成标志
void F_Delay()
{
int i;
for(i=0;i<0xff;i++)
{
*P_Watchdog_Clear=1;
}
}
//===================================================
//函数:void Revise(double x,double y)
//语法:void Revise(double x,double y)
//描述:校正物体移动后，物体的坐标、线长
//参数:物体移动后，目前的坐标(x,y)
//返回:无
//===================================================
void Revise(double x,double y)
{
X_Now = x;
Y_Now = y;
L12(x,y,&Now_L1,&Now_L2);
X_Set=x;
Y_Set=y;
}
//=======================================================
//函数:void ChgLine()
//语法:void ChgLine()
//描述:根据设定的线长和当前线长，改变线长，以达到设定长度
// 主要通过控制电机正、反旋转实现收、放线
//参数:无
//返回:无
//=======================================================
void ChgLine()
{
double dl1,dl2;
unsigned int angle;
dl1=Now_L1-Set_L1;
dl2=Now_L2-Set_L2;
//根据两线长匹配两电机转速
if(fabs(dl1)>fabs(dl2))
{
DeCnt1=Min_DC;
DeCnt2=fabs(dl1/dl2 * DeCnt1);
}
else
{
DeCnt2=Min_DC;
DeCnt1=fabs(dl2/dl1 * DeCnt2);
}
//控制电机1收放线
if(dl1>0) //1收线
{
angle=dl1/C1 * 360.0; //计算旋转角度
AngleRunA(angle,1); //调用电机角度旋转函数
}
else if(dl1<0) //1放线
{
angle=-dl1/C1 * 360.0;
AngleRunA(angle,0);
}
else M1_Over=1; //dl1==0时，电机1停机
//控制电机2收放线
if(dl2>0) //2收线
{
angle=dl2/C2 * 360.0;
AngleRunB(angle,1);
}
else if(dl2<0) //2放线
{
angle=-dl2/C2 * 360.0;
AngleRunB(angle,0);
}
else M2_Over=1;
}
//=======================================================
//函数:void PointGo(double x,double y)
//语法:void PointGo(double x,double y)
//描述:定点运动，移动到(x,y)点
//参数:目标点(x,y)
//返回:无
//=======================================================
void PointGo(double x,double y)
{
X_Set=x;
Y_Set=y;
*P_Watchdog_Clear = 1;
L12(x,y,&Set_L1,&Set_L2); //计算目标点到两滑轮的距离
ChgLine();
}
//=======================================================
//函数:void CirculGo(double x,double y,double R)
//语法:void CirculGo(double x,double y,double R)
//描述:以(x,y)为圆心，作半径为R的圆周运动，从(x+R,y)点开始移动
// 如物体不在(x+R,y)点上，则会先移动到该点，再开始作圆周运动
// [注:由于计算的控制坐标和实际物体运动坐标存在偏差，
// 所以在函数中加了偏差校正，简单说:
//
// 设定轨迹 实际轨迹
// 校正前: 圆 椭圆
// 校正后: 椭圆 圆
// ]
//参数:圆心(x,y)和半径R
//返回:无
//=======================================================
void CirculGo(double x,double y,double R) //B_3
{
int i=0,j;
double nextX,nextY,radian,R_revise,L_revise=5.0,cosQ,sinQ;
Min_DC=1000;
PointGo(x+R,y); //先移动到(x+R,y)点
//把圆等分成360几点，连续走完各点
while(i<=360)
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1) //两电机都停机，即到了某点
{ //才走下一点
i+=1; //角度加一
//偏差校正
if(i<=180)
{
R_revise=L_revise * i / 180.0;
}
else
{
R_revise=L_revise*(360.0-i)/180.0;
}
//计算下一点坐标
Revise(X_Set,Y_Set);
radian=i/180.0 * 3.14159;
sinQ=sin(radian);
cosQ=cos(radian);
nextY=(R+R_revise) * sinQ;
nextX=(R+R_revise) * cosQ;
//调用坐标显示函数(实现实时显示)
DisplayCorr((unsigned int)(R*cosQ+x+0.3),(unsigned int)(R*sinQ+y+0.7));
PointGo(nextX+x,nextY+y); //向目标点移动
}
}
ActOver=1; //圆周运动完成
}
//=======================================================
//函数:void Rodam()
//语法:void Rodam()
//描述:作事先设定好的任意轨迹运动
//参数:无
//返回:无
//=======================================================
void Rodam() //B_2
{
int i=0;
//事先设定好的坐标点:
//1(40.0,30.0) 2(00.0,00.0) 3(40.0,70.0) 4(20.0,70.0)
double Xarr[4]={40.0,0.0,40.0,20.0};
double Yarr[4]={30.0,0.0,70.0,70.0};
Min_DC=500;
while(i<4)
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1) //到了某点,才走下一点,分别走完4点
{
Revise(X_Set,Y_Set); //到了某点,要校正新坐标
//调用坐标显示函数
DisplayCorr((unsigned int)X_Now,(unsigned int)Y_Now);
PointGo(Xarr[i],Yarr[i]);
i++;
}
}
while(M1_Over==0|M2_Over==0)
{
*P_Watchdog_Clear=1;
}
ActOver=1;
}
//=======================================================
//函数:void XoYoStar(double x,double y)
//语法:void XoYoStar(double x,double y)
//描述:从原点(0,0)移动到目标点(x,y)
// 如物体不在原点上，则会先移动到原点
//参数:目标点(x,y)
//返回:无
//=======================================================
void XoYoStar(double x,double y) //B_4
{
int i=0;
Min_DC=500;
PointGo(0.0,0.0);
while(i<1)
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
PointGo(x,y);
i++;
}
}
while(M1_Over==0|M2_Over==0)
{
*P_Watchdog_Clear=1;
}
ActOver=1;
}
//=======================================================
//函数:void AngR(unsigned int angle,double R)
//语法:void AngR(unsigned int angle,double R)
//描述:以angle度为方向，从当前位置移动R厘米
//参数:移动方向angle，移动距离R
//返回:无
//=======================================================
void AngR(unsigned int angle,double R)
{
double dx,dy,radian;
radian=angle/180.0 * 3.14159;
dx=R*cos(radian);
dy=R*sin(radian);
PointGo(X_Now+dx,Y_Now+dy);
Angl_Now=angle;
}
//=======================================================
//函数:void L_IO_init()
//语法:void L_IO_init()
//描述:光电传感器使用端口的初始化(输入)
// L1_IO----IOB0 左光感
// L2_IO----IOB1 上光感
// L3_IO----IOB8 右光感
// L4_IO----IOB10 下光感
// L5_IO----IOB2 中光感
//参数:无
//返回:无
//=======================================================
void L_IO_init()
{
unsigned temp;
temp=L1_IO | L2_IO | L3_IO | L4_IO | L5_IO;
*P_IOB_Dir &= ~temp;
*P_IOB_Attrib &= ~temp;
*P_IOB_Data &= ~temp;
}
//=======================================================
//函数:int TestL(int Li)
//语法:int TestL(int Li)
//描述:测试光电传感器Li(上、下、左、右或中)的信号，高有效
//参数:光电传感器编号Li(1:左 2:上 3:右 4:下 5:中)
//返回:光电传感器信号(1:有效 2:无效)
//=======================================================
int TestL(int Li)
{
switch(Li)
{
case 1: //左
if(*P_IOB_Data&L1_IO)
return 1;
else
return 0;
case 2: //上
if(*P_IOB_Data&L2_IO)
return 1;
else
return 0;
case 3: //右
if(*P_IOB_Data&L3_IO)
return 1;
else
return 0;
case 4: //下
if(*P_IOB_Data&L4_IO)
return 1;
else
return 0;
case 5: //中
if(*P_IOB_Data&L5_IO)
return 1;
else
return 0;
default:
return 0;
}
}
//==================================================================
//函数:void ForExInt()
//语法:void ForExInt()
//描述:对移出黑线物体的处理，使物体沿黑线移动
// 当物体移出黑线时(既中间光感移出黑线)，激发外部中断
// 由中断程序间接调用ForExInt()，只要out!=0就会调用该函数
// [注:原理
// 当前移动方向 上 下 左 右
// 感应到黑线的光感
// 1左 向左调整 向左调整 \ \
// 2上 \ \ 向上调整 向上调整
// 3右 向右调整 向右调整 \ \
// 4下 \ \ 向下调整 向下调整
// ]
//参数:无
//返回:无
//==================================================================
void ForExInt()
{
int AtL,AtR;
if(Angl_Now>=45&&Angl_Now<135) //向上运动时的处理
{
do //如果左右光感信号无效(没感应到黑线)，则以原方向继续移动
{ //直到光感信号有效，从而得知偏离方向，为校正方向提供依据
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
//偏离方向未确定，以原方向继续移动
//角度为Angl_Now，距离R_Run_Back
AngR(Angl_Now,R_Run_Back);
}
AtL=TestL(1); //检测左光感信号
AtR=TestL(3); //检测右光感信号
if(AtL==1) break; //左光感感应到黑线(得知右偏)
if(AtR==1) break; //右光感感应到黑线(得知左偏)
}while(1);
}
else if(Angl_Now>=135&&Angl_Now<225) //向左运动时的处理(同上)
{
do
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(Angl_Now,R_Run_Back);
}
AtL=TestL(4); //检测下光感信号
AtR=TestL(2); //检测上光感信号
if(AtL==1) break;
if(AtR==1) break;
}while(1);
}
else if(Angl_Now>=225&&Angl_Now<315) //向下运动
{
do
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(Angl_Now,R_Run_Back);
}
AtL=TestL(3); //检测右光感信号
AtR=TestL(1); //检测左光感信号
if(AtL==1) break;
if(AtR==1) break;
}while(1);
}
else //向右运动
{
do
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(Angl_Now,R_Run_Back);
}
AtL=TestL(2); //检测上光感信号
AtR=TestL(4); //检测下光感信号
if(AtL==1) break;
if(AtR==1) break;
}while(1);
}
//移动角度校正
if(AtL==1) //增加30度角
Angl_Now=(Angl_Now+D_Ang)%360;
else //减少30度角
Angl_Now=(360+Angl_Now-D_Ang)%360;
// if(*P_IOB_Data&0x0004)
Out--; //校正控制次数减一
}
//=======================================================
//函数:void BlkLinR()
//语法:void BlkLinR()
//描述:沿黑线移动入口函数，主要实现物体的移动(调用AngR()函数)
// 当有外部中断时(既物体偏离黑线)，负责调用ForExInt()校正
//参数:无
//返回:无
//=======================================================
void BlkLinR()
{
unsigned int temp;
Min_DC=1000;
Init_IRQ3_EXT1();
L_IO_init();
while(1)
{
*P_Watchdog_Clear=1;
if(Out!=0) ForExInt(); //校正
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(Angl_Now,R_Run_Back);
}
// temp=KeyProess();
// if(temp!=0) Ask();
// {
// AngR(Angl_Now,0.0);
// ActOver=1;
// }
}
}
/*
void LineGo(double x,double y)
{
int i=0;
double nextx=X_Now,nexty,k,dx=0.7,DX,DY,temp=L-0.5;
if(x!=X_Now)
k=(Y_Now-y)/(X_Now-x);
if(x<X_Now) dx=-dx;
Min_DC=1500;
while(L>2.0)
{
F_Delay();
if(M1_Over==1&&M2_Over==1)
{
nextx+=dx;
nexty=k*(nextx-x)+y;
DX=nextx-x;
DY=nexty-y;
DX*=DX;
DY*=DY;
L=sqrt(DX+DY);
Revise(X_Set,Y_Set);
PointGo(nextx,nexty);
i++;
}
}
ActOver=1;
}
void LineGo_R(double x,double y)
{
int i=0,n,angl;
double DX,DY,L,DR=0.5;
DX=x-X_Now;
DY=y-Y_Now;
angl=atan2(DX,DY) / 3.141592 * 180;
DX*=DX;
DY*=DY;
L=sqrt(DX+DY);
n=(int)L/DR;
X_Set=X_Now;
Y_Set=Y_Now;
while(1)
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
Revise(X_Set,Y_Set);
AngR(angl,DR);
i++;
}
if(i>n) break;
}
ActOver=1;
}
void BacKLine()
{
int Ang=90,R=0.3;
L_IO_init();
do
{
*P_Watchdog_Clear=1;
if(M1_Over==1&&M2_Over==1)
{
if(TestL(5)==0)
{
if(TestL(1)==1&&TestL(2)==0&&TestL(3)==0&&TestL(4)==0)
Ang=180;
if(TestL(1)==0&&TestL(2)==1&&TestL(3)==0&&TestL(4)==0)
Ang=90;
if(TestL(1)==0&&TestL(2)==0&&TestL(3)==1&&TestL(4)==0)
Ang=0;
if(TestL(1)==0&&TestL(2)==0&&TestL(3)==0&&TestL(4)==1)
Ang=270;
}
Revise(X_Set,Y_Set);
AngR(Ang,R);
}
}while(1);
}
*/
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -