这个“手到病除”名字起的是土了点。。。。不过这么大的工程比赛还是要记录一下，也因为这个比赛初步了解了 stm32 ，不得不说和C简直没法比，当时画场地就画了有一周，一共前前后后做了有4辆车子，前两辆是毕业杨学长留下来的，哎大佬大佬，stm32的系统集成相较于我们现在所学的语言，更加的系统谨慎，层次感鲜明，很适合用来编写工业上的流水作业工程机器人感觉，要不是亲眼所见我都不会相信一个循迹模块里面可以加上一个51芯片来做灰度运算处理，这次大赛的成绩可以说更多的是站在了巨人的肩膀上。
什么是单片机：
一片半导体硅片集成：中央处理单元（CPU)、存储器（RAM、RAM）GPIO、定时器、中断系统、系统时钟电路及系统总线的微型计算机。
它具有微型计算机的属性，因为被称为单片微型计算机，简称单片机。
什么是STM32
新的给予ARM内核的32位MCU系列
--内核为ARM公司为要求高性能、低成本、低功耗的嵌入式应用专门设计的cortex-M内核
--标准的ARM架构
两者对比： STM32单片机程序都是模块化的，接口相对简单些，因为它自身带好多功能，工作速度也快。而51的自身功能少，需要外围元件多，要求对电子熟悉。
STM32系列基于专为要求高性能、低成本、低功耗的嵌入式应用专门设计的ARM Cortex-M3内核。按性能分成两个不同的系列：STM32F103“增强型”系列和STM32F101“基本型”系列。增强型系列时钟频率达到72MHz，是同类产品中性能最高的产品;基本型时钟频率为36MHz，以16位产品的价格得到比16位产品大幅提升的性能，是16位产品用户的最佳选择。两个系列都内置32K到128K的闪存，不同的是SRAM的最大容量和外设接口的组合。时钟频率72MHz时，从闪存执行代码，STM32功耗36mA，是32位市场上功耗最低的产品，相当于0.5mA/MHz。 全新STM32互连型（ConnecTIvity）系列微控制器增加一个全速USB（OTG）接口，使终端产品在连接另一个USB设备时既可以充当USB主机又可充当USB从机;还增加一个硬件支持IEEE1588精确时间协议（PTP）的以太网接口，用硬件实现这个协议可降低CPU开销，提高实时应用和联网设备同步通信的响应速度。
ARM操作页面 
//比赛相关模块代码
Motor；；；
#include "includes.h"
#include "Motor.h"
/*
PWM相关控制操作
主要是 模拟舵机 和 直流电机
*/
extern __IO uint16_t MOTOR_COUNT_1,MOTOR_SPEED_1; //测速计数器 测速速度寄存器
extern __IO uint16_t MOTOR_COUNT_2,MOTOR_SPEED_2;
extern Grayscale Front;
extern Grayscale Back; //灰度传感器 结构体
extern _pid pid_L;
extern _pid pid_R;
extern int PID_EN;
//extern VL53L0X_Typedef VL53L0X_struct;
uint16_t MOTOR_SPEED_L; //目标速度 左
uint16_t MOTOR_SPEED_R; //目标速度 右
Tracing_typedef Tracing_struct; //寻迹数据结构体
Analog_Steering_typedef Analog_Steering_1; //模拟舵机结构体1
Analog_Steering_typedef Analog_Steering_2; //模拟舵机结构体2
/*
模拟舵机驱动框架：
先在BSP.C文件中调用 “ Analog_Steering_Init(); 模拟舵机参数初始化 ”
初始化舵机舵机相关参数 包括管脚参数 PWM定时器参数
再需要设置舵机角度时 调用 void SET_Analog_Steering_Engine(Analog_Steering_typedef *Analog_Steering_Struct,uint16_t Position,uint16_t delay);
函数进行舵机位置及转动速度设置
调用例子：
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,10);
设置1号模拟舵机 位置1250 转角延时10
*/
void Analog_Steering_Init() //模拟舵机参数初始化
{
Analog_Steering_1.GPIOx=GPIOC; //端口
Analog_Steering_1.GPIO_Pin=GPIO_Pin_7; //PIN
Analog_Steering_1.TIMx=TIM3; //定时器
Analog_Steering_1._ucChannel=2; //定时器通道
Analog_Steering_1._ulFreq=50; //频率
Analog_Steering_1._ulDutyCycle=1250; //占空比 也就是位置
Analog_Steering_1.Last_Position=1250;
Analog_Steering_2.GPIOx=GPIOC; //端口
Analog_Steering_2.GPIO_Pin=GPIO_Pin_9; //PIN
Analog_Steering_2.TIMx=TIM3; //定时器
Analog_Steering_2._ucChannel=4; //定时器通道
Analog_Steering_2._ulFreq=50; //频率
Analog_Steering_2._ulDutyCycle=1000; //占空比 也就是位置
Analog_Steering_2.Last_Position=1000;
bsp_SetTIMOutPWM(Analog_Steering_1.GPIOx, Analog_Steering_1.GPIO_Pin, Analog_Steering_1.TIMx, Analog_Steering_1._ucChannel,Analog_Steering_1._ulFreq,Analog_Steering_1.Last_Position); //舵机1
bsp_SetTIMOutPWM(Analog_Steering_2.GPIOx, Analog_Steering_2.GPIO_Pin, Analog_Steering_2.TIMx, Analog_Steering_2._ucChannel,Analog_Steering_2._ulFreq,Analog_Steering_2.Last_Position); //舵机2
}
void Motor_IO_Init() //直流电机IO配置
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; //开漏输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOC, &GPIO_InitStructure);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; //开漏输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOG, &GPIO_InitStructure);
GPIO_WriteBit(GPIOC, GPIO_Pin_14, Bit_RESET); //右侧车轮 C14
GPIO_WriteBit(GPIOG, GPIO_Pin_10, Bit_SET); //左侧车轮 G10
}
/*
在main 文件中 初始化了一个软件定时器 10 MS 周期
MOTOR_COUNT_TMR_Ballback 即为定时器回调函数
同时 编码器输出线接入单片机管脚上 轮子转动一圈可产生500个脉冲
在脉冲的上升沿 会出发单片机外部中断 在中断服务函数中 （"bsp_exti.c"中） 每中断一次 MOTOR_COUNT++
每 10 MS 读取一次 MOTOR_COUNT 并置零 一次来定时测量电机转速
*/
void MOTOR_COUNT_TMR_Ballback(void *p_tmr, void *p_arg) //电机测速定时器回调函数
{
MOTOR_SPEED_1=MOTOR_COUNT_1;
MOTOR_COUNT_1=0;
MOTOR_SPEED_2=MOTOR_COUNT_2;
MOTOR_COUNT_2=0;
}
void Dc_Motor(uint8_t ON,int SPEED,uint8_t Direction)
{
if(ON==Motor_L) //左侧电机设置
{
MOTOR_SPEED_L=SPEED;
bsp_SetTIMOutPWM(GPIOB, GPIO_Pin_6, TIM4, 1,1000, MOTOR_SPEED_L);
bsp_SetTIMOutPWM(GPIOB, GPIO_Pin_7, TIM4, 2,1000, MOTOR_SPEED_L);
if(Direction==Direction_Q) GPIO_WriteBit(GPIOG, GPIO_Pin_10, Bit_SET); //左侧
else GPIO_WriteBit(GPIOG, GPIO_Pin_10, Bit_RESET); //左侧
}
if(ON==Motor_R) //右侧电机设置
{
MOTOR_SPEED_R=SPEED; //写入速度值
bsp_SetTIMOutPWM(GPIOB, GPIO_Pin_8, TIM4, 3,1000, MOTOR_SPEED_R);
bsp_SetTIMOutPWM(GPIOB, GPIO_Pin_9, TIM4, 4,1000, MOTOR_SPEED_R);
if(Direction==Direction_Q) //确定电机转向
GPIO_WriteBit(GPIOC, GPIO_Pin_14, Bit_RESET); //右侧
else
GPIO_WriteBit(GPIOC, GPIO_Pin_14, Bit_SET); //右侧
}
}
/*
PID 恒定速度调节任务
任务从系统初始化时开始运行
PID_EN标志 一开始写入 0
任务处于 while(!PID_EN) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); 处等待
当需要电机调速运行时 可通过 Pid_EN()；（定义于"pid.h"中） 结构来置位 PID_EN 等于1 同时 写入 Tracing_struct.Direction=Direction_H 或 Tracing_struct.Direction=Direction_Q 设定 运行方向
例如：
Tracing_struct.Direction=Direction_H;
Pid_EN();
当需要结束恒速运行时 只需
Pid_disEN();
即可
*/
void MOTOR_task(void *p_arg) //PID调速任务
{
OS_ERR err;
p_arg = p_arg;
uint32_t PWM;
PID_EN=0;
while(1)
{
while(!PID_EN) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //等待PID使能置位
while(PID_EN) //PID调速
{
/* 计算输出速度值 目标速度 实际速度 */
PWM=(int)PID_realize(33,MOTOR_SPEED_1,&pid_R); Dc_Motor(Motor_R,PWM,Tracing_struct.Direction); //右侧电机
PWM=(int)PID_realize(33,MOTOR_SPEED_2,&pid_L); Dc_Motor(Motor_L,PWM,Tracing_struct.Direction); //左侧电机
OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
}
Dc_Motor(Motor_L,0,Direction_H);
Dc_Motor(Motor_R,0,Direction_H); //关闭电机
}
}
/*
最优化寻迹任务
if(向前)
{
if(前面有白线)
{
正常寻迹
}
else 跑出去了
{
倒车
}
}
if(向后)
{
if(后面有白线) 正常寻迹
{
Angle_error = (Back.Position - Front.Position)*3.35; //角度偏差
Position_error = Back.Position*16; //位置偏差
if(abs((int)Angle_error)>10) //偏角过大 原地旋转
{
if(Angle_error>0)
{
顺时针转一下
}
else
{
逆时针转一下
}
}
else 小偏角 微微修正就好
{
计算左右目标差速
设置左右差速
}
}
else 没有线 倒车
{
先停住
倒车 直到有线为止
}
}
if(绕圈寻迹)
{
计算差值
设置差值
}
当想让小车停止寻迹时 运行 OSTaskSuspend(&TracingTaskTCB,&err); 挂起寻迹任务 在关闭左右电机即可
当想让小车重新运行时 先写入运行方向 在重启寻迹任务即可
Tracing_struct.Direction=Direction_H; //写入方向参数 设定寻迹任务方向
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
*/
void Tracing_task(void *p_arg) // 寻迹任务
{
OS_ERR err;
p_arg = p_arg;
float Angle_error=0;
float Position_error=0;
float K[2]={1.1773,1.0000};
float Speed_disparity=0;
/*
最优化路径寻迹
有关最优矩阵 K
请参阅 “ 吉林大学 基于机器视觉引导的两轮差速转向AGV控制问题的研究 ”
*/
int speed; //实际
OSTimeDly ( 100,OS_OPT_TIME_DLY, & err );
while(1)
{
OSTimeDly ( 2,OS_OPT_TIME_DLY, & err );
/* *************** 向前寻迹 ****************** */
if(Tracing_struct.Direction==Direction_Q)
{
if(Front.SUM>0) //前置传感器 能检测到白线的传感器数量
{
/* 灰度检测参数递交 偏角计算 */
Angle_error = (Front.Position - Back.Position)*3.35; //角度偏差
Position_error = Front.Position*16; //位置偏差
Speed_disparity=Angle_error*K[0]+Position_error*K[1]; //理论速度差值
speed=(int)Speed_disparity*60; //差速值放大 适应小车实际实际情况
Dc_Motor(Motor_L,1800+speed,Direction_Q);
Dc_Motor(Motor_R,1800-speed,Direction_Q);
}
else
{
Dc_Motor(Motor_L,0,Direction_H);
Dc_Motor(Motor_R,0,Direction_H); //先刹车
OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,5000,Direction_H);
Dc_Motor(Motor_R,5000,Direction_H);
while(Front.SUM==0) OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
}
}
/* *************** 向后寻迹 ****************** */
if(Tracing_struct.Direction==Direction_H)
{
if(Back.SUM>0) //后置传感器 能检测到白线的传感器数量
{
/* 灰度检测参数递交 偏角计算 */
Angle_error = (Back.Position - Front.Position)*3.35; //角度偏差
Position_error = Back.Position*16; //位置偏差
if(abs((int)Angle_error)>10) //偏角过大 原地旋转
{
if(Angle_error>0)
{
Dc_Motor(Motor_L,9900,Direction_H);
Dc_Motor(Motor_R,9900,Direction_Q);
}
else
{
Dc_Motor(Motor_L,9900,Direction_Q);
Dc_Motor(Motor_R,9900,Direction_H);
}
}
else
{
Speed_disparity=Angle_error*K[0]+Position_error*K[1]; //理论速度差值
speed=(int)Speed_disparity*60; //差速值放大 适应小车实际实际情况
Dc_Motor(Motor_L,7000+speed,Direction_H);
Dc_Motor(Motor_R,7000-speed,Direction_H);
}
}
else
{
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //先刹车
OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,5000,Direction_Q);
Dc_Motor(Motor_R,5000,Direction_Q);
while(Back.SUM==0) OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
}
}
/* *************** 绕圈寻迹 ****************** */
if((Tracing_struct.Direction==Direction_R))
{ /* 灰度检测参数递交 偏角计算 */
Angle_error = (Back.Position - Front.Position)*3.35; //角度偏差
Position_error = Back.Position*16; //位置偏差
Speed_disparity=Angle_error*K[0]+Position_error*K[1]; //理论速度差值
speed=(int)Speed_disparity*50; //差速值放大 适应小车实际实际情况
Dc_Motor(Motor_L,0,Direction_H);
Dc_Motor(Motor_R,7400-speed,Direction_H);
}
}
}
/*
void Analog_Steering_Engine(uint8_t ON,uint16_t Position)
模拟舵机位置设定
ON 舵机编号
Position
此函数基本不用 用下面那个
*/
void Analog_Steering_Engine(uint8_t ON,uint16_t Position)
{
if(ON==1) bsp_SetTIMOutPWM(GPIOC, GPIO_Pin_7, TIM3, 2,50, Position); //舵机1
if(ON==2) bsp_SetTIMOutPWM(GPIOC, GPIO_Pin_9, TIM3, 4,50, Position); //舵机2
}
/*
函数名:void SET_Analog_Steering_Engine(Analog_Steering_typedef *Analog_Steering_Struct,uint16_t Position,uint16_t delay)
功能：模拟舵机慢速转动到目标位置
入口参数：
* Analog_Steering_typedef *Analog_Steering_Struct： 接入舵机编号数据结构体 用于存储模拟舵机上一位置 GPIO口等
* uint16_t Position ： 舵机目标位置
* uint16_t delay 转动过程中的延时 以达到减速的目的
if(SPEED==0) 不进行慢速转动 则直接给定占空比后返回
else
从上一位置 逐渐往目标位置转动 每转动一小点位置 就延时一小段时间 在转动 再延时 以达到延时的作用
调用例子：
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,10);
设置1号模拟舵机 位置1250 转角延时10ms 延时越大 转的越慢
*/
void SET_Analog_Steering_Engine(Analog_Steering_typedef *Analog_Steering_Struct,uint16_t Position,uint16_t delay)
{
OS_ERR err;
if(delay==0) //不延时 直接设置完就走
{
bsp_SetTIMOutPWM(Analog_Steering_Struct->GPIOx, Analog_Steering_Struct->GPIO_Pin, Analog_Steering_Struct->TIMx, Analog_Steering_Struct->_ucChannel,Analog_Steering_Struct->_ulFreq,Position);
Analog_Steering_Struct->Last_Position=Position; //保存位置
return ;
}
if( Analog_Steering_Struct->Last_Position > Position) //上一位置比当前位置大 递减的方式
for(uint16_t i=Analog_Steering_Struct->Last_Position;i>=Position;i-=5)
{ /* bsp_SetTIMOutPWM() 占空比设置函数 GPIOx 端口号 GPIO_Pin TIMx 定时器x _ucChannel 定时器通道 _ulFreq 频率 i 占空比 */
bsp_SetTIMOutPWM(Analog_Steering_Struct->GPIOx, Analog_Steering_Struct->GPIO_Pin, Analog_Steering_Struct->TIMx, Analog_Steering_Struct->_ucChannel,Analog_Steering_Struct->_ulFreq, i); //设置位置
OSTimeDly (delay,OS_OPT_TIME_DLY, & err); //小间隔延时
}
else //上一位置比当前位置小 递增的方式
for(uint16_t i=Analog_Steering_Struct->Last_Position;i<=Position;i+=5)
{
bsp_SetTIMOutPWM(Analog_Steering_Struct->GPIOx, Analog_Steering_Struct->GPIO_Pin, Analog_Steering_Struct->TIMx, Analog_Steering_Struct->_ucChannel,Analog_Steering_Struct->_ulFreq,i); //设置位置
OSTimeDly (delay,OS_OPT_TIME_DLY, & err); //小间隔延时
}
Analog_Steering_Struct->Last_Position=Position; //保存位置
}
下面是代码Operation.c部分，不知道为什么我们训练的时候最好一块的转圈循迹总是出错，把循迹删掉盲走就可以？？？不知道为啥，先备注出来。
Operation.c
#include "includes.h"
#include "Maisshk.h"
#include "Operation.h"
int BCDE=1;
extern OS_TCB TracingTaskTCB; //寻迹任务控制块
extern Grayscale Front;
extern Grayscale Back; //灰度传感器 结构体
extern Grayscale Middle;
extern Uranus_typedef Uranus_Structure; //姿态数据
extern VL53L0X_Typedef VL53L0X_struct; //测距数据
extern Tracing_typedef Tracing_struct; //寻迹参数
extern Analog_Steering_typedef Analog_Steering_1; //模拟舵机结构体1
extern Analog_Steering_typedef Analog_Steering_2; //模拟舵机结构体2
extern int8_t L1,L2;
extern int8_t R1,R2; //中置传感器 标记
extern int PID_flag;
extern int extern_speed;
extern int mode_flag;//光电触发标志位
extern unsigned int flag_gd;
void Main_Operation_task(void *p_arg) //主流程任务
{
OS_ERR err;
p_arg = p_arg;
#if 1
while(BCDE) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
BCDE=1;
extern_speed = 8000;//向后循迹的速度变量
beging();
along_Stop(Direction_H,3);
extern_speed = 7000;//向后循迹的速度变量
Turn_Left();
along_Stop(Direction_H,2);
Turn_Right();
along_Stop(Direction_H,1);
Turn_Left();
Tack_1(); //抓取第一个
along_Stop(Direction_H,1);
Turn_Left();
along_Stop(Direction_H,1);
Turn_Left();
along_Stop(Direction_H,2);
Turn_Right();
along_Stop(Direction_H,1);
Turn_Left();
Put_1(); //放置第一个
Turn_Left();
along_Stop(Direction_H,1);
Turn_Left();
along_Stop(Direction_H,1);
Turn_Left();
Take_2(); //抓第二个
Turn_Left();
along_Stop(Direction_H,1);
Turn_Right();
along_Stop(Direction_H,1);
Turn_Right();
Put_2(); //放第二个
Turn_Right();
along_Stop(Direction_H,1);
Turn_Right();
along_Stop(Direction_H,1);
Turn_Right();
Take_2(); //抓取第三个
Turn_Right();
along_Stop(Direction_H,1);
Turn_Left();
along_Stop(Direction_H,2);
Turn_Left();
along_Stop(Direction_H,1);
Turn_Left();
Put_3(); //放下第三个
Turn_Left();
along_Stop(Direction_H,1);
Turn_Right();
along_Stop(Direction_H,2);
Turn_Right();
along_Stop(Direction_H,2);
Turn_Left();
along_Stop(Direction_H,1);
Turn_Right();
Tack_4(); // 抓取第四个
along_Stop(Direction_H,1);
Turn_Right();
along_Stop(Direction_H,1);
Turn_Right();
along_Stop(Direction_H,2);
Turn_Left();
//tiao:
along_Stop(Direction_H,2);
Turn_Right();
along_Stop(Direction_H,1);
Turn_Right();
Put_4();
if(mode_flag==1)
{
//原来的内容，赛道是连接起来的
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
Turn_Left();
along_Stop(Direction_H,1);
//OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
Turn_Left();
//自己写的实验室调试内容，赛道是断开的
// C//等待五秒，移动车辆
// along_Stop(Direction_H,1); //向前寻迹
// Turn_Left();
}
if(mode_flag==2)
{
//原来赛道用的内容
Turn_Right();
along_Stop(Direction_H,3);
Turn_Left();
//分段测试的开始阶段
// OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );//等待一秒，移动车辆
// along_Stop(Direction_H,1); //向前寻迹
// Turn_Left();
}
#if 1 /* 宏编译开关 */
SET_Dynamixel_Position(AX_12_NO1 ,403);
SET_Dynamixel_Position(AX_12_NO2 ,606);
SET_Analog_Steering_Engine(&Analog_Steering_1,750,10);
SET_Analog_Steering_Engine(&Analog_Steering_2,730,10);
/////////////////////////////////////////////////////////////////////
//along_Stop(Direction_Q,1); //向前寻迹 ////
// ////
// Dc_Motor(Motor_L,2000,Direction_Q);////////////////////////////////////
// Dc_Motor(Motor_R,2000,Direction_Q);////////////////////////////////////
// OSTimeDly ( 2000, OS_OPT_TIME_DLY, & err );//往前跑一点点，自己写的///
///////////////////////////////////////////////////////////////////////////
//套路开始
//倒车校正方向
Tracing_struct.Direction=Direction_H; //写入方向参数 设定寻迹任务方向，倒车校正方向
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );//延时走路
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
//Car_Stop();
//OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );//停车
along_Stop(Direction_Q,1);//循线来到夹取物前
Dc_Motor(Motor_L,1500,Direction_Q);
Dc_Motor(Motor_R,1500,Direction_Q);
OSTimeDly ( 3200, OS_OPT_TIME_DLY, & err );
/////////////////////////////////////////////////////////////////////////////////////////记得调整循迹速度
//往前走准备抓取
if(mode_flag == 1)
Tracing_struct.Direction=Direction_Q; //写入方向参数 设定寻迹任务方向，倒车校正方向
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
if(mode_flag == 1)
{
OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );//延时走路
}
else if(mode_flag==2)
{
OSTimeDly ( 120, OS_OPT_TIME_DLY, & err );//延时走路
}
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
Car_Stop();
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );//停车
SET_Analog_Steering_Engine(&Analog_Steering_2,1020,10); //夹住瓶子
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO1 ,341);
SET_Dynamixel_Position(AX_12_NO2 ,820);
OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );//抬高一点
OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
along_Stop(Direction_Q,1);//后改为Q
//along_Stop(Direction_H,2);///////////////////////////////////////////////////////////
SET_Dynamixel_Position(AX_12_NO1 ,800); //爪子举高
SET_Dynamixel_Position(AX_12_NO2 ,820);
OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
// along_Stop(Direction_Q,1);//注释了
// while(GET_Dynamixel_Position(AX_12_NO1)<800) OSTimeDly ( 20, OS_OPT_TIME_DLY, & err ); //等待举起
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q);
//停车
OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
Turn_Left();
////这里是原本打算校正一下，看来不用了，但防止会用到，所以不删，保留一下
// while(!(R2&&L2))//碰到线是1，没碰到线是0，只要有没碰到线的就进入
// {
//
//
//
// while(R1||L1)
// //右侧压线左侧未压线即左偏进入
// {
//
// if(R1&&!L1)
// {
// Car_Stop();
// OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
// Dc_Motor(Motor_L,9500,Direction_Q);//Dc_Motor(Motor_R,4500,Direction_H);//Q是往前，以爪子为准，往右校正一下
// OSTimeDly ( 50, OS_OPT_TIME_DLY, & err );
// Dc_Motor(Motor_R,9500,Direction_Q);
// OSTimeDly ( 50, OS_OPT_TIME_DLY, & err );
// Car_Stop();OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
// }
// else if(!R1&&L1)
// {
// Car_Stop();
// OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
// Dc_Motor(Motor_L,9500,Direction_Q);//Dc_Motor(Motor_R,7500,Direction_Q);//往左校正一下
// OSTimeDly ( 50, OS_OPT_TIME_DLY, & err );
// Dc_Motor(Motor_R,9500,Direction_Q);
// OSTimeDly ( 50, OS_OPT_TIME_DLY, & err );
// Car_Stop();OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
// }
// else
// {
// Car_Stop();
// OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
//// Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);
//// OSTimeDly ( 250, OS_OPT_TIME_DLY, & err );
// break;
// }
// };
//
// if((!R1)&&(!L1))//两个都是0即两个都不在线上就出发
// {
// Dc_Motor(Motor_L,2500,Direction_Q);Dc_Motor(Motor_R,2500,Direction_Q);
// OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
// } ;
//
//
// Car_Stop();
while(!(R2&&L2))//碰到线是1，没碰到线是0，只要有没碰到线的就进入
{
if(R2&&!L2)//右侧压线左侧未压线即左偏进入
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前，以爪子为准，往右校正一下
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(L2&&!R2)//判断是否右偏
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(!(L2||R2))//两个都是0即两个都不在线上就出发
{Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
}
Car_Stop();
Dc_Motor(Motor_L,3000,Direction_Q);
Dc_Motor(Motor_R,3000,Direction_Q); //向前行进 测距
while(VL53L0X_struct.distance>90) OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
Car_Stop();
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO1 ,603);
SET_Dynamixel_Position(AX_12_NO2 ,160); //向前放下
OSTimeDly ( 1500, OS_OPT_TIME_DLY, & err );
SET_Analog_Steering_Engine(&Analog_Steering_2,730,50); //放下瓶子
OSTimeDly ( 1500, OS_OPT_TIME_DLY, & err );
// Dc_Motor(Motor_L,1000,Direction_H);
//Dc_Motor(Motor_R,1000,Direction_H); //往回倒
//OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO1 ,635); //2号舵机微微同时往上抬
along_Stop(Direction_H,1);
OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
while(Back.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
BEEP_Function(300);
Turn_Left();
Dc_Motor(Motor_L,3000,Direction_H);
Dc_Motor(Motor_R,3000,Direction_H);
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); //再往前走一段
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO1 ,160);
SET_Dynamixel_Position(AX_12_NO2 ,500); //两台舵机放至最低
SET_Analog_Steering_Engine(&Analog_Steering_2,1020,10); //爪子成夹起状态
//Turn_Left();
along_Stop(Direction_Q,1);
along_Stop(Direction_H,2); //现场只有两条白线 到时候要去掉一条
Dc_Motor(Motor_L,3000,Direction_H);
Dc_Motor(Motor_R,3000,Direction_H);
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); //再往前走一段
Car_Stop();
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
Turn_Left();
along_Stop(Direction_R,2); /* 绕圈 */
// Dc_Motor(Motor_L,4500,Direction_Q);
// Dc_Motor(Motor_R,7000,Direction_Q);
Dc_Motor(Motor_L,3500,Direction_H);
Dc_Motor(Motor_R,7000,Direction_H);
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); //再往前走一段
Car_Stop();
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
Turn_Right();
extern_speed=5000; //设定撞击速度
Tracing_struct.Direction=Direction_H; //写入方向参数 设定寻迹任务方向
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
while(flag_gd==0) OSTimeDly ( 5, OS_OPT_TIME_DLY, & err );
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );//////////////////////////第一次撞击延时，待改动
Car_Stop();
extern_speed=7000; //还原寻迹速度
OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,2000,Direction_Q);
Dc_Motor(Motor_R,2000,Direction_Q);
//along_Stop(Direction_Q,1);
while(Front.SUM<=4) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,3000,Direction_Q);
Dc_Motor(Motor_R,3000,Direction_Q);
OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往前走一段
Car_Stop();
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
Turn_Left();
along_Stop(Direction_R,1); /* 绕圈 */
Dc_Motor(Motor_L,0,Direction_H);
Dc_Motor(Motor_R,7000,Direction_H);
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err ); //再往前走一段
Car_Stop();
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
Turn_Left();
along_Stop(Direction_H,2);
/* 过坎程序段 */
extern_speed=4000; //设定撞击速度
Tracing_struct.Direction=Direction_H; //写入方向参数 设定寻迹任务方向
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
while(Back.SUM<=3) OSTimeDly ( 20, OS_OPT_TIME_DLY, & err );
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
Car_Stop();
extern_speed=5000; //还原寻迹速度
Tracing_struct.Direction=Direction_H;
Pid_EN(); //启动PID调速
// while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
while(Front.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
Pid_disEN(); //关闭PID调速
Tracing_struct.Direction=Direction_H; //写入方向参数 设定寻迹任务方向
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
while(flag_gd==0) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );/////////////////////////第二次撞击延时，待改动
Car_Stop();//关闭电机 碰撞顶端柱子
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
/* 往回倒车一段 */
Tracing_struct.Direction=Direction_Q; //写入方向参数 设定寻迹任务方向
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
while(Front.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //等待前置传感器遇到障碍
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
Tracing_struct.Direction=Direction_Q;
Pid_EN(); //启动PID调速
/* 第二次上坎 */
while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
Pid_disEN(); //关闭PID调速
//过坎结束
along_Stop(Direction_Q,1);
Car_Stop();
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
if(mode_flag==1)
{
Turn_Left();
along_Stop(Direction_H,2);
Turn_Right();
}
if(mode_flag==2)
{
Turn_Left();
along_Stop(Direction_H,2);
Turn_Left();
}
extern_speed = 8000;//向后循迹的速度变量
along_Stop(Direction_H,1);
Dc_Motor(Motor_L,3000,Direction_H);
Dc_Motor(Motor_R,3000,Direction_H);
while(Front.SUM) OSTimeDly( 10, OS_OPT_TIME_DLY, & err );
OSTimeDly( 1000, OS_OPT_TIME_DLY, & err );
Car_Stop();
for(int i=5;i>0;i--)
{
BEEP_Function(300);
OSTimeDly( 500, OS_OPT_TIME_DLY, & err );
}
#endif
#endif
while(1) OSTimeDly( 10, OS_OPT_TIME_DLY, & err );
}
void Car_Stop()
{
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
}
void beging()
{
OS_ERR err;
Dc_Motor(Motor_L,5000,Direction_H);
Dc_Motor(Motor_R,5000,Direction_H); //倒车
while(Back.SUM<3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //走空地
while(Back.SUM>=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //走横线
BEEP_Function(300);
}
/*
void along_Stop(uint8_t Direction,uint8_t Number)
功能： 让现车走过几条线后停止下来
Direction： 方向 Direction_Q 向前 Direction_H 向后
Number ： 要走过线的数量
*/
int LR=1;
void along_Stop(uint8_t Direction,uint8_t Number)
{
OS_ERR err;
Tracing_struct.Direction=Direction; //写入方向参数 设定寻迹任务方向
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
while(Number--)
{
if(Direction==Direction_H) //倒着走
{
while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
while(Back.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
BEEP_Function(300);
}
if(Direction==Direction_R) //倒着绕圈走
{
if(LR) //LR=1时，为前半圈，LR=0时，为后半圈
{
while(!L2) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
while(L2) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
BEEP_Function(300);
LR=0;
}
else
{
while(!R2) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
while(R2) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
BEEP_Function(300);
LR=1;
}
}
if(Direction==Direction_Q) //正着走
{
while(Front.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
while(Front.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
BEEP_Function(300);
}
}
/*
前/后 寻迹是结束上面的循环之后 先关闭寻迹任务 再往后倒一段
绕圈 是结束上面的循环之后 再往后寻迹一段 再关闭寻迹任务
*/
if(Direction==Direction_H) //倒着走
{
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
Dc_Motor(Motor_L,7000,Direction_H);
Dc_Motor(Motor_R,7000,Direction_H);
OSTimeDly ( 240, OS_OPT_TIME_DLY, & err ); //再往前走一段
}
if(Direction==Direction_R) //倒着绕圈走
{
// OSTimeDly ( 320, OS_OPT_TIME_DLY, & err ); //再往前走一段
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
}
if(Direction==Direction_Q) //正着走
{
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
Dc_Motor(Motor_L,3000,Direction_Q);
Dc_Motor(Motor_R,3000,Direction_Q);
OSTimeDly ( 230, OS_OPT_TIME_DLY, & err ); //再往前走一段
}
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
}
void Turn_Right() //倒车右转其实就是顺时针转动九十度
{
OS_ERR err;
int16_t Yaw=0; //航向缓存
Yaw=Uranus_Structure.Yaw/10; //当前航向
Yaw=(360+(Yaw+82))%360; //计算目标航向
printf("当前航向%d 目标航向%d",Uranus_Structure.Yaw/10,Yaw);
Dc_Motor(Motor_L,8000,Direction_Q);
Dc_Motor(Motor_R,8000,Direction_H);
while(1)
{
if(Yaw== (Uranus_Structure.Yaw/10)) break;
if(Yaw== (Uranus_Structure.Yaw/10+1)) break;
if(Yaw== (Uranus_Structure.Yaw/10+2)) break;
OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
}
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
}
void Turn_Left() //倒车左转
{
OS_ERR err;
int16_t Yaw=0; //航向缓存
Yaw=Uranus_Structure.Yaw/10; //当前航向
Yaw=(360+(Yaw-82))%360; //计算目标航向
printf("当前航向%d 目标航向%d\r\n",Uranus_Structure.Yaw/10,Yaw);
Dc_Motor(Motor_L,8000,Direction_H);
Dc_Motor(Motor_R,8000,Direction_Q);
while(1)
{
if(Yaw== (Uranus_Structure.Yaw/10)) break;
if(Yaw== (Uranus_Structure.Yaw/10+1)) break;
if(Yaw== (Uranus_Structure.Yaw/10+2)) break;
OSTimeDly ( 1, OS_OPT_TIME_DLY, & err );
}
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
}
void Tack_1()
{
OS_ERR err;
SET_Dynamixel_SPEED(AX_12_NO1 ,60);
SET_Dynamixel_SPEED(AX_12_NO2 ,60);
SET_Dynamixel_Position(AX_12_NO2 ,820);
SET_Dynamixel_Position(AX_12_NO1 ,160);
OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,10); //1号舵机竖直
SET_Analog_Steering_Engine(&Analog_Steering_2,730,10);
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //往后倒一点
Tracing_struct.Direction=Direction_Q;
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
while(VL53L0X_struct.distance>120) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
BEEP_Function(300);
///////////////* 向前到位 准备抓取 */
SET_Analog_Steering_Engine(&Analog_Steering_2,1020,0); //模拟舵机夹紧
OSTimeDly (400, OS_OPT_TIME_DLY, & err ); //等待延时
SET_Dynamixel_Position(AX_12_NO1 ,360); //1号舵机往上微微举起
// while(GET_Dynamixel_Position(AX_12_NO1)<250) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err ); //往后倒一点
SET_Analog_Steering_Engine(&Analog_Steering_1,750,10); //模拟舵机1旋转
Dc_Motor(Motor_L,4000,Direction_H);
Dc_Motor(Motor_R,4000,Direction_H);
OSTimeDly ( 250, OS_OPT_TIME_DLY, & err ); //往后倒一点
SET_Dynamixel_Position(AX_12_NO1 ,160);
SET_Dynamixel_Position(AX_12_NO2 ,520);
}
//等待调试
void Tack_4()
{
OS_ERR err;
SET_Dynamixel_SPEED(AX_12_NO1 ,40);
SET_Dynamixel_SPEED(AX_12_NO2 ,40);
SET_Dynamixel_Position(AX_12_NO1 ,761);
SET_Dynamixel_Position(AX_12_NO2 ,823);
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,10); //1号舵机竖直
SET_Analog_Steering_Engine(&Analog_Steering_2,730,10);
OSTimeDly (2500, OS_OPT_TIME_DLY, & err );
Tracing_struct.Direction=Direction_Q;
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
while(VL53L0X_struct.distance>190) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );//距离到150为止都是循迹的，小于150之后盲走一小段
//OSTimeDly (1000, OS_OPT_TIME_DLY, & err );
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
OSTimeDly (10, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,2000,Direction_Q);
Dc_Motor(Motor_R,2000,Direction_Q);
OSTimeDly (800, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
BEEP_Function(300);
/* 向前到位 准备抓取 */
SET_Dynamixel_Position(AX_12_NO1 ,506);
SET_Dynamixel_Position(AX_12_NO2 ,160);
OSTimeDly (500, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO1 ,548);
OSTimeDly (2500, OS_OPT_TIME_DLY, & err );
SET_Analog_Steering_Engine(&Analog_Steering_2,1020,0); //模拟舵机夹紧
OSTimeDly (500, OS_OPT_TIME_DLY, & err ); //等待延时
OSTimeDly (500, OS_OPT_TIME_DLY, & err );
SET_Analog_Steering_Engine(&Analog_Steering_1,750,10); //模拟舵机1旋转
OSTimeDly (500, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO1 ,524);
OSTimeDly (500, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO2 ,302);
OSTimeDly (500, OS_OPT_TIME_DLY, & err );
// SET_Dynamixel_Position(AX_12_NO1 ,524);
// SET_Dynamixel_Position(AX_12_NO2 ,162);
// OSTimeDly (500, OS_OPT_TIME_DLY, & err );
//
// along_Stop(Direction_H,1);
// OSTimeDly (500, OS_OPT_TIME_DLY, & err );
// SET_Dynamixel_Position(AX_12_NO1 ,160);
// SET_Dynamixel_Position(AX_12_NO2 ,520);
//
// OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
}
void Put_1()
{
OS_ERR err;
while(!(R2&&L2))//碰到线是1，没碰到线是0，只要有没碰到线的就进入
{
if(R2&&!L2)//右侧压线左侧未压线即左偏进入
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前，以爪子为准，往右校正一下
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(L2&&!R2)//判断是否右偏
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(!(L2||R2))//两个都是0即两个都不在线上就出发
{Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
}
Car_Stop();
SET_Dynamixel_Position(AX_12_NO2 ,818); //数字舵机举起来
SET_Dynamixel_Position(AX_12_NO1 ,271);
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,5); //1号舵机竖直
//Analog_Steering_Engine(2,850);
while(GET_Dynamixel_Position(AX_12_NO2)<800) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,2500,Direction_Q);
Dc_Motor(Motor_R,2500,Direction_Q); //走
while(VL53L0X_struct.distance>165) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //测距
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //停
OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO2 ,818); //2号数字舵机往下放一放
SET_Dynamixel_Position(AX_12_NO1 ,181);
OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
SET_Analog_Steering_Engine(&Analog_Steering_2,850,30); //放开物品
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,4000,Direction_H);
Dc_Motor(Motor_R,4000,Direction_H);
while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
while(Back.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
BEEP_Function(300);
Dc_Motor(Motor_L,7000,Direction_H);
Dc_Motor(Motor_R,7000,Direction_H);
OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往前走一段
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
SET_Dynamixel_Position(AX_12_NO2 ,500); //放下数字舵机2
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
}
//
//void Put_1()
//{
// OS_ERR err;
//
//
// while(!(R2&&L2))//碰到线是1，没碰到线是0，只要有没碰到线的就进入
// {
// if(R2&&!L2)//右侧压线左侧未压线即左偏进入
// {
// Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
// Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前，以爪子为准，往右校正一下
// OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
// }
// if(L2&&!R2)//判断是否右偏
// {
// Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
// Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
// OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
// }
// if(!(L2||R2))//两个都是0即两个都不在线上就出发
// {Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
// OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
// }
// Car_Stop();
//// OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
//// //Car_Stop();
//// SET_Dynamixel_SPEED(AX_12_NO1 ,40);
//// SET_Dynamixel_SPEED(AX_12_NO2 ,40);
////
//// OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
////
//// SET_Dynamixel_Position(AX_12_NO2 ,808); ////////////////////////////////
//// SET_Dynamixel_Position(AX_12_NO1 ,310);
//// SET_Analog_Steering_Engine(&Analog_Steering_1,1250,0); //1号舵机竖直
//// // SET_Analog_Steering_Engine(&Analog_Steering_2,730,0);
////
//// Dc_Motor(Motor_L,3000,Direction_Q);Dc_Motor(Motor_R,3000,Direction_Q);
////
//// while(VL53L0X_struct.distance>135) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
////
//// Car_Stop();
////
////
//// BEEP_Function(300);
//// OSTimeDly (100, OS_OPT_TIME_DLY, & err ); //等待延时
////
//// SET_Dynamixel_Position(AX_12_NO2 ,753);
//// OSTimeDly (300, OS_OPT_TIME_DLY, & err ); //等待延时
//// ////////////////////////////////
//// SET_Dynamixel_Position(AX_12_NO1 ,238);
//// OSTimeDly (300, OS_OPT_TIME_DLY, & err ); //等待延时
//// SET_Analog_Steering_Engine(&Analog_Steering_2,730,0);
//// OSTimeDly (300, OS_OPT_TIME_DLY, & err ); //等待延时
////
//// SET_Analog_Steering_Engine(&Analog_Steering_1,750,10); //模拟舵机1旋转
////
//// Dc_Motor(Motor_L,4000,Direction_H);
//// Dc_Motor(Motor_R,4000,Direction_H); //倒车
////
//// while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
//// while(Back.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
//// BEEP_Function(300);
////
//// OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //先倒车 再收爪子
////
//// Dc_Motor(Motor_L,6000,Direction_H);
//// Dc_Motor(Motor_R,6000,Direction_H);
//// OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //再往前走一段
////
//// Dc_Motor(Motor_L,0,Direction_Q);
//// Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
////
//// SET_Dynamixel_Position(AX_12_NO1 ,160);
//// SET_Dynamixel_Position(AX_12_NO2 ,520);
////
//// OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
//
// SET_Dynamixel_Position(AX_12_NO2 ,808); //数字舵机举起来
// SET_Dynamixel_Position(AX_12_NO1 ,311);
// SET_Analog_Steering_Engine(&Analog_Steering_1,1250,5); //1号舵机竖直
// //Analog_Steering_Engine(2,850);
// while(GET_Dynamixel_Position(AX_12_NO2)<800) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
//
// Dc_Motor(Motor_L,2500,Direction_Q);
// Dc_Motor(Motor_R,2500,Direction_Q); //走
//
// while(VL53L0X_struct.distance>135) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //测距
//
// Dc_Motor(Motor_L,0,Direction_Q);
// Dc_Motor(Motor_R,0,Direction_Q); //停
//
// OSTimeDly ( 100, OS_OPT_TIME_DLY, & err );
//
// SET_Dynamixel_Position(AX_12_NO2 ,818); //2号数字舵机往下放一放
// SET_Dynamixel_Position(AX_12_NO1 ,181);
// OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
//
// SET_Analog_Steering_Engine(&Analog_Steering_2,850,30); //放开物品
//
// OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//
// Dc_Motor(Motor_L,4000,Direction_H);
// Dc_Motor(Motor_R,4000,Direction_H);
//
// while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
// while(Back.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
// BEEP_Function(300);
//
// Dc_Motor(Motor_L,7000,Direction_H);
// Dc_Motor(Motor_R,7000,Direction_H);
// OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往前走一段
//
// Dc_Motor(Motor_L,0,Direction_Q);
// Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
//
// SET_Dynamixel_Position(AX_12_NO1 ,160);
// SET_Dynamixel_Position(AX_12_NO2 ,500); //放下数字舵机2
//
// OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
//
//}
void Take_2()
{
OS_ERR err;
while(!(R2&&L2))
{
if(R2&&!L2)
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,6000,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(L2&&!R2)
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,6000,Direction_Q);
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(!(L2||R2)){Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
}
Car_Stop();
OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO2 ,820);
SET_Dynamixel_Position(AX_12_NO1 ,160);
//SET_Dynamixel_Position(AX_12_NO2 ,808);////////////////////////////////
OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,0); //1号舵机竖直
SET_Analog_Steering_Engine(&Analog_Steering_2,730,0);
Dc_Motor(Motor_L,3000,Direction_Q);Dc_Motor(Motor_R,3000,Direction_Q);
while(VL53L0X_struct.distance>150) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
Car_Stop();
BEEP_Function(300);
/* 向前到位 准备抓取 */
SET_Analog_Steering_Engine(&Analog_Steering_2,1030,0); //模拟舵机夹紧
OSTimeDly (400, OS_OPT_TIME_DLY, & err ); //等待延时
SET_Dynamixel_Position(AX_12_NO1 ,260); //1号舵机往上微微举起
// while(GET_Dynamixel_Position(AX_12_NO1)<250) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
OSTimeDly (800, OS_OPT_TIME_DLY, & err ); //等待延时
SET_Analog_Steering_Engine(&Analog_Steering_1,750,10); //模拟舵机1旋转
Dc_Motor(Motor_L,4000,Direction_H);
Dc_Motor(Motor_R,4000,Direction_H); //倒车
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //先倒车 再收爪子
SET_Dynamixel_Position(AX_12_NO1 ,160);
SET_Dynamixel_Position(AX_12_NO2 ,500);
while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
while(Back.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
BEEP_Function(300);
Dc_Motor(Motor_L,5000,Direction_H);
Dc_Motor(Motor_R,5000,Direction_H);
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //再往前走一段
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
}
//////////////////////////////////////////////////////////////
void Put_2()
{
OS_ERR err;
while(!(R2&&L2))//碰到线是1，没碰到线是0，只要有没碰到线的就进入
{
if(R2&&!L2)//右侧压线左侧未压线即左偏进入
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前，以爪子为准，往右校正一下
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(L2&&!R2)//判断是否右偏
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(!(L2||R2))//两个都是0即两个都不在线上就出发
{Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
}
Car_Stop();
//Car_Stop();
SET_Dynamixel_Position(AX_12_NO1 ,660);
SET_Dynamixel_Position(AX_12_NO2 ,160);
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,15); //1号舵机竖直////////////////////////////////////////////////
Tracing_struct.Direction=Direction_Q;
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
while(VL53L0X_struct.distance>90) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q);//关闭左右电机
SET_Dynamixel_Position(AX_12_NO1 ,620);
BEEP_Function(300);
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
SET_Analog_Steering_Engine(&Analog_Steering_2,750,30);
SET_Dynamixel_Position(AX_12_NO1 ,700);//
OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,3000,Direction_H);
Dc_Motor(Motor_R,3000,Direction_H); //走曲线
along_Stop(Direction_H,1);
SET_Dynamixel_Position(AX_12_NO1 ,160);
SET_Dynamixel_Position(AX_12_NO2 ,500);
OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
}
/////////////////////////////////////////////////////////////
//
void Put_3()
{
OS_ERR err;
while(!(R2&&L2))
{
if(R2&&!L2)
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,7000,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(L2&&!R2)
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,7000,Direction_Q);
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(!(L2||R2)){Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
}
Car_Stop();
SET_Dynamixel_Position(AX_12_NO1 ,546);
SET_Dynamixel_Position(AX_12_NO2 ,160);
SET_Analog_Steering_Engine(&Analog_Steering_1,1250,15); //1号舵机竖直
/* 前进 */
Tracing_struct.Direction=Direction_Q;
OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
OSTimeDly ( 1800, OS_OPT_TIME_DLY, & err );
OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
BEEP_Function(300);
Dc_Motor(Motor_L,3000,Direction_Q);
Dc_Motor(Motor_R,3050,Direction_Q); //走曲线
OSTimeDly ( 3200, OS_OPT_TIME_DLY, & err );
BEEP_Function(300);
while(VL53L0X_struct.distance>75) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,0,Direction_Q);
Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
BEEP_Function(300);
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
SET_Analog_Steering_Engine(&Analog_Steering_2,850,30); //
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,3000,Direction_H);
Dc_Motor(Motor_R,3100,Direction_H);
OSTimeDly ( 1500, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO1 ,160); //下放舵机至最低处
SET_Dynamixel_Position(AX_12_NO2 ,500);
BEEP_Function(300);
OSTimeDly ( 2500, OS_OPT_TIME_DLY, & err );
along_Stop(Direction_H,1);
}
//void Put_3()
//{
// OS_ERR err;
//
// while(!(R2&&L2))//碰到线是1，没碰到线是0，只要有没碰到线的就进入
// {
// if(R2&&!L2)//右侧压线左侧未压线即左偏进入
// {
// Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
// Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前，以爪子为准，往右校正一下
// OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
// }
// if(L2&&!R2)//判断是否右偏
// {
// Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
// Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
// OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
// }
// if(!(L2||R2))//两个都是0即两个都不在线上就出发
// {Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
// OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
// }
// Car_Stop();
//
// //Car_Stop();
//
// SET_Dynamixel_Position(AX_12_NO1 ,700);
// SET_Dynamixel_Position(AX_12_NO2 ,300);
//
// SET_Analog_Steering_Engine(&Analog_Steering_1,1250,15); //1号舵机竖直
//
//// Tracing_struct.Direction=Direction_Q;
//// OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
////
//// OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
////
//// OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
//
// BEEP_Function(300);
//
// Dc_Motor(Motor_L,3000,Direction_Q);
// Dc_Motor(Motor_R,3000,Direction_Q); //走曲线
//
// // OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
//
// // BEEP_Function(300);
//
// //OS_TaskResume(&TracingTaskTCB,&err); //恢复寻迹任务
//
// while(VL53L0X_struct.distance>80) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
//
// //OSTaskSuspend(&TracingTaskTCB,&err); //挂起寻迹任务
// //while(VL53L0X_struct.distance>95) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
//
//
// Dc_Motor(Motor_L,0,Direction_Q);
// Dc_Motor(Motor_R,0,Direction_Q); //关闭左右电机
//
//
//
// SET_Dynamixel_Position(AX_12_NO2 ,160);
// SET_Dynamixel_Position(AX_12_NO1 ,600);
//
//
//
// BEEP_Function(300);
//
// OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
// SET_Analog_Steering_Engine(&Analog_Steering_2,800,30); //
// OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//
// Dc_Motor(Motor_L,3000,Direction_H);
// Dc_Motor(Motor_R,3000,Direction_H);
//
// // OSTimeDly ( 2500, OS_OPT_TIME_DLY, & err );
//
// SET_Dynamixel_Position(AX_12_NO1 ,160); //下放舵机至最低处
// SET_Dynamixel_Position(AX_12_NO2 ,500);
//
// BEEP_Function(300);
//
// //OSTimeDly ( 2000, OS_OPT_TIME_DLY, & err );
//
// while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
// while(Back.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
// BEEP_Function(300);
//
// Dc_Motor(Motor_L,5000,Direction_H);
// Dc_Motor(Motor_R,5000,Direction_H);
// OSTimeDly ( 500, OS_OPT_TIME_DLY, & err ); //再往前走一段
//
// Dc_Motor(Motor_L,0,Direction_H);
// Dc_Motor(Motor_R,0,Direction_H); //关闭左右电机
//
// OSTimeDly ( 200, OS_OPT_TIME_DLY, & err );
// // BEEP_Function(300);
// // along_Stop(Direction_H,1);
//}
void Put_4()
{
OS_ERR err;
while(!(R2&&L2))//碰到线是1，没碰到线是0，只要有没碰到线的就进入
{
if(R2&&!L2)//右侧压线左侧未压线即左偏进入
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,9500,Direction_Q);Dc_Motor(Motor_R,4500,Direction_H);//Q是往前，以爪子为准，往右校正一下
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(L2&&!R2)//判断是否右偏
{
Car_Stop(); OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,4500,Direction_H);Dc_Motor(Motor_R,9500,Direction_Q);//往左校正一下
OSTimeDly ( 80, OS_OPT_TIME_DLY, & err );
}
if(!(L2||R2))//两个都是0即两个都不在线上就出发
{Dc_Motor(Motor_L,1500,Direction_Q);Dc_Motor(Motor_R,1500,Direction_Q);}
OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
}
Car_Stop();
OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
Dc_Motor(Motor_L,3000,Direction_Q);
Dc_Motor(Motor_R,3000,Direction_Q); //先跑起来
SET_Dynamixel_Position(AX_12_NO2 ,820); //抬起爪子
SET_Dynamixel_Position(AX_12_NO1 ,160);
SET_Analog_Steering_Engine(&Analog_Steering_1,750,15); //1号舵机竖直
while(VL53L0X_struct.distance>170) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
Car_Stop();
OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO2 ,732);
OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
SET_Analog_Steering_Engine(&Analog_Steering_2,770,15); //放开瓶子
OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//
// SET_Dynamixel_Position(AX_12_NO2 ,800);
// SET_Dynamixel_Position(AX_12_NO1 ,221);
// OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );//微微上台
Dc_Motor(Motor_L,4000,Direction_H);
Dc_Motor(Motor_R,4000,Direction_H); //后退
OSTimeDly ( 1500, OS_OPT_TIME_DLY, & err );
SET_Dynamixel_Position(AX_12_NO2 ,510); //往下收起数字舵机
SET_Analog_Steering_Engine(&Analog_Steering_2,1010,15); //模拟舵机爪子夹起
while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
while(Back.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
BEEP_Function(300);
Dc_Motor(Motor_L,7000,Direction_H);
Dc_Motor(Motor_R,7000,Direction_H);
OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往后走一段
Car_Stop();
//Car_Stop();
// OSTimeDly ( 300, OS_OPT_TIME_DLY, & err );
//
// Dc_Motor(Motor_L,3000,Direction_Q);
// Dc_Motor(Motor_R,3000,Direction_Q); //先跑起来
//
// SET_Dynamixel_Position(AX_12_NO2 ,300); //抬起爪子
// SET_Dynamixel_Position(AX_12_NO1 ,700);
//
// SET_Analog_Steering_Engine(&Analog_Steering_1,750,15); //1号舵机竖直
//
// while(VL53L0X_struct.distance>150) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err );
//
// Car_Stop();
//
// OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );
//
// SET_Dynamixel_Position(AX_12_NO2 ,370);
// SET_Dynamixel_Position(AX_12_NO1 ,490);
//
// OSTimeDly ( 1000, OS_OPT_TIME_DLY, & err );
//
// SET_Analog_Steering_Engine(&Analog_Steering_2,780,15); //放开瓶子
// OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );
//
// Dc_Motor(Motor_L,1500,Direction_H);Dc_Motor(Motor_R,1500,Direction_H);
// OSTimeDly ( 400, OS_OPT_TIME_DLY, & err );
//
//
//
//// SET_Dynamixel_Position(AX_12_NO2 ,800);
//// SET_Dynamixel_Position(AX_12_NO1 ,221);
//// OSTimeDly ( 500, OS_OPT_TIME_DLY, & err );//微微上台
//
// Dc_Motor(Motor_L,4000,Direction_H);
// Dc_Motor(Motor_R,4000,Direction_H); //后退
// OSTimeDly ( 800, OS_OPT_TIME_DLY, & err );
//
// SET_Dynamixel_Position(AX_12_NO2 ,520); //抬起爪子
// SET_Dynamixel_Position(AX_12_NO1 ,160);
// SET_Analog_Steering_Engine(&Analog_Steering_1,1250,15); //1号舵机竖直
// SET_Analog_Steering_Engine(&Analog_Steering_2,1020,15);
//
//
// Dc_Motor(Motor_L,4000,Direction_H);
// Dc_Motor(Motor_R,4000,Direction_H); //后退
//
// while(Back.SUM<=3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //先是在主线上
// while(Back.SUM>3) OSTimeDly ( 10, OS_OPT_TIME_DLY, & err ); //压到横线
// BEEP_Function(300);
//
// Dc_Motor(Motor_L,6000,Direction_H);
// Dc_Motor(Motor_R,6000,Direction_H);
// OSTimeDly ( 300, OS_OPT_TIME_DLY, & err ); //再往后走一段
//
// Car_Stop();
}
Comments | 1 条评论
谢谢分享,每日打卡,学生卡~