功能很多的单片机智能小车源码(PS2手柄控制+舵机+W25Q64存储) -

/*
1、串口1调试 ok
2、串口2调试 ok
3、定时器0调试 ok
4、定时器1调试 ok
5、PS2手柄调试 ok
6、4通道PWM调试 ok
7、舵机调试 ok
8、W25Q64存储调试 ok
调试的过程：
如上，一个一个模块调通，最后组合
左边的目录结构就是正队每一个模块调试好做成一个模块文件，便于移植
看程序方法：
看程序的时候，从main文件的main函数看起
基本的程序思路是
主函数->各个模块初始化->大循环while(1)
->中断(串口、定时器等)
大家在深究本程序时，建议大家先去了解各个模块的原理，然后看懂文件结构和程序结构，最后再细究算法问题
智能传感器版本增加内容：
所需硬件
循迹模块 2个
超声波模块 1个
颜色识别模块 1个
声音模块 1个
木块红、蓝、绿各一个
IO口分布
循迹左 P0^6
循迹右 P0^7
超声波 P1^6
颜色 P1^2
声音 P1^0
智能识别功能（手柄绿灯模式左边上下左右和右边上下左右）
功能0 循迹模式
功能1 声控夹取
功能2 自由避障
功能3 颜色识别
功能4 定距夹取
功能5 跟随功能
功能6 循迹避障
功能7 循迹识别
功能8 循迹定距
手动遥控功能
1、手柄遥控
2、APP遥控
3、WIFI遥控
图形化编程功能
功能切换：绿灯模式下通过左边上下左右键切换功能，通过蜂鸣器的声音的响声播报功能
*/
//头文件
#include <stdio.h>
#include <string.h>
#include "z_stc15.h"
#include "z_main.h"
#include "z_uart.h"
#include "z_delay.h"
#include "z_gpio.h"
#include "z_ps2.h"
#include "z_pwm.h"
#include "z_timer.h"
#include "z_w25q64.h"
#include "z_global.h"
#include "z_adc.h"
#include "z_sensor.h"
//定义些变量，关于变量的作用，可以查找下是哪里用到了，具体看在那里是什么作用
u16 adc7_value = 0, adc0_value = 0;
u16 do_start_index, do_time, group_num_start, group_num_end, group_num_times;
u8 i;
u8 car_dw = 1;
u32 bias_systick_ms_bak = 0;
u8 djBiasSaveFlag = 0;
u8 dbt_flag = 0;
float vol_adc = 0;u32 save_addr_sector = 0, save_action_index_bak = 0;
u8 psx_buf[9]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; //存储手柄信息
code const char *pre_cmd_set_red[PSX_BUTTON_NUM] = {
"<PS2_RED01:#005P0600T2000!^$DST!>", //L2
"<PS2_RED02:#005P2400T2000!^$DST!>", //R2
"<PS2_RED03:#004P0600T2000!^$DST!>", //L1
"<PS2_RED04:#004P2400T2000!^$DST!>", //R1
"<PS2_RED05:#002P2400T2000!^$DST!>", //RU
"<PS2_RED06:#003P2400T2000!^$DST!>", //RR
"<PS2_RED07:#002P0600T2000!^$DST!>", //RD
"<PS2_RED08:#003P0600T2000!^$DST!>", //RL
"<PS2_RED09:$!>", //SE
"<PS2_RED10:$!>", //AL
"<PS2_RED11:$!>", //AR
"<PS2_RED12:$DJR!>", //ST
"<PS2_RED13:#001P0600T2000!^$DST!>", //LU
"<PS2_RED14:#000P0600T2000!^$DST!>", //LR
"<PS2_RED15:#001P2400T2000!^$DST!>", //LD
"<PS2_RED16:#000P2400T2000!^$DST!>", //LL
};
code const char *pre_cmd_set_grn[PSX_BUTTON_NUM] = {
"<PS2_GRN01:$!>", //L2
"<PS2_GRN02:$!>", //R2
"<PS2_GRN03:$!>", //L1
"<PS2_GRN04:$!>", //R1
"<PS2_GRN05:$SMODE5!>", //RU
"<PS2_GRN06:$SMODE8!>", //RR
"<PS2_GRN07:$SMODE6!>", //RD
"<PS2_GRN08:$SMODE7!>", //RL
"<PS2_GRN09:$SMODE0!>", //SE
"<PS2_GRN10:$!>", //AL
"<PS2_GRN11:$!>", //AR
"<PS2_GRN12:$DJR!>", //ST
"<PS2_GRN13:$SMODE1!>", //LU
"<PS2_GRN14:$SMODE4!>", //LR
"<PS2_GRN15:$SMODE2!>", //LD
"<PS2_GRN16:$SMODE3!>", //LL
};
code const char *action_pre_group[] = {
//动作生成数据为(可直接全选复制粘贴到程序中)：
//偏差调节组
"{G0000#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1500T1000!}",
//直立
"{G0001#000P1500T1500!#001P1500T1500!#002P1500T1500!#003P1500T1500!#004P1500T1500!#005P1500T0000!}",
//蜷缩
"{G0002#000P1500T1500!#001P2200T1500!#002P2500T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
//大前抓右放 K0001(3-11)
"{G0003#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1000T1000!}",
"{G0004#000P1500T1000!#001P1100T1000!#002P1600T1000!#003P2100T1000!#004P1500T1000!#005P1000T0000!}",
"{G0005#000P1500T1500!#001P1050T1500!#002P1600T1000!#003P2100T1000!#004P1500T1000!#005P2000T0000!}",
"{G0006#000P1500T1000!#001P1800T1000!#002P1800T1000!#003P2100T1000!#004P1500T1000!#005P2000T0000!}",
"{G0007#000P0800T1000!#001P1500T1000!#002P1500T1000!#003P2100T1000!#004P1500T1000!#005P2000T0000!}",
"{G0008#000P0800T1000!#001P1300T1000!#002P1800T1000!#003P2100T1000!#004P1500T1000!#005P2000T0000!}",
"{G0009#000P0800T1000!#001P1300T1000!#002P1800T1000!#003P2100T1000!#004P1500T1000!#005P1000T0000!}",
"{G0010#000P0800T1000!#001P1600T1000!#002P1800T1000!#003P2100T1000!#004P1500T1000!#005P1000T0000!}",
"{G0011#000P1500T1500!#001P2200T1500!#002P2500T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
//前爪前放 K0002(12-18)
"{G0012#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1500T1000!}",
"{G0013#000P1520T1000!#001P1100T2000!#002P1850T1000!#003P2100T1000!#004P1500T1000!#005P1000T0000!}",
"{G0014#000P1520T1000!#001P1100T2000!#002P1900T1000!#003P2100T1000!#004P1500T1000!#005P2000T0000!}",
"{G0015#000P1520T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P2000T1000!}",
"{G0016#000P1520T1000!#001P1100T2000!#002P1800T1000!#003P2000T1000!#004P1500T1000!#005P2000T1000!}",
"{G0017#000P1520T1000!#001P1100T2000!#002P1800T1000!#003P2000T1000!#004P1500T1000!#005P1000T0000!}",
"{G0018#000P1500T1500!#001P2200T1500!#002P2500T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
//前爪左放 K0003(19-27)
"{G0019#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1500T1000!}",
"{G0020#000P1520T1000!#001P1100T2000!#002P1850T1000!#003P2100T1000!#004P1500T1000!#005P1000T0000!}",
"{G0021#000P1520T1000!#001P1100T2000!#002P1900T1000!#003P2100T1000!#004P1500T1000!#005P2000T0000!}",
"{G0022#000P1520T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P2000T1000!}",
"{G0023#000P2200T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P2000T1000!}",
"{G0024#000P2200T1000!#001P1200T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P2000T1000!}",
"{G0025#000P2200T1000!#001P1200T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1000T0000!}",
"{G0026#000P2200T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1000T1000!}",
"{G0027#000P1500T1500!#001P2200T1500!#002P2500T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
//前爪右放 K0004(28-36)
"{G0028#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1500T1000!}",
"{G0029#000P1520T1000!#001P1100T2000!#002P1850T1000!#003P2100T1000!#004P1500T1000!#005P1000T0000!}",
"{G0030#000P1520T1000!#001P1100T2000!#002P1900T1000!#003P2100T1000!#004P1500T1000!#005P2000T0000!}",
"{G0031#000P1520T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P2000T1000!}",
"{G0032#000P0800T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P2000T1000!}",
"{G0033#000P0800T1000!#001P1200T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P2000T1000!}",
"{G0034#000P0800T1000!#001P1200T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1000T0000!}",
"{G0035#000P0800T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1000T1000!}",
"{G0036#000P1500T1500!#001P2200T1500!#002P2500T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
};
/*
代码从main里开始执行
在进入大循环while(1)之前都为各个模块的初始化
最后在大循环处理持续执行的事情
另外注意uart中的串口中断，接收数据处理
timer中的定时器中断，舵机的脉冲收发就在那里
*/
void main(void) {
setup_global(); //初始化全局变量
setup_gpio(); //初始化IO口
setup_nled(); //初始化工作指示灯
setup_beep(); //初始化定时器
setup_djio(); //初始化舵机IO口
setup_sensor(); //初始化传感器IO口
setup_w25q64(); //初始化存储器W25Q64
setup_ps2(); //初始化PS2手柄
setup_vol(); //初始化电压采集
setup_car_pwm(); //初始化电机PWM定时
setup_uart1(); //初始化串口1
setup_uart2(); //初始化串口2
setup_uart4(); //初始化串口4
setup_systick(); //初始化滴答时钟，1S增加一次systick_ms的值
setup_others(); //初始化其他
setup_dj_timer(); //初始化定时器2 处理舵机PWM输出
setup_interrupt(); //初始化总中断
setup_start(); //初始化启动信号
while (1) {
loop_nled(); //循环执行工作指示灯，500ms跳动一次
loop_uart(); //串口数据接收处理
loop_action(); //动作组批量执行
loop_bt_once(); //蓝牙修改波特率和名称
loop_ps2_data(); //循环读取PS2手柄数据
loop_ps2_button(); //处理手柄上的按钮
loop_ps2_car_pwm(); //处理小车电机摇杆控制
loop_save_something(); //定时保存一些变量
// loop_smart_sensor(); //处理智能传感器功能
smart_xunji(); //循迹模式
// smart_soundjiaqu(); //声控夹取
// smart_ziyoubizhang(); //自由避障
// smart_yssbjiaqu(); //颜色识别
// smart_csbjiaqu(); //定距夹取
// smart_gensui(); //跟随功能
// smart_xunjibizhang(); //循迹避障
// smart_xunjiyanse(); //循迹识别
// smart_xunjicsbjiaqu(); //循迹定距
}
}
//--------------------------------------------------------------------------------
/*
初始化函数实现
*/
//初始化全局变量
void setup_global(void) {
//全局变量初始化
global_init();
}
//初始化IO口
void setup_gpio(void) {
//IO初始化
io_init();
}
//初始化工作指示灯初始化已在io_init中初始化
void setup_nled(void) {
nled_off(); //工作指示灯关闭
}
//初始化蜂鸣器初始化已在io_init中初始化
void setup_beep(void) {
beep_off(); //关闭蜂鸣器
}
//初始化舵机IO口
void setup_djio(void) {
dj_io_init(); //舵机IO口初始化
}
void setup_vol(void) {
adc_init(ADC_VOL);
}
//初始化存储器W25Q64
void setup_w25q64(void) {
//存储器初始化，读取ID进行校验，若错误则长鸣不往下执行
w25x_init();
while(w25x_readId()!= W25Q64)beep_on();
w25x_read((u8 *)(&eeprom_info), W25Q64_INFO_ADDR_SAVE_STR, sizeof(eeprom_info_t)); //读取全局变量
if(eeprom_info.version != VERSION) { //判断版本是否是当前版本
eeprom_info.version = VERSION; //复制当前版本
eeprom_info.dj_record_num = 0; //学习动作组变量赋值0
rewrite_eeprom(); //写入到存储器
}
if(eeprom_info.dj_bias_pwm[DJ_NUM] != FLAG_VERIFY) {
for(i=0;i<DJ_NUM;i++) {
eeprom_info.dj_bias_pwm[i] = 0;
}
eeprom_info.dj_bias_pwm[DJ_NUM] = FLAG_VERIFY;
}
}
//初始化PS2手柄
void setup_ps2(void) {
//手柄初始化
psx_init();
}
//初始化定时器2 处理舵机PWM输出
void setup_dj_timer(void) {
timer1_init(); //舵机定时器初始化
}
//初始化电机PWM定时
void setup_car_pwm(void) {
//小车 pwm 初始化
pwm_init(CYCLE);
car_pwm_set(0,0); //设置小车的左右轮速度为0
}
//初始化串口1
void setup_uart1(void) {
//串口1初始化
uart1_init(115200);
//uart1_close();
uart1_open();
//串口发送测试字符
uart1_send_str((u8 *)"uart1 check ok!");
}
//初始化串口2
void setup_uart2(void) {
//串口2初始化
uart2_init(115200);
//uart2_close();
uart2_open();
//串口发送测试字符
uart2_send_str((u8 *)"uart2 check ok!");
}
//初始化串口4
void setup_uart4(void) {
//串口4初始化
uart4_init(115200);
//uart4_close();
uart4_open();
//串口发送测试字符
uart4_send_str((u8 *)"uart4 check ok!");
}
//初始化滴答时钟，1S增加一次systick_ms的值
void setup_systick(void) {
//系统滴答时钟初始化
timer0_init();
}
//初始化启动信号
void setup_start(void) {
//蜂鸣器LED 名叫闪烁示意系统启动
beep_on();nled_on();mdelay(100);beep_off();nled_off();mdelay(100);
beep_on();nled_on();mdelay(100);beep_off();nled_off();mdelay(100);
beep_on();nled_on();mdelay(100);beep_off();nled_off();mdelay(100);
}
//初始化其他
void setup_others(void) {
//机械臂蜷缩 G0002组
memset(uart_receive_buf, 0, sizeof(uart_receive_buf));
if(ACTION_USE_ROM) {
//使用存储在单片机内部rom中的动作组
strcpy((char *)uart_receive_buf, action_pre_group[2]);
} else {
//从存储芯片中读取第group_num个动作组
w25x_read(uart_receive_buf, 2*ACTION_SIZE, ACTION_SIZE);
}
//把读取出来的动作组传递到do_action执行
for(i=16;i<strlen((char *)uart_receive_buf);i+=15) {
uart_receive_buf[i] = '0';
uart_receive_buf[i+1] = '0';
uart_receive_buf[i+2] = '0';
uart_receive_buf[i+3] = '0';
}
do_action(uart_receive_buf);
//执行预存命令 {G0000#000P1500T1000!#000P1500T1000!}
if(eeprom_info.pre_cmd[PRE_CMD_SIZE] == FLAG_VERIFY) {
strcpy((char *)uart_receive_buf, (char *)eeprom_info.pre_cmd);
if(eeprom_info.pre_cmd[0] == '
) {
parse_cmd(eeprom_info.pre_cmd);
} else {
for(i=16;i<strlen((char *)uart_receive_buf);i+=15) {
uart_receive_buf[i] = '0';
uart_receive_buf[i+1] = '0';
uart_receive_buf[i+2] = '0';
uart_receive_buf[i+3] = '0';
}
do_action(uart_receive_buf);
}
}
}
//初始化总中断
void setup_interrupt(void) {
//串口1设为高优先级
IP = 0X10;
//IP2 = 0X01;
//总中断打开
EA = 1;
}
//--------------------------------------------------------------------------------
//--------------------------------------------------------------------------------
/*
主循环函数实现
*/
//循环执行工作指示灯，500ms跳动一次
void loop_nled(void) {
static u32 systick_ms_bak = 0;
if(millis() - systick_ms_bak >= 500) {
systick_ms_bak = millis();
nled_switch();
}
}
//串口数据接收处理
void loop_uart(void) {
static u8 do_once1 = 0, do_once2 = 0;
if(uart1_get_ok) {
//测试发回去
//uart1_send_str(uart_receive_buf);
if(uart1_mode == 1) { //命令模式
//uart1_send_str(">cmd");
parse_cmd(uart_receive_buf);
} else if(uart1_mode == 2) { //单个舵机模式
//uart1_send_str(">sig");
do_action(uart_receive_buf);
} else if(uart1_mode == 3) { //多个舵机模式
//uart1_send_str(">group:");
do_action(uart_receive_buf);
} else if(uart1_mode == 4) { //保存模式
//uart1_send_str(">save");
//uart1_send_str(uart_receive_buf);
action_save(uart_receive_buf);
}
uart1_mode = 0;
uart1_get_ok = 0;
//uart1_open();
}
if(millis() - get_uart_timeout() > 100) {
if(!do_once1) {
timer1_open();
do_once1 = 1;
do_once2 = 0;
}
} else {
if(!do_once2) {
timer1_close();
do_once1 = 0;
do_once2 = 1;
}
}
return;
}
//定时保存一些变量
void loop_save_something(void) {
static u32 saveTime = 3000;
if((djBiasSaveFlag == 1) && (millis() - bias_systick_ms_bak > saveTime)) {
djBiasSaveFlag = 0;
bias_systick_ms_bak = millis();
rewrite_eeprom();
}
return;
}
void loop_ps2_data(void) {
static u32 systick_ms_bak = 0;
if(millis() - systick_ms_bak < 20) {
return;
}
systick_ms_bak = millis();
psx_write_read(psx_buf);
//#if 0
// sprintf(cmd_return, "0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x\r\n",
// (int)psx_buf[0], (int)psx_buf[1], (int)psx_buf[2], (int)psx_buf[3],
// (int)psx_buf[4], (int)psx_buf[5], (int)psx_buf[6], (int)psx_buf[7], (int)psx_buf[8]);
// uart1_send_str(cmd_return);
//#endif
return;
}
//处理遥感数据，控制电机
void loop_ps2_car_pwm(void) {
static int car_left_bak=0, car_right_bak=0;
int car_left, car_right;
if(psx_buf[1] != PS2_LED_RED)return;
car_left = (127 - psx_buf[8]) * 8;
car_right = (127 - psx_buf[6]) * 8;
if(car_left != car_left_bak || car_right != car_right_bak) {
car_pwm_set(car_left, car_right);
car_left_bak = car_left;
car_right_bak = car_right;
}
}
//接收手柄按键数据
void loop_ps2_button(void) {
static unsigned char psx_button_bak[2] = {0};
static unsigned char mode_bak;
//处理智能模式红灯模式下智能模式取消，此时为遥控模式
if(mode_bak != psx_buf[1]) {
mode_bak = psx_buf[1];
if(PS2_LED_RED == psx_buf[1]) {
smart_mode = 255;
}
car_pwm_set(0,0);
group_do_ok = 1;
//beep_on_times(1, 500);
}
if((psx_button_bak[0] == psx_buf[3])
&& (psx_button_bak[1] == psx_buf[4])) {
} else {
parse_psx_buf(psx_buf+3, psx_buf[1]);
psx_button_bak[0] = psx_buf[3];
psx_button_bak[1] = psx_buf[4];
}
return;
}
//处理手柄接收到的数据
void parse_psx_buf(unsigned char *buf, unsigned char mode) {
u8 i, pos = 0;
static u16 bak=0xffff, temp, temp2;
temp = (buf[0]<<8) + buf[1];
if(bak != temp) {
temp2 = temp;
temp &= bak;
for(i=0;i<16;i++) {
if((1<<i) & temp) {
} else {
if((1<<i) & bak) { //press
memset(uart_receive_buf, 0, sizeof(uart_receive_buf));
if(mode == PS2_LED_RED) {
memcpy((char *)uart_receive_buf, (char *)pre_cmd_set_red[i], strlen(pre_cmd_set_red[i]));
} else if(mode == PS2_LED_GRN) {
memcpy((char *)uart_receive_buf, (char *)pre_cmd_set_grn[i], strlen(pre_cmd_set_grn[i]));
} else continue;
pos = str_contain_str(uart_receive_buf, "^");
if(pos) uart_receive_buf[pos-1] = '\0';
if(str_contain_str(uart_receive_buf, "[ DISCUZ_CODE_0 ]quot;)) {
//uart1_close();
//uart1_get_ok = 1;
//uart1_mode = 1;
strcpy(cmd_return, uart_receive_buf+11);
strcpy(uart_receive_buf, cmd_return);
parse_cmd(uart_receive_buf);
} else if(str_contain_str(uart_receive_buf, "#")) {
//uart1_close();
//uart1_get_ok = 1;
//uart1_mode = 2;
strcpy(cmd_return, uart_receive_buf+11);
strcpy(uart_receive_buf, cmd_return);
do_action(uart_receive_buf);
}
//uart1_send_str(uart_receive_buf);
//zx_uart_send_str(uart_receive_buf);
bak = 0xffff;
} else {//release
memset(uart_receive_buf, 0, sizeof(uart_receive_buf));
if(mode == PS2_LED_RED) {
memcpy((char *)uart_receive_buf, (char *)pre_cmd_set_red[i], strlen(pre_cmd_set_red[i]));
} else if(mode == PS2_LED_GRN) {
memcpy((char *)uart_receive_buf, (char *)pre_cmd_set_grn[i], strlen(pre_cmd_set_grn[i]));
} else continue;
pos = str_contain_str(uart_receive_buf, "^");
if(pos) {
if(str_contain_str(uart_receive_buf+pos, "[ DISCUZ_CODE_0 ]quot;)) {
//uart1_close();
//uart1_get_ok = 1;
//uart1_mode = 1;
strcpy(cmd_return, uart_receive_buf+pos);
cmd_return[strlen(cmd_return) - 1] = '\0';
strcpy(uart_receive_buf, cmd_return);
parse_cmd(uart_receive_buf);
} else if(str_contain_str(uart_receive_buf+pos, "#")) {
//uart1_close();
//uart1_get_ok = 1;
//uart1_mode = 2;
strcpy(cmd_return, uart_receive_buf+pos);
cmd_return[strlen(cmd_return) - 1] = '\0';
strcpy(uart_receive_buf, cmd_return);
do_action(uart_receive_buf);
}
//uart1_send_str(uart_receive_buf);
//zx_uart_send_str(uart_receive_buf);
}
}
//测试执行指令
//uart1_send_str(uart_receive_buf);
}
}
bak = temp2;
beep_on();mdelay(50);beep_off();
}
return;
}
//void handle_uart(void) {
// return;
//}
/*
parse_cmd( )的作用是解析下列指令
$DST!
$DST:x!
$RST!
$CGP:%d-%d!
$DEG:%d-%d!
$DGS:x!
$DGT:%d-%d,%d!
$DCR:%d,%d!
$DWA!
$DWD!
$DJR!
$GETA!
*/
void parse_cmd(u8 *cmd) {
//u32 uint1;
static u8 djrFlag=0;
u16 pos, i, index;
int int1, int2;
uart1_send_str(cmd);
if(pos = str_contain_str(cmd, "$DST!"), pos) {
dbt_flag = 0;
group_do_ok = 1;
for(i=0;i<DJ_NUM;i++) {
duoji_doing[i].inc = 0;
duoji_doing[i].aim = duoji_doing[i].cur;
}
zx_uart_send_str("#255PDST!");
} else if(pos = str_contain_str(cmd, "$DST:"), pos) {
if(sscanf(cmd, "$DST:%d!", &index)) {
duoji_doing[index].inc = 0;
duoji_doing[index].aim = duoji_doing[index].cur;
sprintf(cmd_return, "#%03dPDST!", (int)index);
zx_uart_send_str(cmd_return);
}
} else if(pos = str_contain_str(cmd, "$RST!"), pos) {
soft_reset();
} else if(pos = str_contain_str(cmd, "$CGP:"), pos) {
if(sscanf(cmd, "$CGP:%d-%d!", &int1, &int2)) {
print_group(int1, int2);
}
} else if(pos = str_contain_str(cmd, "$DEG:"), pos) {
if(sscanf(cmd, "$DEG:%d-%d!", &int1, &int2)) {
erase_sector(int1, int2);
}
} else if(pos = str_contain_str(cmd, "$DGS:"), pos) {
if(sscanf(cmd, "$DGS:%d!", &int1)) {
do_group_once(int1);
group_do_ok = 1;
}
} else if(pos = str_contain_str(cmd, "$DGT:"), pos) {
if(sscanf((char *)cmd, "$DGT:%d-%d,%d!", &group_num_start, &group_num_end, &group_num_times)) {
//uart1_send_str("111111");
if(group_num_start != group_num_end) {
do_start_index = group_num_start;
do_time = group_num_times;
group_do_ok = 0;
//uart1_send_str("22222");
} else {
group_do_ok = 1;
do_group_once(group_num_start);
//uart1_send_str("33333");
}
}
} else if(pos = str_contain_str(cmd, "$DCR:"), pos) {
if(sscanf(cmd, "$DCR:%d,%d!", &int1, &int2)) {
car_pwm_set(int1, int2);
}
} else if(pos = str_contain_str(cmd, "$DWA!"), pos) {
car_dw--;
if(car_dw == 0)car_dw = 1;
beep_on();mdelay(100);beep_off();
} else if(pos = str_contain_str(cmd, "$DWD!"), pos) {
car_dw++;
if(car_dw == 4)car_dw = 3;
beep_on();mdelay(100);beep_off();
} else if(pos = str_contain_str(cmd, "$CAR_FARWARD!"), pos) {
car_pwm_set(1000, 1000);
} else if(pos = str_contain_str(cmd, "$CAR_BACKWARD!"), pos) {
car_pwm_set(-1000, -1000);
} else if(pos = str_contain_str(cmd, "$CAR_LEFT!"), pos) {
car_pwm_set(-1000, 1000);
} else if(pos = str_contain_str(cmd, "$CAR_RIGHT!"), pos) {
car_pwm_set(1000, -1000);
} else if(pos = str_contain_str(cmd, "$DJR!"), pos) {
zx_uart_send_str("#255P1500T2000!");
for(i=0;i<DJ_NUM;i++) {
duoji_doing[i].aim = 1500;
duoji_doing[i].time = 2000;
duoji_doing[i].inc = (duoji_doing[i].aim - duoji_doing[i].cur) / (duoji_doing[i].time/20.000);
}
if(djrFlag) {
do_group_once(1);
} else {
do_group_once(2);
}
djrFlag = !djrFlag;
} else if(pos = str_contain_str(cmd, "$GETA!"), pos) {
uart1_send_str("AAA");
} else if(pos = str_contain_str(cmd, "$GETS!"), pos) {
if(group_do_ok == 0) {
uart1_send_str("group_do_ok=0");
} else {
uart1_send_str("group_do_ok=1");
}
} else if(pos = str_contain_str(cmd, "$GETINC!"), pos) {
for(i=0;i<8;i++) {
sprintf(cmd_return, "inc%d = %f \r\n", (int)i, duoji_doing[i].inc);
uart1_send_str(cmd_return);
}
}else if(pos = str_contain_str(uart_receive_buf, "$DBT:"), pos) {
if(sscanf(uart_receive_buf, "$DBT:%d,%d!", &int1, &int2)) {
if(int1 == 1) {
group_num_start = 1;
group_num_end = 10;
group_num_times = int2;
} else if(int1 == 2) {
group_num_start = 11;
group_num_end = 20;
group_num_times = int2;
} else if(int1 == 3) {
group_num_start = 21;
group_num_end = 30;
group_num_times = int2;
} else if(int1 == 4) {
group_num_start = 31;
group_num_end = 40;
group_num_times = int2;
} else {
group_num_start = 0;
group_num_end = 0;
}
if(group_num_start != group_num_end) {
do_start_index = group_num_start;
do_time = group_num_times;
group_do_ok = 0;
dbt_flag = 1;
} else {
do_group_once(group_num_start);
}
}
} else if(pos = str_contain_str(cmd, "$DRS!"), pos) {
uart1_send_str("51MCU-IAP15W4K61S4");
} else if(pos = str_contain_str(cmd, (u8 *)"$SMODE"), pos) {
if(sscanf((char *)cmd, "$SMODE%d!", &int1)) {
if(int1 < 10) {
smart_mode = int1;
beep_on_times(1, 100);
car_pwm_set(0,0);
//uart1_send_str(cmd);
}
}
}
}
//预存指令处理
void action_save(u8 *str) {
int action_index = 0;
//预存命令处理
if(str[1] == '
&& str[2] == '!') {
eeprom_info.pre_cmd[PRE_CMD_SIZE] = 0;
rewrite_eeprom();
uart1_send_str((u8 *)"@CLEAR PRE_CMD OK!");
return;
} else if(str[1] == '
) {
if(sscanf((char *)str, "<$DGT:%d-%d,%d!>", &group_num_start, &group_num_end, &group_num_times)) {
if(group_num_start == group_num_end) {
w25x_read(eeprom_info.pre_cmd, group_num_start*ACTION_SIZE, ACTION_SIZE);
} else {
memset(eeprom_info.pre_cmd, 0, sizeof(eeprom_info.pre_cmd));
strcpy((char *)eeprom_info.pre_cmd, (char *)str+1);
eeprom_info.pre_cmd[strlen((char *)str) - 2] = '\0';
}
eeprom_info.pre_cmd[PRE_CMD_SIZE] = FLAG_VERIFY;
rewrite_eeprom();
//uart1_send_str(eeprom_info.pre_cmd);
uart1_send_str((u8 *)"@SET PRE_CMD OK!");
}
return;
}
action_index = get_action_index(str);
//<G0001#001...>
if((action_index == -1) || str[6] != '#'){
//if( action_index == -1 ){
uart1_send_str("E");
return;
}
//save_action_index_bak++;
if(action_index*ACTION_SIZE % W25Q64_SECTOR_SIZE == 0)w25x_erase_sector(action_index*ACTION_SIZE/W25Q64_SECTOR_SIZE);
replace_char(str, '<', '{');
replace_char(str, '>', '}');
w25x_write(str, action_index*ACTION_SIZE, strlen(str) + 1);
//uart1_send_str(str);
uart1_send_str("A");
return;
}
//获取指令中舵机ID号
int get_action_index(u8 *str) {
int index = 0;
//uart_send_str(str);
while(*str) {
if(*str == 'G') {
str++;
while((*str != '#') && (*str != '
)) {
index = index*10 + *str-'0';
str++;
}
return index;
} else {
str++;
}
}
return -1;
}
//打印动作组到串口
void print_group(int start, int end) {
if(start > end) {
int_exchange(&start, &end);
}
for(;start<=end;start++) {
memset(uart_receive_buf, 0, sizeof(uart_receive_buf));
w25x_read(uart_receive_buf, start*ACTION_SIZE, ACTION_SIZE);
uart1_send_str(uart_receive_buf);
uart1_send_str("\r\n");
}
}
//交换两个变量
void int_exchange(int *int1, int *int2) {
int int_temp;
int_temp = *int1;
*int1 = *int2;
*int2 = int_temp;
}
//擦除相应扇区
void erase_sector(int start, int end) {
if(start > end) {
int_exchange(&start, &end);
}
if(end >= 127)end = 127;
for(;start<=end;start++) {
SpiFlashEraseSector(start);
sprintf(cmd_return, "@Erase %d OK!", start);
uart1_send_str(cmd_return);
}
save_action_index_bak = 0;
}
//执行动作一次
void do_group_once(int group_num) {
memset(uart_receive_buf, 0, sizeof(uart_receive_buf));
if(ACTION_USE_ROM) {
//使用存储在单片机内部rom中的动作组
strcpy((char *)uart_receive_buf, action_pre_group[group_num]);
} else {
//从存储芯片中读取第group_num个动作组
w25x_read(uart_receive_buf, group_num*ACTION_SIZE, ACTION_SIZE);
}
//把读取出来的动作组传递到do_action执行
do_action(uart_receive_buf);
sprintf((char *)cmd_return, "@DoGroup %d OK!\r\n", group_num);
uart1_send_str(cmd_return);
}
//取绝对值
float abs_float(float value) {
if(value>0) {
return value;
}
return (-value);
}
//舵机增量处理
void duoji_inc_handle(u8 index) {
int aim_temp;
if(duoji_doing[index].inc != 0) {
aim_temp = duoji_doing[index].aim;
if(aim_temp > 2500){
aim_temp = 2500;
} else if(aim_temp < 500) {
aim_temp = 500;
}
if(abs_float(aim_temp - duoji_doing[index].cur) <= abs_float(duoji_doing[index].inc + duoji_doing[index].inc)) {
duoji_doing[index].cur = aim_temp;
duoji_doing[index].inc = 0;
} else {
duoji_doing[index].cur += duoji_doing[index].inc;
}
}
}
//动作组批量执行
void loop_action(void) {
if(timer1_flag_dj) {
duoji_inc_handle(duoji_index1);
timer1_flag_dj = 0;
}
if((check_dj_state() == 0) && (group_do_ok == 0)) {
do_group_once(do_start_index);
if(group_num_start<group_num_end) {
if(do_start_index == group_num_end) {
do_start_index = group_num_start;
if(group_num_times != 0) {
do_time--;
if(do_time == 0) {
group_do_ok = 1;
uart1_send_str((u8*)"@GroupDone!");
}
}
return;
}
do_start_index++;
} else {
if(do_start_index == group_num_end) {
do_start_index = group_num_start;
if(group_num_times != 0) {
do_time--;
if(do_time == 0) {
group_do_ok = 1;
uart1_send_str((u8*)"@GroupDone!");
}
}
return;
}
do_start_index--;
}
}
}
//舵机状态处理，有舵机处理动作返回1，否则返回0
u8 check_dj_state(void) {
int i;
for(i=0;i<DJ_NUM;i++) {
if(duoji_doing[i].inc) return 1;
}
return 0;
}
//单个动作处理
void do_action(u8 *uart_receive_buf) {
u16 index, time,i, lst_i, parse_ok;
int bias;
float pwm;
float aim_temp;
zx_uart_send_str(uart_receive_buf);
zx_uart_send_str("\r\n");
if(uart_receive_buf[0] == '#' && uart_receive_buf[4] == 'P' && uart_receive_buf[5] == 'S' && uart_receive_buf[6] == 'C' && uart_receive_buf[7] == 'K' && uart_receive_buf[12] == '!') {
index = (uart_receive_buf[1] - '0')*100 + (uart_receive_buf[2] - '0')*10 + (uart_receive_buf[3] - '0');
bias = (uart_receive_buf[9] - '0')*100 + (uart_receive_buf[10] - '0')*10 + (uart_receive_buf[11] - '0');
if((bias >= -500) && (bias <= 500) && (index < DJ_NUM)) {
if(uart_receive_buf[8] == '+') {
} else if(uart_receive_buf[8] == '-') {
bias = -bias;
}
aim_temp = duoji_doing[index].cur + 0.043198 - eeprom_info.dj_bias_pwm[index] + bias;
eeprom_info.dj_bias_pwm[index] = bias;
if(aim_temp > 2497){
aim_temp = 2497;
} else if(aim_temp < 500) {
aim_temp = 500;
}
duoji_doing[index].aim = aim_temp;
duoji_doing[index].cur = aim_temp;
duoji_doing[index].inc = 0;
bias_systick_ms_bak = millis();
djBiasSaveFlag = 1;
}
return;
}
i = 0;parse_ok = 0;
while(uart_receive_buf[i]) {
if(uart_receive_buf[i] == '#') {
lst_i = i;
index = 0;i++;
while(uart_receive_buf[i] && (uart_receive_buf[i] != 'P')) {
index = index*10 + uart_receive_buf[i]-'0';i++;
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码