#include <stdio.h>
#include <string.h>
#include <Arduino.h>
#include <Wire.h> // 包含Wire(I2C)通訊庫 Include Wire library
#define u8 uint8_t
#define PIN_xj0 A5
#define PIN_xj1 A4
#define PIN_trig A7
#define PIN_echo A6
#include <PS2X_lib.h>
#define KEY1_PIN A1
#include <Servo.h> //聲明調用Servo.h庫
#include <winbondflash.h> //flash調用的庫
#define ACTION_SIZE 512
#define RECV_SIZE 168
#define INFO_ADDR_SAVE_STR (((8<<10)-4)<<10) //(8*1024-4)*1024 //eeprom_info結構體存儲的位置
#define BIAS_ADDR_VERIFY 0 //偏差存儲的地址
#define FLAG_VERIFY 0x38
#define PIN_nled 13 //宏定義工作指示燈引腳
#define PIN_beep 4 //蜂鳴器引腳定義
#define nled_on() {digitalWrite(PIN_nled, LOW);}
#define nled_off() {digitalWrite(PIN_nled, HIGH);}
#define beep_on() {digitalWrite(PIN_beep, HIGH);}
#define beep_off() {digitalWrite(PIN_beep, LOW);}
#define PRE_CMD_SIZE 32
#define SERVO_NUM 6
#define SERVO_TIME_PERIOD 20 //每隔20ms處理一次（累加）舵機的PWM增量
#define Speed_Line (30) //巡線的速度
#define KPx (15) //
#define KIx (0.0001) //
#define KDx (1) //
#define Motor_L1_PIN 6 // 控制小車左前方電機前進 Control the motor on the left front of the car
#define Motor_L2_PIN 8 // 控制小車左后方電機前進 Control car left rear motor forward
#define Motor_R1_PIN 7 // 控制小車右前方電機前進 Control the right front motor of the car to move forward
#define Motor_R2_PIN 9 // 控制小車右后方電機前進 Control car right rear motor forward
#define PWM_FREQUENCY 50
#define Ignore_speed (0) //電機最小起轉
#define MAX_Speed (255) //最大速度設置
/*******按鍵管腳映射表*******/
#define KEY1_PIN A1
#define KEY2_PIN A2
bool in_zone=false;
enum OmniDirectionalCar {
STOP,
FORWARD,
BACKWARD,
LEFT,
RIGHT,
LEFT_ROTATE,
RIGHT_ROTATE,
LEFT_FORWARD,
RIGHT_BACKWARD,
RIGHT_FORWARD,
LEFT_BACKWARD,
};
typedef struct {
long myversion;
long dj_record_num;
byte pre_cmd[PRE_CMD_SIZE+1];
int dj_bias_pwm[SERVO_NUM+1];
}eeprom_info_t;
eeprom_info_t eeprom_info;
Servo myservo[SERVO_NUM]; //創建一個舵機類
typedef struct { //舵機結構體變量聲明
unsigned int aim = 1500; //舵機目標值
float cur = 1500.0; //舵機當前值
unsigned int time1 = 1000; //舵機執行時間
float inc= 0.0; //舵機值增量，以20ms為周期
}duoji_struct;
duoji_struct servo_do[SERVO_NUM]; //用結構體變量聲明一個舵機變量組
winbondFlashSPI mem;
String uart_receive_str = ""; //聲明一個字符串數組
String uart_receive_str_bak = ""; //聲明一個字符串數組
byte uart_receive_buf[RECV_SIZE];
byte uart1_get_ok = 0, uart1_mode=0;
char cmd_return[64];
int uart_receive_str_len;
int zx_read_id = 0, zx_read_flag = 0, zx_read_value = 0;
byte flag_sync=0;
bool downLoad = false;
byte servo_pin[SERVO_NUM] = {7, 3, 5, 6, 9 ,8}; //宏定義舵機控制引腳
u32 downLoadSystickMs=0;
char buffer[RECV_SIZE]; // 定義一個數組用來存儲每小組動作組
int do_start_index, do_time, group_num_start, group_num_end, group_num_times;
char group_do_ok = 1;
long long action_time = 0;
void(* resetFunc) (void) = 0; //declare reset function at address 0
const int Press_KEY_1 = 0;
const int Release_KEY_1 = 1;
const int Press_KEY_2 = 0;
const int Release_KEY_2 = 1;
extern uint8_t x1,x2,x3,x4,x5,x6,x7,x8;
volatile long systick_ms;
volatile long nextTime;
volatile long distance;
volatile int dis_hold;
volatile boolean debug_mode;
float KP_x,KI_x,KD_x; //x軸方向PID
int error_x,error_last_x,integral_x;
int err;//紅外檢測線位置上的錯誤
//電機控制函數
void car_run(short speed_left, short speed_right) {
sprintf(cmd_return, "#006P%04dT0000!#007P%04dT0000!", 1500+speed_left, 1500-speed_right);
Serial.println((char *)cmd_return);
sprintf(cmd_return, "#008P%04dT0000!#009P%04dT0000!", 1500+speed_left, 1500-speed_right);
Serial.println((char *)cmd_return);
sprintf(cmd_return, "#000P%04dT0000!", 1500-(speed_left-speed_right)/2);
Serial.println((char *)cmd_return);
}
//led燈初始化
void setup_nled() {
pinMode(PIN_nled,OUTPUT); //設置引腳為輸出模式
nled_off();
}
//蜂鳴器初始化
void setup_beep() {
pinMode(PIN_beep,OUTPUT);
beep_off();
}
//六路舵機初始化
void setup_servo() {
//蜷縮 "{G0002#000P1500T1500!#001P2200T1500!#002P2500T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
if(eeprom_info.pre_cmd[PRE_CMD_SIZE] != FLAG_VERIFY) {
servo_do[0].aim = servo_do[0].cur = 1500 + eeprom_info.dj_bias_pwm[0];servo_do[0].inc=0;
servo_do[1].aim = servo_do[1].cur = 1500 + eeprom_info.dj_bias_pwm[1];servo_do[1].inc=0;
servo_do[2].aim = servo_do[2].cur = 1500 + eeprom_info.dj_bias_pwm[2];servo_do[2].inc=0;
servo_do[3].aim = servo_do[3].cur = 1500 + eeprom_info.dj_bias_pwm[3];servo_do[3].inc=0;
servo_do[4].aim = servo_do[4].cur = 1500 + eeprom_info.dj_bias_pwm[4];servo_do[4].inc=0;
servo_do[5].aim = servo_do[5].cur = 1500 + eeprom_info.dj_bias_pwm[5];servo_do[5].inc=0;
}
for(byte i = 0; i < SERVO_NUM; i ++){
myservo.attach(servo_pin); // 將10引腳與聲明的舵機對象連接起來
myservo.writeMicroseconds(servo_do.aim);
}
}
//啟動提示
void setup_start_pre_cmd() {
if(eeprom_info.pre_cmd[PRE_CMD_SIZE] == FLAG_VERIFY) {
parse_cmd(eeprom_info.pre_cmd);
}
}
//功能介紹：LED燈閃爍，每秒閃爍一次
void loop_nled() {
static u8 val = 0;
static unsigned long systick_ms_bak = 0;
if(millis() - systick_ms_bak > 500) {
systick_ms_bak = millis();
if(val) {
nled_on();
} else {
nled_off();
}
val = !val;
}
}
//舵機PWM增量處理函數，每隔SERVO_TIME_PERIOD毫秒處理一次，這樣就實現了舵機的連續控制
void loop_servo() {
static byte servo_monitor[SERVO_NUM] = {0};
static long long systick_ms_bak = 0;
if(millis() - systick_ms_bak > SERVO_TIME_PERIOD) {
systick_ms_bak = millis();
for(byte i=0; i<SERVO_NUM; i++) {
if(servo_do.inc) {
if(abs( servo_do.aim - servo_do.cur) <= abs (servo_do.inc) ) {
myservo.writeMicroseconds(servo_do.aim);
servo_do.cur = servo_do.aim;
servo_do.inc = 0;
} else {
servo_do.cur += servo_do.inc;
myservo.writeMicroseconds((int)servo_do.cur);
}
} else {
}
}
}
}
//解析串口接收到的字符串指令
void loop_uart(){
if(uart1_get_ok) {
//如果有同步標志直接返回，讓同步函數處理
if(flag_sync)return;
//打印字符串，測試的時候可以用
//轉換成字符串數組
uart_receive_str.toCharArray(uart_receive_buf, uart_receive_str.length()+1);
uart_receive_str_len = uart_receive_str.length();
if(uart1_mode == 1) {
parse_cmd(uart_receive_buf);
} else if(uart1_mode == 2 || uart1_mode == 3){
parse_action(uart_receive_buf);
} else if(uart1_mode == 4){
save_action(uart_receive_buf);
}
uart1_get_ok = 0;
uart1_mode = 0;
uart_receive_str = "";
}
if(millis()-downLoadSystickMs>3000) {
downLoad = false;
}
}
//對 a 數進行排序
void selection_sort(int *a, int len) {
int i,j,mi,t;
for(i=0;i<len-1;i++) {
mi = i;
for(j=i+1;j<len;j++) {
if(a[mi] > a[j]) {
mi = j;
}
}
if(mi != i) {
t = a[mi];
a[mi] = a;
a = t;
}
}
}
//把eeprom_info寫入到W25Q64_INFO_ADDR_SAVE_STR位置
void read_eeprom(void) {
mem.begin(_W25Q64,SPI,SS);
mem.read( INFO_ADDR_SAVE_STR, (char *)(&eeprom_info), sizeof(eeprom_info_t));
mem.end();
}
//把eeprom_info寫入到INFO_ADDR_SAVE_STR位置
void rewrite_eeprom(void) {
mem.begin(_W25Q64,SPI,SS);
mem.eraseSector(INFO_ADDR_SAVE_STR);
mem.write( INFO_ADDR_SAVE_STR, (char *)(&eeprom_info), sizeof(eeprom_info_t));
mem.end();
}
//存儲器初始化
void setup_w25q() {
read_eeprom();
if(eeprom_info.dj_bias_pwm[SERVO_NUM] != FLAG_VERIFY) {
for(int i=0;i<SERVO_NUM; i++) {
eeprom_info.dj_bias_pwm = 0;
}
eeprom_info.dj_bias_pwm[SERVO_NUM] = FLAG_VERIFY;
}
}
//獲取最大時間
int getMaxTime(char *str) {
int i = 0, max_time = 0, tmp_time = 0;
while(str) {
if(str == 'T') {
tmp_time = (str[i+1]-'0')*1000 + (str[i+2]-'0')*100 + (str[i+3]-'0')*10 + (str[i+4]-'0');
if(tmp_time>max_time)max_time = tmp_time;
i = i+4;
continue;
}
i++;
}
return max_time;
}
//存儲動作組
void save_action(char *str) {
long long action_index = 0, max_time;
//預存命令處理
if(str[1] == '$' && str[2] == '!') {
eeprom_info.pre_cmd[PRE_CMD_SIZE] = 0;
rewrite_eeprom();
Serial.println("@CLEAR PRE_CMD OK!");
return;
} else if(str[1] == '$') {
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();
Serial.println("@SET PRE_CMD OK!");
Serial.println((char *)eeprom_info.pre_cmd);
return;
}
//<G0000#000P1500T1000!>
if((str[1] == 'G') && (str[6] == '#')) {
action_index = (str[2]-'0')*1000 + (str[3]-'0')*100 + (str[4]-'0')*10 + (str[5]-'0') ;
if(action_index<10000) {
str[0] = '{';
str[uart_receive_str_len-1] = '}';
max_time = getMaxTime(str);
uart_receive_str_len = uart_receive_str_len+4;
str[uart_receive_str_len-4] = max_time/1000 + '0';
str[uart_receive_str_len-3] = max_time%1000/100 + '0';
str[uart_receive_str_len-2] = max_time%100/10 + '0';
str[uart_receive_str_len-1] = max_time%10 + '0';
str[uart_receive_str_len] = '\0';
mem.begin(_W25Q64,SPI,SS);
if((action_index*ACTION_SIZE % 4096) == 0){
mem.eraseSector(action_index*ACTION_SIZE); delay(5);
}
if(uart_receive_str_len<256) {
mem.write((long long)action_index*ACTION_SIZE, str, uart_receive_str_len);
} else {
mem.write((long long)action_index*ACTION_SIZE, str, 256);
delay(5);
mem.write((long long)action_index*ACTION_SIZE+256, str+256, uart_receive_str_len-256);
}
mem.end();
//返回發送接收成功
delay(30);
Serial.println("A");
}
}
}
//解析串口動作
void parse_action(u8 *uart_receive_buf) {
static unsigned int index, time1, pwm1, pwm2, i, len;//聲明三個變量分別用來存儲解析后的舵機序號，舵機執行時間，舵機PWM
if((uart_receive_buf[0] == '#') && (uart_receive_buf[4] == 'P') && (uart_receive_buf[5] == '!')) {
delay(500);
}
Serial.println((char *)uart_receive_buf);
if(zx_read_flag) {
//#001P1500! 回讀處理
if((uart_receive_buf[0] == '#') && (uart_receive_buf[4] == 'P') && (uart_receive_buf[9] == '!')) {
index = (uart_receive_buf[1]-'0')*100 + (uart_receive_buf[2]-'0')*10 + (uart_receive_buf[3]-'0');
if(index == zx_read_id) {
zx_read_flag = 0;
zx_read_value = (uart_receive_buf[5]-'0')*1000 + (uart_receive_buf[6]-'0')*100 + (uart_receive_buf[7]-'0')*10 + (uart_receive_buf[8]-'0');
}
}
//#001PSCK+100! 偏差處理
} else 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')) {
index = (uart_receive_buf[1]-'0')*100 + (uart_receive_buf[2]-'0')*10 + (uart_receive_buf[3]-'0');
if(index < SERVO_NUM) {
int bias_tmp = (uart_receive_buf[9]-'0')*100 + (uart_receive_buf[10]-'0')*10 + (uart_receive_buf[11]-'0');
if(bias_tmp < 127) {
myservo[index].attach(servo_pin[index]);
if(uart_receive_buf[8] == '+') {
servo_do[index].cur = servo_do[index].cur-eeprom_info.dj_bias_pwm[index]+bias_tmp;
eeprom_info.dj_bias_pwm[index] = bias_tmp;
} else if(uart_receive_buf[8] == '-') {
servo_do[index].cur = servo_do[index].cur-eeprom_info.dj_bias_pwm[index]-bias_tmp;
eeprom_info.dj_bias_pwm[index] = -bias_tmp;
}
rewrite_eeprom();
servo_do[index].cur = 1500;
servo_do[index].aim = 1500+eeprom_info.dj_bias_pwm[index]; //舵機PWM賦值,加上偏差的值
servo_do[index].time1 = 100; //舵機執行時間賦值
servo_do[index].inc = eeprom_info.dj_bias_pwm[index]/5.000; //根據時間計算舵機PWM增量
//Serial.print("input bias:");
//Serial.println(eeprom_info.dj_bias_pwm[index]);
}
}
//停止處理
} else if((uart_receive_buf[0] == '#') && (uart_receive_buf[4] == 'P') && (uart_receive_buf[5] == 'D') && (uart_receive_buf[6] == 'S') && (uart_receive_buf[7] == 'T')) {
index = (uart_receive_buf[1]-'0')*100 + (uart_receive_buf[2]-'0')*10 + (uart_receive_buf[3]-'0');
if(index < SERVO_NUM) {
servo_do[index].inc = 0.001;
servo_do[index].aim = servo_do[index].cur;
}
} else if((uart_receive_buf[0] == '#') || (uart_receive_buf[0] == '{')) { //解析以“#”或者以“{”開頭的指令
len = strlen(uart_receive_buf); //獲取串口接收數據的長度
index=0; pwm1=0; time1=0; //3個參數初始化
for(i = 0; i < len; i++) { //
if(uart_receive_buf == '#') { //判斷是否為起始符“#”
i++; //下一個字符
while((uart_receive_buf != 'P') && (i<len)) { //判斷是否為#之后P之前的數字字符
index = index*10 + (uart_receive_buf - '0'); //記錄P之前的數字
i++;
}
i--; //因為上面i多自增一次，所以要減去1個
} else if(uart_receive_buf == 'P') { //檢測是否為“P”
i++;
while((uart_receive_buf != 'T') && (i<len)) { //檢測P之后T之前的數字字符并保存
pwm1 = pwm1*10 + (uart_receive_buf - '0');
i++;
}
i--;
} else if(uart_receive_buf == 'T') { //判斷是否為“T”
i++;
while((uart_receive_buf != '!') && (i<len)) {//檢測T之后!之前的數字字符并保存
time1 = time1*10 + (uart_receive_buf - '0'); //將T后面的數字保存
i++;
}
if(time1<SERVO_TIME_PERIOD)time1=SERVO_TIME_PERIOD;//很重要，防止被除數為0
if((index == 255) && (pwm1 >= 500) && (pwm1 <= 2500) && (time1<10000)) { //如果舵機號和PWM數值超出約定值則跳出不處理
for(int i=0;i<SERVO_NUM;i++) {
pwm2 = pwm1+eeprom_info.dj_bias_pwm;
if(pwm2 > 2500)pwm2 = 2500;
if(pwm2 < 500)pwm2 = 500;
servo_do.aim = pwm2; //舵機PWM賦值,加上偏差的值
servo_do.time1 = time1; //舵機執行時間賦值
float pwm_err = servo_do.aim - servo_do.cur;
servo_do.inc = (pwm_err*1.00)/(time1/SERVO_TIME_PERIOD); //根據時間計算舵機PWM增量
}
} else if((index >= SERVO_NUM) || (pwm1 > 2500) ||(pwm1 < 500)|| (time1>10000)) { //如果舵機號和PWM數值超出約定值則跳出不處理
} else {
servo_do[index].aim = pwm1+eeprom_info.dj_bias_pwm[index];; //舵機PWM賦值,加上偏差的值
if(servo_do[index].aim > 2500)servo_do[index].aim = 2500;
if(servo_do[index].aim < 500)servo_do[index].aim = 500;
servo_do[index].time1 = time1; //舵機執行時間賦值
float pwm_err = servo_do[index].aim - servo_do[index].cur;
servo_do[index].inc = (pwm_err*1.00)/(time1/SERVO_TIME_PERIOD); //根據時間計算舵機PWM增量
}
index = pwm1 = time1 = 0;
}
}
}
}
//查詢str是包含str2，并返回最后一個字符所在str的位置
u16 str_contain_str(u8 *str, u8 *str2) {
u8 *str_temp, *str_temp2;
str_temp = str;
str_temp2 = str2;
while(*str_temp) {
if(*str_temp == *str_temp2) {
while(*str_temp2) {
if(*str_temp++ != *str_temp2++) {
str_temp = str_temp - (str_temp2-str2) + 1;
str_temp2 = str2;
break;
}
}
if(!*str_temp2) {
return (str_temp-str);
}
} else {
str_temp++;
}
}
return 0;
}
//解析串口指令
void parse_cmd(u8 *uart_receive_buf) {
static u8 jxb_r_flag = 0;
int int1,int2,int3,int4;
u16 pos;
if(pos = str_contain_str((char *)uart_receive_buf, "$DRS!"), pos) {
Serial.println("$DRS!");
} else if(pos = str_contain_str((char *)uart_receive_buf, "$RST!"), pos) {
resetFunc();
} else if(pos = str_contain_str((char *)uart_receive_buf, "$DST!"), pos) {
Serial.println("#255PDST!");
for(int i;i<SERVO_NUM;i++) {
servo_do.aim = servo_do.cur;
}
group_do_ok = 1;
Serial.println("@DoStop!");
}else if(pos = str_contain_str((char *)uart_receive_buf, "$DST:"), pos) {
if(sscanf(uart_receive_buf, "$DST:%d!", &int1)) {
servo_do[int1].aim = servo_do[int1].cur;
}
Serial.println("@DoStop!");
}else if(pos = str_contain_str((char *)uart_receive_buf, "$BON!"), pos) {
beep_on();
}else if(pos = str_contain_str((char *)uart_receive_buf, "$BOFF!"), pos) {
beep_off();
} else if(sscanf(uart_receive_buf, "$DCR:%d,%d!", &int2, &int3)) {
car_run(int2,int3);
} else if(pos = str_contain_str((char *)uart_receive_buf, "$PTG:"), pos) {
if(sscanf(uart_receive_buf, "$PTG:%d-%d!", &int1, &int2)) {
Serial.println(F("Your Action:"));
mem.begin(_W25Q64,SPI,SS);
for(int i=int1; i<=int2; i++) {
mem.read((unsigned long)i*ACTION_SIZE, uart_receive_buf, RECV_SIZE-1);
if(uart_receive_buf[0] == '{' && uart_receive_buf[1] == 'G') {
Serial.println((char *)uart_receive_buf);
} else {
sprintf(uart_receive_buf, "@NoGroup %d!", i);
Serial.println((char *)uart_receive_buf);
}
delay(10);
}
mem.end();
}
} else if(pos = str_contain_str((char *)uart_receive_buf, "$DGT:"), pos) {
if(sscanf(uart_receive_buf, "$DGT:%d-%d,%d!", &int1, &int2, &int3)) {
group_num_start = int1;
group_num_end = int2;
group_num_times = int3;
do_time = int3;
do_start_index = int1;
if(int1 == int2) {
do_group_once(int1);
} else {
group_do_ok = 0;
}
}
}
else if(pos = str_contain_str((char *)uart_receive_buf, "$GETA!"), pos) {
downLoad = true;
downLoadSystickMs = millis();
Serial.println(F("A"));
} else {
}
}
//循環執行動作組
void loop_action() {
static long long systick_ms_bak = 0;
if(group_do_ok == 0) {
//循環檢測單個動作執行時間是否到位
if(millis() - systick_ms_bak > action_time) {
systick_ms_bak = millis();
//Serial.println(do_start_index);
if(group_num_times != 0 && do_time == 0) {
group_do_ok = 1;
Serial.println("@GroupDone!");
return;
}
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--;
}
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--;
}
return;
}
do_start_index--;
}
}
} else {
action_time = 10;
}
}
//執行動作組單次
void do_group_once(int index) {
static long long systick_ms_bak = 0;
int len;
//讀取動作
mem.begin(_W25Q64,SPI,SS);
mem.read((unsigned long)index*ACTION_SIZE, uart_receive_buf, RECV_SIZE-1);
//獲取時間
action_time = getMaxTime(uart_receive_buf);
parse_action(uart_receive_buf);
mem.end();
}
//執行動作組多次 只適用于總線舵機
void do_groups_times(int st, int ed, int tm) {
int myst = st, myed = ed, mytm = tm;
if(tm<=0)return;
while(mytm) {
for(int i=myst;i<=myed;i++) {
do_group_once(i);
delay(action_time);
}
mytm --;
if(mytm) {
myst = st;
myed = ed;
}
}
}
//串口中斷
void serialEvent() {
static char sbuf_bak;
while(Serial.available()) { //
sbuf_bak = char(Serial.read());
if(uart1_get_ok) return;
if(uart1_mode == 0) {
if(sbuf_bak == '<') {
uart1_mode = 4;
downLoadSystickMs = millis();
downLoad = true;
} else if(sbuf_bak == '{') {
uart1_mode = 3;
} else if(sbuf_bak == '$') {
uart1_mode = 1;
} else if(sbuf_bak == '#') {
uart1_mode = 2;
}
uart_receive_str = "";
}
uart_receive_str += sbuf_bak;
if((uart1_mode == 4) && (sbuf_bak == '>')){
uart1_get_ok = 1;
} else if((uart1_mode == 3) && (sbuf_bak == '}')){
uart1_get_ok = 1;
} else if((uart1_mode == 1) && (sbuf_bak == '!')){
uart_receive_str_bak = uart_receive_str;
uart1_get_ok = 1;
} else if((uart1_mode == 2) && (sbuf_bak == '!')){
uart_receive_str_bak = uart_receive_str;
uart1_get_ok = 1;
}
if(uart_receive_str.length() >= RECV_SIZE) {
uart_receive_str = "";
}
}
}
void set_servo(int mindex, int mpwm, int mtime) {
servo_do[mindex].aim = mpwm;
servo_do[mindex].time1 = mtime;
servo_do[mindex].inc = (servo_do[mindex].aim - servo_do[mindex].cur) / (servo_do[mindex].time1/20.000);
sprintf((char *)cmd_return, "#%03dP%04dT%04d! ", mindex, mpwm, mtime);
Serial.print(cmd_return);
//Serial.println(kinematics.servo_angle[mindex]);
}
;
char cmd_return_tmp[64];
void AI_parse_cmd() {
if (String(uart_receive_str_bak).length()) {
if (String(uart_receive_str_bak).equals(String("$SMODE0!"))) {
Serial.println("switch AI mode!");
} else if (String(uart_receive_str_bak).equals(String("$SMART_STOP!"))) {
Serial.println("#255PDST!");
} else if (String(uart_receive_str_bak).equals(String("$ZPY!"))) {
sprintf(cmd_return_tmp, "{#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!}",6,1500+(-800),7,1500-800,8,1500+800,9,1500-(-800)); //組合指令
Serial.println(cmd_return_tmp); //解析ZMotor4指令
} else if (String(uart_receive_str_bak).equals(String("$YPY!"))) {
sprintf(cmd_return_tmp, "{#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!}",6,1500+800,7,1500-(-800),8,1500+(-800),9,1500-800); //組合指令
Serial.println(cmd_return_tmp); //解析ZMotor4指令
} else if (String(uart_receive_str_bak).equals(String("$QJ!"))) {
sprintf(cmd_return_tmp, "{#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!}",6,1500+800,7,1500-800,8,1500+800,9,1500-800); //組合指令
Serial.println(cmd_return_tmp); //解析ZMotor4指令
} else if (String(uart_receive_str_bak).equals(String("$HT!"))) {
sprintf(cmd_return_tmp, "{#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!}",6,1500+(-800),7,1500-(-800),8,1500+(-800),9,1500-(-800)); //組合指令
Serial.println(cmd_return_tmp); //解析ZMotor4指令
} else if (String(uart_receive_str_bak).equals(String("$ZZ!"))) {
sprintf(cmd_return_tmp, "{#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!}",6,1500+(-800),7,1500-800,8,1500+(-800),9,1500-800); //組合指令
Serial.println(cmd_return_tmp); //解析ZMotor4指令
} else if (String(uart_receive_str_bak).equals(String("$YZ!"))) {
sprintf(cmd_return_tmp, "{#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!#%03dP%04dT0000!}",6,1500+800,7,1500-(-800),8,1500+800,9,1500-(-800)); //組合指令
Serial.println(cmd_return_tmp); //解析ZMotor4指令
}
uart_receive_str_bak = ""; //數據清零
}
}
float checkdistance_A7_A6() {
digitalWrite(A7, LOW);
delayMicroseconds(2);
digitalWrite(A7, HIGH);
delayMicroseconds(10);
digitalWrite(A7, LOW);
float distance = pulseIn(A6, HIGH) / 58.00;
//delay(10);
return distance;
}
void avoidObstacle() {
// Step1: 右平移
Set_speed(0, 50); // x軸正方向平移
delay(1000); // 可調節避障距離
// Step2: 前進
Set_speed(60, 0); // y軸前進
delay(2000);
// Step3: 左平移回歸
Set_speed(0, -50); // x軸負方向平移
delay(1000);
// 停止
Set_speed(0, 0);
delay(100);
}
void init_x_PID(void)
{
KP_x = KPx;
KI_x = KIx;
KD_x = KDx;
error_x = 0;
error_last_x = 0;
integral_x = 0;
}
int PID_count_x(void)
{
int pwmoutx = 0;
//這里求算錯誤
LineWalking();
error_x = err - 0; //0:代表在線中間
Serial.println("error_x=" + String(error_x));
//位置式
pwmoutx = error_x * KP_x + integral_x *KI_x + error_last_x *KD_x ;
Serial.println("integral_x=" + String(integral_x));
Serial.println("error_last_x=" + String(error_last_x));
integral_x += error_x;
error_last_x = error_x;
if(integral_x > 30)
integral_x = 30;
else if(integral_x < -30)
integral_x = -30;
return pwmoutx;
}
int R_count=0;//左轉路口
int L_count=0;//右轉路口
int ALL_count=0;//檢測到所有黑線
int block=0;
int UR=0;//超聲波傳感器是否識別到物塊
int clo=1;//顏色分別1：黑色，假設在第一個格子 2：白色，假設在第二個格子 3：紅色，假設在第三個格子 4：黃色，假設在第4個格子 5：綠色，假設在第四個格子
//x1-x8 從左往右數
uint8_t x1,x2,x3,x4,x5,x6,x7,x8;
void LineWalking(void)
{
if(x1 == 1 && x2 == 1 && x3 == 1&& x4 == 0 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1) // 1110 1111
{
err = -1;
}
else if(x1 == 1 && x2 == 1 && x3 == 0&& x4 == 0 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1) // 1100 1111
{
err = -1;
}
else if(x1 == 1 && x2 == 1 && x3 == 0&& x4 == 1 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1) // 1101 1111
{
err = -1;
}
else if(x1 == 1 && x2 == 0 && x3 == 1&& x4 == 1 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1) // 1011 1111
{
err = -2;
}
else if(x1 == 1 && x2 == 0 && x3 == 0&& x4 == 1 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1) // 1001 1111
{
err = -2;
}
else if(x1 == 1 && x2 == 1 && x3 == 1&& x4 == 1 && x5 == 0 && x6 == 1 && x7 == 1 && x8 == 1) // 1111 0111
{
err = 1;
}
else if(x1 == 1 && x2 == 1 && x3 == 1&& x4 == 1 && x5 == 0 && x6 == 0 && x7 == 1 && x8 == 1) // 1111 0011
{
err = 1;
}
else if(x1 == 1 && x2 == 1 && x3 == 1&& x4 == 1 && x5 == 1 && x6 == 0 && x7 == 1 && x8 == 1) // 1111 1011
{
err = 1;
}
else if(x1 == 1 && x2 == 1 && x3 == 1&& x4 == 1 && x5 == 1 && x6 == 0 && x7 == 0 && x8 == 1) // 1111 1001
{
err = 2;
}
else if(x1 == 1 && x2 == 1 && x3 == 1&& x4 == 1 && x5 == 1 && x6 == 1 && x7 == 0 && x8 == 1) // 1111 1101
{
err = 2;
}
else if(x1 == 1 && x2 == 1 && x3 == 1&& x4 == 0 && x5 == 0 && x6 == 1 && x7 == 1 && x8 == 1) // 1110 0111
{
err = 0;
}
if(x1 == 1 && x2 == 1 && x3 == 1&& x4 == 0 && x5 == 0 && x6 == 0 && x7 == 0 && x8 == 0) // 1110 0000
{
R_count++;
if((R_count==3&&block==1)||(R_count==4&&block==1)||(R_count==5&&block==1)||(R_count==1&&block==0))
{
car_run(200,200);
delay(1 000);
}
else if((R_count==2&&block==0)||(R_count==3&&L_count==3&&block==0))
{
Set_speed(0,0);
err = 0;
error_x = 0;
error_last_x = 0;
integral_x = 0;
in_zone = true;
beep_off();
car_run(200,200)
delay(2000);
//car_run(0,0)
car_run(-500, 500);
delay(200);
in_zone = false;
}
else if(R_count-clo==11&&clo==1)
{
Set_speed(0,0);
err = 0;
error_x = 0;
error_last_x = 0;
integral_x = 0;
in_zone = true;
beep_off();
car_run(200,200)
delay(2000);
//car_run(0,0)
car_run(500, -500);
delay(2000);
in_zone = false;
}
}
else if(x1 == 1 && x2 == 1 && x3 == 1&& x4 == 1 && x5 == 0 && x6 == 0 && x7 == 0 && x8 == 0) // 1111 0000
{
R_count++;
if((R_count==3&&block==1)||(R_count==4&&block==1)||(R_count==5&&block==1)||(R_count==1&&block==0))
{
car_run(800,800);
delay(200);
err=0;
}
else if((R_count==2&&block==0)||(R_count==3&&L_count==3&&block==0))
{
car_run(800,-800);
delay(250);
}
else if(R_count-clo==11&&clo==1)
{
car_run(800,-800);
delay(250);
}
}
else if(x1 == 1 && x2 == 1 && x3 == 0&& x4 == 0 && x5 == 0 && x6 == 0 && x7 == 0 && x8 == 0) // 1100 0000
{
R_count++;
if((R_count==3&&block==1)||(R_count==4&&block==1)||(R_count==5&&block==1)||(R_count==1&&block==0))
{
car_run(800,800);
delay(200);
err=0;
}
else if((R_count==2&&block==0)||(R_count==3&&L_count==3&&block==0))
{
car_run(800,-800);
delay(250);
}
else if(R_count-clo==11&&clo==1)
{
car_run(800,-800);
delay(250);
}
}
else if(x1 == 0 && x2 == 0 && x3 == 0&& x4 == 0 && x5 == 0 && x6 == 1 && x7 == 1 && x8 == 1) // 0000 0111
{
L_count++;
car_run(-800,1000);
delay(250);
car_run(0,0);
delay(1500);
if((R_count==2&&L_count==3&&block==1)||(R_count==3&&L_count==4&&block==1)||(R_count==5&&L_count==5&&block==1))
{
R_count=10;
}
in_zone = false;
}
else if(x1 == 0 && x2 == 0 && x3 == 0&& x4 == 0 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1) // 0000 1111
{
L_count++;
car_run(-800,1000);
delay(250);
car_run(0,0);
delay(1500);
if((R_count==2&&L_count==3&&block==1)||(R_count==3&&L_count==4&&block==1)||(R_count==5&&L_count==5&&block==1))
{
R_count=10;
}
}
else if(x1 == 0 && x2 == 0 && x3 == 0&& x4 == 0 && x5 == 0 && x6 == 0 && x7 == 1 && x8 == 1) // 0000 0011
{
L_count++;
car_run(-800,1000);
delay(250);
car_run(0,0);
delay(1500);
if((R_count==2&&L_count==3&&block==1)||(R_count==3&&L_count==4&&block==1)||(R_count==5&&L_count==5&&block==1))
{
R_count=10;
}
}
else if ((x1 == 0 && x2 == 0 && x3 == 0 && x4 == 0 && x5 == 0 && x6 == 0 && x7 == 0 && x8 == 0)||(x1 == 1 && x2 == 0 && x3 == 0 && x4 == 0 && x5 == 0 && x6 == 0 && x7 == 0 && x8 == 0)||(x1 == 1 && x2 == 0 && x3 == 0 && x4 == 0 && x5 == 0 && x6 == 0 && x7 == 0 && x8 == 1)) // 全黑（Zone 區域）
{
ALL_count++;
// 第一次進入 Zone，未檢測到物塊
if (ALL_count %2== 1 && UR == 0 && block == 0)
{
Set_speed(0,0);
err = 0;
error_x = 0;
error_last_x = 0;
integral_x = 0;
in_zone = true;
beep_off();
delay(2000);
//car_run(0,0)
car_run(-1000, 1000); // 掉頭
delay(900);
in_zone = false;
}
// 第一次進入 Zone，檢測到物塊
else if (ALL_count %2== 1 && UR == 1 && block == 0)
{
err = 0;
error_x = 0;
error_last_x = 0;
integral_x = 0;
in_zone = true;
beep_off();
delay(1000); // 等待夾取
in_zone = false;
car_run(-1000, 1000); // 掉頭
delay(400);
block = 1;
}
// 第二次進入 Zone，準備放置物塊
else if ((ALL_count == 3 && block == 1)||(ALL_count == 5 && block == 1)||(ALL_count == 6 && block == 1))
{
car_run(0, 0);
err = 0;
error_x = 0;
error_last_x = 0;
integral_x = 0;
beep_off();
in_zone = true;
delay(1000);
// 放置函數調用（可擴展）
car_run(0, 0);
delay(100);
while (1)
{
// LED 閃爍或其他終止動作
}
}
// 其他偶數次進入 Zone
else if (ALL_count % 2 == 0)
{
if (block == 1)
{
car_run(1000, -900); // 右轉
delay(250);
}
else
{
car_run(-900, 1000); // 左轉
delay(200);
}
}
}
//剩下的就保持上一個狀態
}
void Car_line_track(void)
{
if (in_zone) {
Set_speed(0, 0);
return;
}
int PWMx;
PWMx = PID_count_x();
if (error_x == 6 || error_x == -6)
{
Set_speed(Speed_Line+1,PWMx+1);
delay(500);
}
else
{
Set_speed(Speed_Line,PWMx); //傳入值即可
}
}
int speed_L = 0;
int speed_R = 0;
void Motor_init() {
Wire.begin(); // 初始化I2C通訊 Initialize I2C communication
delay(1000); // 如果小車功能異常，可以增加這個延時 If the function is abnormal, you can increase the delay
setCarMove(STOP); // 設置小車停止狀態 Set the car to stop state
}
/**
* @brief 設置單個電機速度 Setting the Motor Speed
* @param motor_forward_pin: 控制電機前進引腳 Control the motor forward pin
* @param motor_backward_pin: 控制電機后退引腳 Control the motor backward pin
* @param motor_speed: 設置電機速度 Setting the Motor Speed
* @retval 無 None
*/
void setMotorSpeed(int motor_speed_L, int motor_speed_R) {
motor_speed_L = map(motor_speed_L, -120, 120, -1200, 1200);
motor_speed_R = map(motor_speed_R, -120, 120, -1200, 1200);
car_run(motor_speed_L,motor_speed_R);
}
/**
* @brief 設置小車運動方式和速度 Set the car movement mode and speed
* @param Movement: 小車運動方式 Car movement
* @param Speed: 小車運動速度 Car speed
* @retval 無 None
*/
void setCarMove(uint8_t Movement) {
switch (Movement) {
case STOP:
car_run(0, 0);
break;
case FORWARD:
car_run(800, 800);
break;
case BACKWARD:
car_run(-800, -800);
break;
case LEFT:
car_run(-800, 800);
break;
case RIGHT:
car_run(800, -800);
break;
default:
car_run(0, 0);
break;
}
}
int myabs(int a)
{
if(a<0)
return -a;
return a;
}
int myignore_speed(int speed)
{
if(speed == 0)
return 0;
if(speed < 0)
{
if(speed > -Ignore_speed)
{
speed = -Ignore_speed;
}
}
else if (speed < Ignore_speed)
{
speed = Ignore_speed;
}
return speed;
}
int limin_speed(int speed,int max,int min)
{
if(speed > max)
{
return max;
}
else if(speed<min)
{
return min;
}
return speed;
}
//speed_fb y軸 speed_lr ：x軸
void Set_speed(int speed_fb,int speed_lr)
{
speed_L = speed_fb + speed_lr ;
speed_R = speed_fb - speed_lr ;
//滿足速度范圍
speed_L = limin_speed(speed_L,MAX_Speed,-MAX_Speed);
speed_R = limin_speed(speed_R,MAX_Speed,-MAX_Speed);
//去速度死區
speed_L = myignore_speed(speed_L);
speed_R = myignore_speed(speed_R);
//直接輸出pwm
setMotorSpeed(speed_L, speed_R);
}
//i2c讀取函數
void I2Cdata()
{
byte data = 0; // 用于存儲讀取的數據
char bufbuf[50]={'\0'};
Wire.beginTransmission(0x12); // I2C設備的地址
Wire.write(0x30); // 寄存器地址
Wire.endTransmission();
delay(10); // 給設備足夠的時間來處理請求
Wire.requestFrom(0x12, 1); // 請求1個字節的數據
while (Wire.available()) {
data = Wire.read(); // 讀取數據
}
x1 = (data>>7)&0x01;
x2 = (data>>6)&0x01;
x3 = (data>>5)&0x01;
x4 = (data>>4)&0x01;
x5 = (data>>3)&0x01;
x6 = (data>>2)&0x01;
x7 = (data>>1)&0x01;
x8 = (data>>0)&0x01;
sprintf(bufbuf,"x1:%d,x2:%d,x3:%d,x4:%d,x5:%d,x6:%d,x7:%d,x8:%d\r\n",x1,x2,x3,x4,x5,x6,x7,x8);
Serial.println(bufbuf);
Serial.println(data, HEX); // 在串行監視器上打印數據（十六進制）
}
/**
* @brief 獲取按鍵狀態 Get key(button) status
* @param pin: 按鍵控制引腳 Control key(button) pins
* @retval 按鍵狀態 Key(button) Status
*/
int getKeyState(uint8_t pin) {
if (digitalRead(pin) == LOW) {
delay(10);
if (digitalRead(pin) == LOW) {
return Press_KEY_1;
}
return Release_KEY_1;
} else {
return Release_KEY_1;
}
}
void I2Cdata(void) ;
void setup()
{
init_x_PID();//PID初始化
Serial.begin(115200);//串口初始化
Wire.begin(); // join i2c bus (address optional for master)
pinMode(KEY1_PIN, INPUT_PULLUP); // 設置按鍵KEY引腳上拉輸入模式 Set the key(button) pin to pull-up input mode
pinMode(KEY2_PIN, INPUT_PULLUP);
delay(100);
Motor_init(); //電機初始化
setup_w25q(); //讀取全局變量
setup_nled(); //led燈閃爍初始化
setup_beep(); //蜂鳴器初始化
setup_start_pre_cmd(); //系統啟動
systick_ms = 0;
nextTime = 0;
distance = 0;
dis_hold = 0;
debug_mode = false;
setup_servo(); //舵機初始化
for(int i=0;i<3;i++) {
beep_on();
delay(100);
beep_off();
delay(100);
}
while (getKeyState(KEY1_PIN) != Press_KEY_1) ;//按下key1開始巡線
}
void loop()
{
loop_nled(); //切換LED燈狀態實現LED燈閃爍
loop_uart(); //解析串口接收到的字符串指令
loop_action(); //循環執行是否需要讀取數據執行動作組
if(downLoad)return;
loop_servo(); //處理模擬舵機增量
I2Cdata();
/* float distance = checkdistance_A7_A6();
if (distance > 0 && distance < 15) {
Serial.println("檢測到障礙，執行避障");
avoidObstacle();
return;
}*/
Car_line_track();
}
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
