手柄序號分布.png (96.05 KB, 下載次數: 21)
下載附件
2018-8-29 03:16 上傳
0.png (11.2 KB, 下載次數: 25)
下載附件
2018-8-29 03:16 上傳
單片機源程序如下:
- /*
- 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黑下載附件…………
復制代碼
所有資料51hei提供下載:
01 循跡模式.rar
(357.14 KB, 下載次數: 66)
2018-8-29 03:18 上傳
點擊文件名下載附件
尋跡車 下載積分: 黑幣 -5
|