📄 app.c
字号:
/*
*********************************************************************************************************
* 中断程序区
*********************************************************************************************************
*/
//////////////////////Timer2计数中断_提供舵机时钟////////////////////
//#pragma interrupt_handler Timer2:11
/*
void Timer2(void)
{
TCNT2=0x06; //2次 每次 0.128ms
time_count++;
if(time_count==84) // 1=0.23ms 86=20ms 9=2ms 2右患 10左极
{
PORTC|=0x0f;
time_count=0;
}
if(time_count==hand_cy[0]) PORTC&=~(1<<0); //矩形波变化 手抓1
if(time_count==hand_cy[1]) PORTC&=~(1<<1); //手抓2
if(time_count==hand_cy[2]) PORTC&=~(1<<2); //手抓3
if(time_count==hand_cy[3]) PORTC&=~(1<<3); //手抓4
//if(hand_cy[0]==7) LED_send_16b(0x0107);
//else if(hand_cy[0]==5) LED_send_16b(0x0105);
//else if(hand_cy[0]==4) LED_send_16b(0x0104);
}
*/
/*
*********************************************************************************************************
* 电机驱动
*********************************************************************************************************
*/
//运动左侧电机驱动(有慢起功能,自主运行，启动有两个速度阶段，每个阶段可任意设置)----------------------------------------------
static void motor_move_l(void *p_arg) //m_action 1正转 0反转 2停 m_speed 16bit 0~1024全速
{
INT16U key=0x000b,speed_old=0x2000,move_fx[2]={0x0002,0x0002}; //运动方向 1 正 0 反 2 停
INT16U pwm_speed[2]={0x0000,0x0000},
pwm_speed_real=0x0000,pwm_speed_real_dely=0x0000; //预定pwm波 和实际pwm波 0x0000-0x03ff 值越大速度越大
p_arg = p_arg;
while(1)
{
motor_l_key=OSMboxAccept(motor_l_Box); //查看邮箱
if( motor_l_key!=0) //如果收到邮件
{
key =*motor_l_key; //获得钥匙
pwm_speed[1] =(speed[speed_model][1][0][key]&0x0fff); //提取速度目标值
move_fx[1] =speed[speed_model][1][0][key]>>12; //提取方向值
if(speed[speed_model][1][0][key]!=speed_old)
{
speed_old=speed[speed_model][1][0][key]; //保存方向速度
if(move_fx[1]!=move_fx[0]) pwm_speed_real=0x0000;pwm_speed_real_dely=0x0000;//如果方向改变 速度先降为0
pwm_speed[0]=pwm_speed[1]; //保存新速度值
move_fx[0] =move_fx[1]; //保存新方向值
switch(move_fx[0]) //运动方向切换
{
case 0:PORTA &=~(1<<0);PORTA &=~(1<<1);break; //正传
case 1:PORTA |=(1<<0);PORTA &=~(1<<1);break; //反转
case 2:pwm_speed[0]=0X0000;pwm_speed_real=0X0000;pwm_speed_real_dely=0x0000;break; //停 需求运动速度清零
}
}
}
//接近预定值，全加上---------------
if(((speed[speed_model][1][0][key]&0x0fff)-pwm_speed_real_dely)<(((speed[speed_model][1][0][key]&0x0fff)-(speed[speed_model][0][0][key]&0x0fff))/dly_time[speed_model][1][key]/10))
{pwm_speed_real=(speed[speed_model][1][0][key]&0x0fff);}
//第一速度区间---------------
else if((speed[speed_model][0][0][key]&0x0fff)>pwm_speed_real_dely) //假速度 小于 设定值
{
pwm_speed_real_dely += (speed[speed_model][0][0][key]&0x0fff)/dly_time[speed_model][0][key]/10; //假速度提升
if(heading[speed_model][0][key]==0) pwm_speed_real=(speed[speed_model][0][0][key]&0x0fff); //如果立即加电模式 延时
else if(heading[speed_model][0][key]==1) pwm_speed_real = pwm_speed_real_dely; //逐步加电模式 磨蹭时间
else
{
pwm_speed_real=(speed[speed_model][0][0][key]&0x0fff);
pwm_speed_real_dely =pwm_speed_real ;
} //否则立即加电 并不延时
}
//第二速度区间---------------
else if((speed[speed_model][1][0][key]&0x0fff)>pwm_speed_real_dely) //假速度 小于 设定值
{
pwm_speed_real_dely +=((speed[speed_model][1][0][key]&0x0fff)-(speed[speed_model][0][0][key]&0x0fff))/dly_time[speed_model][1][key]/10; //假速度提升
if(heading[speed_model][1][key]==0) pwm_speed_real=(speed[speed_model][1][0][key]&0x0fff); //如果立即加电模式
else pwm_speed_real = pwm_speed_real_dely;
}
else pwm_speed_real=(speed[speed_model][1][0][key]&0x0fff); //如果速度超出预定值
OCR3A=pwm_speed_real;
OSTimeDly(OS_TICKS_PER_SEC/10); //1/20s
}
}
//运动右侧电机驱动(有慢起功能,自主运行，启动有两个速度阶段，每个阶段可任意设置)----------------------------------------------
static void motor_move_r(void *p_arg) //m_action 1正转 0反转 2停 m_speed 16bit 0~1024全速
{
INT16U key=0x000b,speed_old=0x2000,move_fx[2]={0x0002,0x0002}; //运动方向 1 正 0 反 2 停
INT16U pwm_speed[2]={0x0000,0x0000},
pwm_speed_real=0x0000,pwm_speed_real_dely=0x0000; //预定pwm波 和实际pwm波 0x0000-0x03ff 值越大速度越大
p_arg = p_arg;
while(1)
{
motor_r_key=OSMboxAccept(motor_r_Box); //查看邮箱
if( motor_r_key!=0) //如果收到邮件
{
key =*motor_r_key; //获得钥匙
pwm_speed[1] =(speed[speed_model][1][1][key]&0x0fff); //提取速度目标值
move_fx[1] =speed[speed_model][1][1][key]>>12; //提取方向值
if(speed[speed_model][1][1][key]!=speed_old)
{
speed_old=speed[speed_model][1][1][key]; //保存方向速度
if(move_fx[1]!=move_fx[0]) pwm_speed_real=0x0000;pwm_speed_real_dely=0x0000;//如果方向改变 速度先降为0
pwm_speed[0]=pwm_speed[1]; //保存新速度值
move_fx[0] =move_fx[1]; //保存新方向值
switch(move_fx[0]) //运动方向切换
{
case 1:PORTB &=~(1<<4);PORTB &=~(1<<5);break; //正传
case 0:PORTB &=~(1<<4);PORTB |=(1<<5);break; //反转
case 2:pwm_speed[0]=0X0000;pwm_speed_real=0X0000;pwm_speed_real_dely=0x0000;break; //停 需求运动速度清零
}
}
}
if(((speed[speed_model][1][1][key]&0x0fff)-pwm_speed_real_dely)<(((speed[speed_model][1][1][key]&0x0fff)-(speed[speed_model][0][1][key]&0x0fff))/dly_time[speed_model][1][key]/10))
{pwm_speed_real=(speed[speed_model][1][1][key]&0x0fff);}
//第一速度区间---------------
else if((speed[speed_model][0][1][key]&0x0fff)>pwm_speed_real_dely) //假速度 小于 设定值
{
pwm_speed_real_dely += (speed[speed_model][0][1][key]&0x0fff)/dly_time[speed_model][0][key]/10; //假速度提升
if(heading[speed_model][0][key]==0) pwm_speed_real=(speed[speed_model][0][1][key]&0x0fff); //如果立即加电模式
else if(heading[speed_model][0][key]==1) pwm_speed_real = pwm_speed_real_dely; //逐步加电模式 磨蹭时间
else
{
pwm_speed_real=(speed[speed_model][0][1][key]&0x0fff);
pwm_speed_real_dely = pwm_speed_real;
} //否则 磨蹭时间
}
//第二速度区间---------------
else if((speed[speed_model][1][1][key]&0x0fff)>pwm_speed_real_dely) //假速度 小于 设定值
{
pwm_speed_real_dely +=((speed[speed_model][1][1][key]&0x0fff)-(speed[speed_model][0][1][key]&0x0fff))/dly_time[speed_model][1][key]/10; //假速度提升
if(heading[speed_model][1][key]==0) pwm_speed_real=(speed[speed_model][1][1][key]&0x0fff); //如果立即加电模式
else pwm_speed_real = pwm_speed_real_dely;
}
else pwm_speed_real=(speed[speed_model][1][1][key]&0x0fff); //如果速度超出预定值
OCR1C=pwm_speed_real;
OSTimeDly(OS_TICKS_PER_SEC/10); //1/20s
}
}
//运动中间电机驱动(有慢起功能,自主运行，启动有两个速度阶段，每个阶段可任意设置)----------------------------------------------
static void motor_move_m(void *p_arg) //m_action 1正转 0反转 2停 m_speed 16bit 0~1024全速
{
INT16U key=0x000b,speed_old=0x2000,move_fx[2]={0x0002,0x0002}; //运动方向 1 正 0 反 2 停
INT16U pwm_speed[2]={0x0000,0x0000},
pwm_speed_real=0x0000,pwm_speed_real_dely=0x0000; //预定pwm波 和实际pwm波 0x0000-0x03ff 值越大速度越大
p_arg = p_arg;
while(1)
{
motor_m_key=OSMboxAccept(motor_m_Box); //查看邮箱
if( motor_m_key!=0) //如果收到邮件
{
key =*motor_m_key; //获得钥匙
pwm_speed[1] =(speed[speed_model][1][2][key]&0x0fff); //提取速度目标值
move_fx[1] =speed[speed_model][1][2][key]>>12; //提取方向值
if(speed[speed_model][1][2][key]!=speed_old)
{
speed_old=speed[speed_model][1][2][key]; //保存方向速度
if(move_fx[1]!=move_fx[0]) pwm_speed_real=0x0000;pwm_speed_real_dely=0x0000;//如果方向改变 速度先降为0
pwm_speed[0]=pwm_speed[1]; //保存新速度值
move_fx[0] =move_fx[1]; //保存新方向值
switch(move_fx[0]) //运动方向切换
{
case 0:PORTA |=(1<<2);PORTA &=~(1<<3);break; //正传
case 1:PORTA &=~(1<<2);PORTA &=~(1<<3);break; //反转
case 2:pwm_speed[0]=0X0000;pwm_speed_real=0X0000;pwm_speed_real_dely=0x0000;break; //停 需求运动速度清零
}
}
}
if(((speed[speed_model][1][2][key]&0x0fff)-pwm_speed_real_dely)<(((speed[speed_model][1][2][key]&0x0fff)-(speed[speed_model][0][2][key]&0x0fff))/dly_time[speed_model][1][key]/10))
{pwm_speed_real=(speed[speed_model][1][2][key]&0x0fff);}
//第一速度区间---------------
else if((speed[speed_model][0][2][key]&0x0fff)>pwm_speed_real_dely) //假速度 小于 设定值
{
pwm_speed_real_dely += (speed[speed_model][0][2][key]&0x0fff)/dly_time[speed_model][0][key]/10; //假速度提升
if(heading[speed_model][0][key]==0) pwm_speed_real=(speed[speed_model][0][2][key]&0x0fff); //如果立即加电模式
else if(heading[speed_model][0][key]==1) pwm_speed_real = pwm_speed_real_dely; //逐步加电模式 磨蹭时间
else
{
pwm_speed_real=(speed[speed_model][0][2][key]&0x0fff);
pwm_speed_real_dely=pwm_speed_real ;
} //否则 磨蹭时间
}
//第二速度区间---------------
else if((speed[speed_model][1][2][key]&0x0fff)>pwm_speed_real_dely) //假速度 小于 设定值
{
pwm_speed_real_dely +=((speed[speed_model][1][2][key]&0x0fff)-(speed[speed_model][0][2][key]&0x0fff))/dly_time[speed_model][1][key]/10; //假速度提升
if(heading[speed_model][1][key]==0) pwm_speed_real=(speed[speed_model][1][2][key]&0x0fff); //如果立即加电模式
else pwm_speed_real = pwm_speed_real_dely;
}
else pwm_speed_real=(speed[speed_model][1][2][key]&0x0fff); //如果速度超出预定值
OCR3B=pwm_speed_real;
OSTimeDly(OS_TICKS_PER_SEC/10); //1/20s
}
}
//运动提升电机驱动(有慢起功能,自主运行，启动有两个速度阶段，每个阶段可任意设置)----------------------------------------------
static void motor_move_t(void *p_arg) //m_action 1正转 0反转 2停 m_speed 16bit 0~1024全速
{
INT16U key=0x000b,speed_old=0x2000,move_fx[2]={0x0002,0x0002}; //运动方向 1 正 0 反 2 停
INT16U pwm_speed[2]={0x0000,0x0000},
pwm_speed_real=0x0000,pwm_speed_real_dely=0x0000; //预定pwm波 和实际pwm波 0x0000-0x03ff 值越大速度越大
p_arg = p_arg;
while(1)
{
motor_t_key=OSMboxAccept(motor_t_Box); //查看邮箱
if( motor_t_key!=0) //如果收到邮件
{
key =*motor_t_key; //获得钥匙
pwm_speed[1] =(speed[speed_model][1][0][key]&0x0fff); //提取速度目标值
move_fx[1] =speed[speed_model][1][0][key]>>12; //提取方向值
if(speed[speed_model][1][0][key]!=speed_old)
{
speed_old=speed[speed_model][1][0][key]; //保存方向速度
if(move_fx[1]!=move_fx[0]) pwm_speed_real=0x0000;pwm_speed_real_dely=0x0000;//如果方向改变 速度先降为0
pwm_speed[0]=pwm_speed[1]; //保存新速度值
move_fx[0] =move_fx[1]; //保存新方向值
switch(move_fx[0]) //运动方向切换
{
case 1:PORTA |=(1<<4);PORTA &=~(1<<5);break; //正传
case 0:PORTA &=~(1<<4);PORTA &=~(1<<5);break; //反转
case 2:pwm_speed[0]=0X0000;pwm_speed_real=0X0000;pwm_speed_real_dely=0x0000;break; //停 需求运动速度清零
}
}
}
if(((speed[speed_model][1][0][key]&0x0fff)-pwm_speed_real_dely)<(((speed[speed_model][1][0][key]&0x0fff)-(speed[speed_model][0][0][key]&0x0fff))/dly_time[speed_model][1][key]/10))
{pwm_speed_real=(speed[speed_model][1][0][key]&0x0fff);}
//第一速度区间---------------
else if((speed[speed_model][0][0][key]&0x0fff)>pwm_speed_real_dely) //假速度 小于 设定值
{
pwm_speed_real_dely += (speed[speed_model][0][0][key]&0x0fff)/dly_time[speed_model][0][key]/10; //假速度提升
if(heading[speed_model][0][key]==0) pwm_speed_real=(speed[speed_model][0][0][key]&0x0fff); //如果立即加电模式
else if(heading[speed_model][0][key]==1) pwm_speed_real = pwm_speed_real_dely; //逐步加电模式 磨蹭时间
else
{
pwm_speed_real=(speed[speed_model][0][0][key]&0x0fff);
pwm_speed_real_dely=pwm_speed_real ;
} //否则 磨蹭时间
}
//第二速度区间---------------
else if((speed[speed_model][1][0][key]&0x0fff)>pwm_speed_real_dely) //假速度 小于 设定值
{
pwm_speed_real_dely +=((speed[speed_model][1][0][key]&0x0fff)-(speed[speed_model][0][0][key]&0x0fff))/dly_time[speed_model][1][key]/10; //假速度提升
if(heading[speed_model][1][key]==0) pwm_speed_real=(speed[speed_model][1][0][key]&0x0fff); //如果立即加电模式
else pwm_speed_real = pwm_speed_real_dely;
}
else pwm_speed_real=(speed[speed_model][1][0][key]&0x0fff); //如果速度超出预定值
OCR3C=pwm_speed_real;
OSTimeDly(OS_TICKS_PER_SEC/10); //1/20s
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -