📄
字号:
check_busy();
temp=rd_data();//读实际位置
relpsr|=temp<<16;
DELAY();
check_busy();
temp=rd_data();//读实际位置
relpsr|=temp<<8;
check_busy();
DELAY();
temp=rd_data();//读实际位置(最低位)
relpsr|=temp;//最终relpsr存放的是30位右轮位置值
//处理
movps=((unsigned long)p1<<24)+((unsigned long)p2<<16)+((unsigned long)p3<<8)+(unsigned long)p4;
loadr=relpsr-movps;
loadp5=(loadr&0xff000000)>>24;
loadp6=(loadr&0x00ff0000)>>16;
loadp7=(loadr&0x0000ff00)>>8;
loadp8=loadr&0x000000ff;
//给右轮装入新的轨迹参数
check_busy();
wr_cmd(RSTI);
DELAY();
filter_sel();
DELAY();
traj_sel(0x00,0x0a,v1,v2,v3,loadp5,loadp6,loadp7,loadp8);
DELAY();
CtrlOUT|=CSR;//CS=1
CtrlOUT&=~CSL;
CtrlOUT&=~CSR;
check_busy();
wr_cmd(STT);
DELAY();
do
{
state_back= rd_st();
}
while((state_back&0x04)==0);//等待直至运动完成
CtrlOUT|=CSL;
CtrlOUT|=CSR;
sign_runback++;//后退运动完成标志位
}
void stop()
{
CtrlOUT&=~CSR;
CtrlOUT&=~CSL;
check_busy();
wr_cmd(LTRJ);
DELAY();
traj_sel(0x02,0x0a,0x00,0x00,0x00,0x00,0x00,0x00,0x00);
check_busy();
wr_cmd(STT);
DELAY();
CtrlOUT|=CSR;
CtrlOUT|=CSL;
}
void turn_left(unsigned char pl1,unsigned char pl2,unsigned char pl3,unsigned char pl4,unsigned char pr1,unsigned pr2,unsigned char pr3,unsigned char pr4)
{
long temp,relpsl,relpsr,movps,loadl,loadr;
unsigned char loadp1,loadp2,loadp3,loadp4,loadp5,loadp6,loadp7,loadp8,state_left;
//读左轮实际位置
CtrlOUT&=~CSL;
check_busy();
wr_cmd(RDRP);
DELAY();
check_busy();
temp=rd_data();//读左轮实际位置(最高位)
relpsl=temp<<24;
DELAY();
check_busy();
temp=rd_data();//读实际位置
relpsl|=temp<<16;
DELAY();
check_busy();
temp=rd_data();//读实际位置
relpsl|=temp<<8;
check_busy();
DELAY();
temp=rd_data();//读实际位置(最低位)
relpsl|=temp;//最终relps1存放的是30位左轮实际位置值
//处理
movps=((unsigned long)pl1<<24)+((unsigned long)pl2<<16)+((unsigned long)pl3<<8)+(unsigned long)pl4;//pl1,pl2必须转换成无符号长整形，不然左移结果将不确定
loadl=relpsl-movps;
loadp1=(loadl&0xff000000)>>24;
loadp2=(loadl&0x00ff0000)>>16;
loadp3=(loadl&0x0000ff00)>>8;
loadp4=loadl&0x000000ff;
//给左轮装入新的轨迹参数
check_busy();
wr_cmd(RSTI);
DELAY();
filter_sel();
DELAY();
traj_sel(0x00,0x0a,0x00,0xc5,0xff,loadp1,loadp2,loadp3,loadp4);
DELAY();
CtrlOUT|=CSL;//CS=1
//读右轮实际位置
CtrlOUT&=~CSR;
check_busy();
wr_cmd(RDRP);
DELAY();
check_busy();
temp=rd_data();//读右轮实际位置(最高位)
relpsr=temp<<24;
DELAY();
check_busy();
temp=rd_data();//读实际位置
relpsr|=temp<<16;
DELAY();
check_busy();
temp=rd_data();//读实际位置
relpsr|=temp<<8;
check_busy();
DELAY();
temp=rd_data();//读实际位置(最低位)
relpsr|=temp;//最终relpsr存放的是30位右轮位置值
//处理
movps=((unsigned long)pr1<<24)+((unsigned long)pr2<<16)+((unsigned long)pr3<<8)+(unsigned long)pr4;
loadr=relpsr+movps;
loadp5=(loadr&0xff000000)>>24;
loadp6=(loadr&0x00ff0000)>>16;
loadp7=(loadr&0x0000ff00)>>8;
loadp8=loadr&0x000000ff;
check_busy();
wr_cmd(RSTI);
DELAY();
filter_sel();
DELAY();
traj_sel(0x00,0x0a,0x00,0xc5,0xff,loadp5,loadp6,loadp7,loadp8);
DELAY();
CtrlOUT|=CSR;
CtrlOUT&=~CSL;
CtrlOUT&=~CSR;
check_busy();
wr_cmd(STT);
DELAY();
do
{
state_left= rd_st();
}
while((state_left&0x04)==0);
CtrlOUT|=CSL;
CtrlOUT|=CSR;
sign_turnleft++;
}
//左右两轮以相同的速度反相运动，左轮在原基础上前进一段距离，右轮在原基础上后退一段距离
void turn_right(unsigned char pl1,unsigned char pl2,unsigned char pl3,unsigned char pl4,unsigned char pr1,unsigned pr2,unsigned char pr3,unsigned char pr4)
{
long temp,relpsl,relpsr,movps,loadl,loadr;
unsigned char loadp1,loadp2,loadp3,loadp4,loadp5,loadp6,loadp7,loadp8,state_right;
//读左轮实际位置
CtrlOUT&=~CSL;
check_busy();
wr_cmd(RDRP);
DELAY();
check_busy();
temp=rd_data();//读左轮实际位置(最高位)
relpsl=temp<<24;
DELAY();
check_busy();
temp=rd_data();//读实际位置
relpsl|=temp<<16;
DELAY();
check_busy();
temp=rd_data();//读实际位置
relpsl|=temp<<8;
check_busy();
DELAY();
temp=rd_data();//读实际位置(最低位)
relpsl|=temp;//最终relps1存放的是30位左轮实际位置值
//处理
movps=((unsigned long)pl1<<24)+((unsigned long)pl2<<16)+((unsigned long)pl3<<8)+(unsigned long)pl4;//pl1,pl2必须转换成无符号长整形，不然左移结果将不确定
loadl=relpsl+movps;
loadp1=(loadl&0xff000000)>>24;
loadp2=(loadl&0x00ff0000)>>16;
loadp3=(loadl&0x0000ff00)>>8;
loadp4=loadl&0x000000ff;
//给左轮装入新的轨迹参数
check_busy();
wr_cmd(RSTI);
DELAY();
filter_sel();
DELAY();
traj_sel(0x00,0x0a,0x01,0x00,0x00,loadp1,loadp2,loadp3,loadp4);
DELAY();
CtrlOUT|=CSL;//CS=1
//读右轮实际位置
CtrlOUT&=~CSR;
check_busy();
wr_cmd(RDRP);
DELAY();
check_busy();
temp=rd_data();//读右轮实际位置(最高位)
relpsr=temp<<24;
DELAY();
check_busy();
temp=rd_data();//读实际位置
relpsr|=temp<<16;
DELAY();
check_busy();
temp=rd_data();//读实际位置
relpsr|=temp<<8;
check_busy();
DELAY();
temp=rd_data();//读实际位置(最低位)
relpsr|=temp;//最终relpsr存放的是30位右轮位置值
//处理
movps=((unsigned long)pr1<<24)+((unsigned long)pr2<<16)+((unsigned long)pr3<<8)+(unsigned long)pr4;
loadr=relpsr-movps;
loadp5=(loadr&0xff000000)>>24;
loadp6=(loadr&0x00ff0000)>>16;
loadp7=(loadr&0x0000ff00)>>8;
loadp8=loadr&0x000000ff;
check_busy();
wr_cmd(RSTI);
DELAY();
filter_sel();
DELAY();
traj_sel(0x00,0x0a,0x01,0x00,0x00,loadp5,loadp6,loadp7,loadp8);
DELAY();
CtrlOUT|=CSR;
CtrlOUT&=~CSL;
CtrlOUT&=~CSR;
check_busy();
wr_cmd(STT);
DELAY();
do
{
state_right= rd_st();
}
while((state_right&0x04)==0);
CtrlOUT|=CSL;
CtrlOUT|=CSR;
sign_turnright++;
}
void traj_selInit()
{
check_busy();
wr_cmd(LTRJ);
DELAY();
check_busy();
wr_data(0x00);
DELAY();
check_busy();
wr_data(0x2a);
DELAY();
//acc
check_busy();
wr_data(0x00);
DELAY();
check_busy();
wr_data(0x00);
DELAY();
check_busy();
wr_data(0x00);
DELAY();
check_busy();
wr_data(0x2f);
DELAY();
//v
check_busy();
wr_data(0x00);
DELAY();
check_busy();
wr_data(0x00);
DELAY();
check_busy();
wr_data(0x80);
DELAY();
check_busy();
wr_data(0xFF);
DELAY();
//position
check_busy();
wr_data(0x00);
DELAY();
check_busy();
wr_data(0xff);
DELAY();
check_busy();
wr_data(0xff);
DELAY();
check_busy();
wr_data(0xff);
DELAY();
check_busy();
wr_cmd(STT);
DELAY();
}
void GoInit()
{
P5OUT&=~BIT3;//CS=0
wr_cmd(RSTI);
DELAY();
filter_sel();
DELAY();
traj_selInit();
DELAY();
P5OUT|=BIT3;//CS=1
delay4ms();
P5OUT&=~BIT2;//CS=0
wr_cmd(RSTI);
DELAY();
filter_sel();
DELAY();
traj_selInit();
DELAY();
P5OUT|=BIT2;
P5OUT|=BIT7;
}
unsigned int rd_front()
{
unsigned int temp1,temp2,temp3;
P6OUT&=~BIT7;
P6OUT|=BIT6;
DELAY();
temp1=P2IN;
temp2=P3IN;
temp1&=0xe0;
temp3=(temp2<<8)+temp1;//前排灯的数据
return temp3;
}
unsigned int counter_table()
{
unsigned int temp,counter=0;
temp=rd_front();
//统计灯亮的总个数：
counter=((temp&0x8000)>>15)+((temp&0x4000)>>14)+((temp&0x2000)>>13)+((temp&0x1000)>>12)+((temp&0x0800)>>11)+((temp&0x0400)>>10)+((temp&0x0200)>>9)+((temp&0x0100)>>8)+((temp&0x0080)>>7)+((temp&0x0040)>>6)+((temp&0x0020)>>5);
if(counter<=6)
sign=1;//用该标志位的目的是避免经过十字交叉点时多计数。
if((sign==1)&(counter>9))
{
counter_tab++;//统计经过的格子数
sign=0;
}
return counter_tab;
}
void correct_left(unsigned char v1,unsigned char v2,unsigned char v3 )
{
CtrlOUT&=~CSL;
check_busy();
wr_cmd(RSTI);
DELAY();
check_busy();
filter_sel();
DELAY();
check_busy();
traj_sel(0x00,0x0a,0x02,0x00,0x00,0x00,0xff,0xff,0xff);
DELAY();
CtrlOUT|=CSL;
CtrlOUT&=~CSR;//CS=0
check_busy();
wr_cmd(RSTI);
DELAY();
check_busy();
filter_sel();
DELAY();
check_busy();
traj_sel(0x00,0x0a,v1,v2,v3,0x00,0xff,0xff,0xff);
DELAY();
CtrlOUT|=CSR;//CS=1
CtrlOUT&=~CSL;
CtrlOUT&=~CSR;
check_busy();
wr_cmd(STT);
DELAY();
CtrlOUT|=CSL;
CtrlOUT|=CSR;
DELAY();
}
void correct_right(unsigned char v4,unsigned char v5,unsigned char v6)
{
CtrlOUT&=~CSL;
check_busy();
wr_cmd(RSTI);
DELAY();
check_busy();
filter_sel();
DELAY();
check_busy();
traj_sel(0x00,0x0a,v4, v5, v6,0x00,0xff,0xff,0xff);
DELAY();
CtrlOUT|=CSL;
CtrlOUT&=~CSR;//CS=0
check_busy();
wr_cmd(RSTI);
DELAY();
check_busy();
filter_sel();
DELAY();
check_busy();
traj_sel(0x00,0x0a,0x02,0x00,0x00,0x00,0xff,0xff,0xff);
DELAY();
CtrlOUT|=CSR;//CS=1
CtrlOUT&=~CSL;
CtrlOUT&=~CSR;
check_busy();
wr_cmd(STT);
DELAY();
CtrlOUT|=CSL;
CtrlOUT|=CSR;
DELAY();
}
void init_TimerA()
{
TACTL=TASSEL0+MC0+TACLR;//时钟源为SMCLK,增计数模式
CCTL0=CCIE;
CCR0=20;//300
_EINT();
}
#pragma vector=TIMERA0_VECTOR
__interrupt void timer_a0_isr()
{
light_val=rd_front(); //前排灯数据
counter_tab=counter_table(); //数格
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -