📄 isr.c
字号:
#include "spce061v004.h"
#include "lcd_disp.h"
#include "math.h"
#include "s480.h"
#include "voice.h"
#define Step 0.05 //表示电机转动最小单位
#define Division 1/Step //表示把1cm分成的格数
#define LineStep (19.44/6400) //表示电机动一格绳子实际变动数
#define Limit 80
void IRQ4(void) __attribute__ ((ISR));
double abs(double in);
double change(unsigned int input,unsigned int rec);
extern const double table[Limit*2];
extern const double vectortab[16];
extern double vertices[Limit*2]; //任务队列 偶数为x坐标 奇数为y坐标 容纳50个点
extern unsigned int pointer; //指向当前任务
extern unsigned int num; //任务队列中还有多少个点
extern unsigned int arrive; //当前坐标是否到达指定点
extern unsigned int left; //左电机剩余驱动步数
extern unsigned int leftdir; //左电机方向
extern unsigned int right; //右电机剩余驱动步数
extern unsigned int rightdir; //左电机方向
extern unsigned long a,b; //a为左边绳子步长 b为右边绳子步长
extern double x,y; //当前坐标
extern double prex,prey; //一次直线运动前x,y的位置
extern unsigned int status; //当前系统状态
//1 直线设置模式 2画圆模式 3自动跟踪模式 4手动调整模式 5运动模式
extern unsigned int temp1,temp2,temp3,temp4,temp5; //输入值的各位暂存值
extern unsigned int flashnum; //闪烁值
extern unsigned int dispcon[32];
extern long time;
extern double vector[2];
extern double vectorlast[2];
//x=(a*a-b*b-11875)/250
//(15+x)^2+(115-y)^2=a^2
//(95-x)^2+(115-y)^2=b^2
extern unsigned int rec[8]; //探测结果变量 (是否接收到)
extern unsigned int reccounter;
//=========================================================================================
//
//=========================================================================================
void IRQ4(void)
{
//IRQ4_1KHz做为处理中断、喂狗
unsigned int recup,recleft,recdown,recright; //探测结果变量 (是否接收到)
double xtemp,ytemp; //临时x,y坐标
double atemp,btemp; //临时左边绳步长和临时右边绳步长
double vx,vy; //运动矢量的x和y分量
unsigned int alast,blast; //a,b的临时存储
unsigned int i,j;
unsigned int f,l; //分别用于表示所使用点起始的编号和结束编号 从小到大
if(*P_INT_Ctrl&0x0010)
{
*P_INT_Clear=0x0010;
}
else if(*P_INT_Ctrl&0x0020)
{
*P_INT_Clear=0x0020;
}
else if(*P_INT_Ctrl&0x0040)
{
*P_Watchdog_Clear=0x0001; //喂狗
time++;
switch (status)
{
case 5:
//画线功能
if(left||right)//判断当前驱动队列中是否还有任务
{
if(left)//驱动左电机 IOA0
{
left--;
if(*P_IOA_Buffer&0x0001)
*P_IOA_Data=*P_IOA_Buffer&0xfffe;
else
*P_IOA_Data=*P_IOA_Buffer|0x0001;
}
if(right)//驱动右电机 IOA12
{
right--;
if(*P_IOA_Buffer&0x1000)
*P_IOA_Data=*P_IOA_Buffer&0xefff;
else
*P_IOA_Data=*P_IOA_Buffer|0x1000;
}
}
//有 则驱动 减去相应的队列
else
{
if(arrive)
//无 则判断当前坐标是否已经达到目的坐标
{
if(num)
{
num--;
if(pointer++==(Limit-1))
pointer=0; //共400个点
arrive=0;
prex=x;
prey=y;
xtemp=x;
ytemp=y;
dispcon[0]=0x0061;
dispcon[1]=0x0078;
dispcon[2]=0x0069;
dispcon[3]=0x0073;
dispcon[4]=0x0020;
dispcon[5]=0x003a;
dispcon[6]=0x0020;
dispcon[7]=0x0020;
dispcon[8]=0x0058;
dispcon[9]=0x0020;
dispcon[10]=0x0020;
dispcon[11]=0x0020;
dispcon[12]=0x0020;
dispcon[13]=0x0059;
dispcon[14]=0x0020;
dispcon[15]=0x0020;
dispcon[16]=0x0076;
dispcon[17]=0x0061;
dispcon[18]=0x006c;
dispcon[19]=0x0075;
dispcon[20]=0x0065;
dispcon[21]=0x003a;
dispcon[22]=0x0020;
dispcon[23]=0x0020;
dispcon[26]=0x0020;
dispcon[27]=0x0020;
dispcon[31]=0x0020;
}
//是 则从目标点队列中取出下一个点(且arrive置0)、无点则结束
else
{
status=1;
pointer=0;
temp1=0;
temp2=0;
temp3=0;
temp4=0;
temp5=0;
dispcon[24]=temp1+0x0030;
dispcon[25]=temp2+0x0030;
dispcon[28]=temp3+0x0030;
dispcon[29]=temp4+0x0030;
dispcon[30]=temp5+0x0030;
flashnum=24;
voice_complete();
lcd_disp(dispcon,flashnum);
asm("IRQ OFF");
//否 则转到键盘设定模式
}
}
else
{
//暂定把1cm分成20份
if(abs(x-vertices[pointer*2])>abs(y-vertices[pointer*2+1]))
{
//斜率小于1
if(x>vertices[pointer*2])
xtemp=floor(x*20-0.5)/20;
else
xtemp=floor(x*20+1.5)/20;
ytemp=prey+(xtemp-prex)/(vertices[pointer*2]-prex)*(vertices[pointer*2+1]-prey);
}
else
{
if(y>vertices[pointer*2+1])
ytemp=floor(y*20-0.5)/20;
else
ytemp=floor(y*20+1.5)/20;
xtemp=prex+(ytemp-prey)/(vertices[pointer*2+1]-prey)*(vertices[pointer*2]-prex);
//斜率大于等于1
}
//没有达到目标 则算出当前位置到下一个点间两步进电机各应该走的步数
//(15+x)^2+(115-y)^2=a^2
//(95-x)^2+(115-y)^2=b^2
atemp=sqrt((15+xtemp)*(15+xtemp)+(115-ytemp)*(115-ytemp));
atemp=atemp/LineStep; //算出新的位置对应左绳长度
btemp=sqrt((95-xtemp)*(95-xtemp)+(115-ytemp)*(115-ytemp));
btemp=btemp/LineStep; //算出新的位置对应右绳长度
alast=floor(atemp+0.5);
blast=floor(btemp+0.5);
if(alast>a) //左电机 IOA1 下拉
{
left=(alast-a)*2;
*P_IOA_Data=*P_IOA_Buffer|0x0002;
}
else if(alast<a) //上拉
{
left=(a-alast)*2;
*P_IOA_Data=*P_IOA_Buffer&0xfffd;
}
if(blast>b) //右电机 IOA13 下拉
{
right=(blast-b)*2;
*P_IOA_Data=*P_IOA_Buffer&0xdfff;
}
else if(blast<b) //上拉
{
right=(b-blast)*2;
*P_IOA_Data=*P_IOA_Buffer|0x2000;
}
//步数取整 若取整后步数为都为0则认为已经达到目标点(arrive置1)
//x=(a*a-b*b+8800)/250
x=(alast*LineStep*alast*LineStep-blast*LineStep*blast*LineStep+8800)/220;
y=115-sqrt(alast*LineStep*alast*LineStep-(15+x)*(15+x));
dispcon[24]=((int)(x+0.5))/10+0x0030;
dispcon[25]=((int)(x+0.5))%10+0x0030;
dispcon[28]=((int)(y+0.5))/100+0x0030;
dispcon[29]=((int)(y-(((int)(y+0.5))/100)*100+0.5))/10+0x0030;
dispcon[30]=((int)(y+0.5))%10+0x0030;
//根据取整后步数算出新的坐标点
a=alast;
b=blast;
if((abs(x-vertices[pointer*2]))<0.05&&(abs(y-vertices[pointer*2+1])<0.05))
arrive=1;
lcd_disp(dispcon,32);
}
}
break;
case 6:
//根据驱动任务驱动电机
if(left||right)//判断当前驱动队列中是否还有任务
{
if(left)//驱动左电机 IOA0
{
left--;
if(*P_IOA_Buffer&0x0001)
*P_IOA_Data=*P_IOA_Buffer&0xfffe;
else
*P_IOA_Data=*P_IOA_Buffer|0x0001;
}
if(right)//驱动右电机 IOA12
{
right--;
if(*P_IOA_Buffer&0x1000)
*P_IOA_Data=*P_IOA_Buffer&0xefff;
else
*P_IOA_Data=*P_IOA_Buffer|0x1000;
}
}
//有 则驱动 减去相应的队列
else
{
//无驱动任务则 探测当前状态
reccounter=0;
for(j=0;j<8;j++)
{
*P_IOB_Data=(*P_IOB_Buffer&0xfff0)|j;
for(i=0;i<100;i++)
;
if(*P_IOA_Data&0x8000)
{
rec[j]=0;
reccounter++;
}
else
rec[j]=1;
}
vx=vectorlast[0];
vy=vectorlast[1];
//根据上一次运动矢量 判断应该读取哪些点的信息
if(abs(vy)>abs(vx))
{
//上次运动方向的Y值比X值大
if(vx==0&&vy>0)
{
f=4;
l=8;
//使用4 5 6 7 0
}
else if(vx==0&&vy<0)
{
f=0;
l=4;
//使用0 1 2 3 4
}
else if(vy>0&&vx>0)
{
f=5;
l=8;
//使用5 6 7 0
}
else if(vy>0&&vx<0)
{
f=4;
l=7;
//使用4 5 6 7
}
else if(vy<0&&vx>0)
{
f=0;
l=3;
//使用0 1 2 3
}
else if(vy>0&&vx<0)
{
f=1;
l=4;
//使用1 2 3 4
}
}
else
{
//上次运动方向的Y值小于等于X值
if(vx==vy&&vx>0)
{
f=5;
l=9;
//使用5 6 7 0 1
}
else if(vx==vy&&vx<0)
{
f=1;
l=5;
//使用1 2 3 4 5
}
else if(vx==-vy&&vx>0)
{
f=7;
l=11;
//使用7 0 1 2 3
}
else if(vx==-vy&&vx<0)
{
f=3;
l=7;
//使用3 4 5 6 7
}
else if(vy==0&&vx>0)
{
f=6;
l=10;
//使用6 7 0 1 2
}
else if(vy==0&&vx<0)
{
f=2;
l=6;
//使用2 3 4 5 6
}
else if(vx>0&&vy>0)
{
f=6;
l=9;
//使用6 7 0 1
}
else if(vx>0&&vy<0)
{
f=7;
l=10;
//使用7 0 1 2
}
else if(vx<0&&vy<0)
{
f=2;
l=5;
//使用2 3 4 5
}
else if(vx<0&&vy>0)
{
f=3;
l=6;
//使用3 4 5 6
}
}
*P_Watchdog_Clear=0x0001;
//根据使用编号确定新的运动矢量
vector[0]=0;
vector[1]=0;
for(i=f;i<=l;i++)
{
if(rec[i%8]==0)
{
vector[0]+=vectortab[(i%8)*2];
vector[1]+=vectortab[(i%8)*2+1];
}//如果在黑线内，则使用其运动方向
}
vectorlast[0]=vector[0];
vectorlast[1]=vector[1];
if(vector[0]==0&&vector[1]==0)
{
status=1;
pointer=0;
temp1=0;
temp2=0;
temp3=0;
temp4=0;
temp5=0;
dispcon[24]=temp1+0x0030;
dispcon[25]=temp2+0x0030;
dispcon[28]=temp3+0x0030;
dispcon[29]=temp4+0x0030;
dispcon[30]=temp5+0x0030;
flashnum=24;
voice_complete();
lcd_disp(dispcon,flashnum);
asm("IRQ OFF");
*P_INT_Ctrl=0x0040;
return;
}
//预留结束条件
//跟据运动矢量填写驱动任务
//暂定把1cm分成20份
if(abs(vector[0])>abs(vector[1]))
{
//斜率小于1
if(vector[0]<0)
xtemp=floor(x*4-0.5)/4;
else
xtemp=floor(x*4+1.5)/4;
ytemp=y+(xtemp-x)*vector[1]/vector[0];
}
else
{
//斜率大于等于1
if(vector[1]<0)
ytemp=floor(y*4-0.5)/4;
else
ytemp=floor(y*4+1.5)/4;
xtemp=x+(ytemp-y)*vector[0]/vector[1];
}
//(15+x)^2+(115-y)^2=a^2
//(95-x)^2+(115-y)^2=b^2
atemp=sqrt((15+xtemp)*(15+xtemp)+(115-ytemp)*(115-ytemp));
atemp=atemp/LineStep; //算出新的位置对应左绳长度
btemp=sqrt((95-xtemp)*(95-xtemp)+(115-ytemp)*(115-ytemp));
btemp=btemp/LineStep; //算出新的位置对应右绳长度
alast=floor(atemp+0.5);
blast=floor(btemp+0.5);
if(alast>a) //左电机 IOA1 下拉
{
left=(alast-a)*2;
*P_IOA_Data=*P_IOA_Buffer|0x0002;
}
else if(alast<a) //上拉
{
left=(a-alast)*2;
*P_IOA_Data=*P_IOA_Buffer&0xfffd;
}
if(blast>b) //右电机 IOA13 下拉
{
right=(blast-b)*2;
*P_IOA_Data=*P_IOA_Buffer&0xdfff;
}
else if(blast<b) //上拉
{
right=(b-blast)*2;
*P_IOA_Data=*P_IOA_Buffer|0x2000;
}
//步数取整 若取整后步数为都为0则认为已经达到目标点(arrive置1)
//x=(a*a-b*b+8800)/250
x=(alast*LineStep*alast*LineStep-blast*LineStep*blast*LineStep+8800)/220;
y=115-sqrt(alast*LineStep*alast*LineStep-(15+x)*(15+x));
dispcon[18]=88;
dispcon[19]=58;
dispcon[20]=((int)(x+0.5))/10+0x0030;
dispcon[21]=((int)(x+0.5))%10+0x0030;
dispcon[24]=89;
dispcon[25]=58;
dispcon[26]=((int)(y+0.5))/100+0x0030;
dispcon[27]=((int)(y-(((int)(y+0.5))/100)*100+0.5))/10+0x0030;
dispcon[28]=((int)(y+0.5))%10+0x0030;
//根据取整后步数算出新的坐标点
a=alast;
b=blast;
lcd_disp(dispcon,32);
}
break;
}
*P_INT_Clear=0x0040;
}
}
//=========================================================================================
//
//=========================================================================================
double abs(double in)
{
return in>0?in:-in;
}
//=========================================================================================
//
//=========================================================================================
double change(unsigned int input,unsigned int rec)
{
if(rec)
return input;
else
return 0;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -