?? 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[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 + -