智能锥桶回收
/*----------------------------------------------------------------------------关于设置类的都放在这个标签
------------------------------------------------------------------------------*/
/******************************************************************
宏定义PS2手柄引脚，用有有意义的字符代表相应是引脚，便于使用
******************************************************************/
#definePS2_CLK 4
#definePS2_CMD 9
#definePS2_ATT 2
#definePS2_DAT 8
#defineLED_PIN 13 //宏定义工作指示灯引脚
#defineSERVO_NUM6 //宏定义舵机个数
#defineBEEP_PIN 12
#defineSERVO_TIME_PERIOD 20 //每隔20ms处理一次（累加）舵机的PWM增量
#define nled_on() {digitalWrite(LED_PIN, LOW);}
#define nled_off() {digitalWrite(LED_PIN, HIGH);}
#define beep_on() {digitalWrite(BEEP_PIN, LOW);}
#define beep_off() {digitalWrite(BEEP_PIN, HIGH);}
//地址存储
#define BIAS_ADDR_VERIFY 30
#define BIAS_ADDR_START 0
#define PRE_ACTION_GROUPS 37
Servo myservo; //创建一个舵机类
PS2X ps2x;
char buffer; // 定义一个数组用来存储每小组动作组
byteservo_pin = {10, A2, A3, A0, A1 ,7}; //宏定义舵机控制引脚
String uart_receive_str = ""; //声明一个字符串数组
char uart1_zbxyz = 0;//后加（坐标标志）
char uart1_get_ok = 0, uart1_mode=0;
char cmd_return;
char uart_receive_buf;
const char pre_action PROGMEM = {
//动作生成数据为(可直接全选复制粘贴到程序中)：
//偏差调节组
"{G0000#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1500T1000!}",
//直立
"{G0001#000P1500T1500!#001P1500T1500!#002P1500T1500!#003P1500T1500!#004P1500T1500!#005P1500T1000!}",
//蜷缩
"{G0002#000P1500T1500!#001P2000T1000!#002P2300T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
//大前抓右放 K0001(3-11,9)
"{G0003#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1100T1000!}",
"{G0004#000P1500T1000!#001P1100T1000!#002P1600T1000!#003P2100T1000!#004P1500T1000!#005P1000T0500!}",
"{G0005#000P1500T1500!#001P1050T1500!#002P1600T1000!#003P2100T1000!#004P1500T1000!#005P1800T0500!}",
"{G0006#000P1500T1000!#001P1800T1000!#002P1800T1000!#003P2100T1000!#004P1500T1000!#005P1800T0500!}",
"{G0007#000P0800T1000!#001P1500T1000!#002P1500T1000!#003P2100T1000!#004P1500T1000!#005P1800T0500!}",
"{G0008#000P0800T1000!#001P1300T1000!#002P1800T1000!#003P2100T1000!#004P1500T1000!#005P1800T1000!}",
"{G0009#000P0800T1000!#001P1300T1000!#002P1800T1000!#003P2100T1000!#004P1500T1000!#005P1100T0500!}",
"{G0010#000P0800T1000!#001P1600T1000!#002P1800T1000!#003P2100T1000!#004P1500T1000!#005P1100T1000!}",
"{G0011#000P1500T1500!#001P2000T1500!#002P2300T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
//前爪前放 K0002(12-18,7)
"{G0012#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1500T1000!}",
"{G0013#000P1500T1000!#001P1100T2000!#002P1900T1000!#003P2050T1000!#004P1500T1000!#005P1000T1000!}",
"{G0014#000P1500T1000!#001P1100T2000!#002P1900T1000!#003P2050T1000!#004P1500T1000!#005P1800T1000!}",
"{G0015#000P1500T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1800T1000!}",
"{G0016#000P1500T1000!#001P1100T2000!#002P1800T1000!#003P2000T1000!#004P1500T1000!#005P1800T1000!}",
"{G0017#000P1500T1000!#001P1100T2000!#002P1800T1000!#003P2000T1000!#004P1500T1000!#005P1100T1000!}",
"{G0018#000P1500T1500!#001P2000T1500!#002P2300T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
//前爪左放 K0003(19-27,9)
"{G0019#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1500T1000!}",
"{G0020#000P1500T1000!#001P1100T2000!#002P1900T1000!#003P2050T1000!#004P1500T1000!#005P1000T0500!}",
"{G0021#000P1500T1000!#001P1050T2000!#002P1900T1000!#003P2050T1000!#004P1500T1000!#005P1800T0500!}",
"{G0022#000P1520T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1800T1000!}",
"{G0023#000P2200T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1800T1000!}",
"{G0024#000P2200T1000!#001P1200T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1800T1000!}",
"{G0025#000P2200T1000!#001P1200T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1100T0500!}",
"{G0026#000P2200T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1100T1000!}",
"{G0027#000P1500T1500!#001P2000T1500!#002P2300T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
//前爪右放 K0004(28-36,9)
"{G0028#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1500T1000!}",
"{G0029#000P1500T1000!#001P1100T2000!#002P1900T1000!#003P2050T1000!#004P1500T1000!#005P1000T0500!}",
"{G0030#000P1500T1000!#001P1100T2000!#002P1900T1000!#003P2050T1000!#004P1500T1000!#005P1800T0500!}",
"{G0031#000P1500T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1800T1000!}",
"{G0032#000P0800T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1800T1000!}",
"{G0033#000P0800T1000!#001P1200T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P2000T1000!}",
"{G0034#000P0800T1000!#001P1200T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1000T0500!}",
"{G0035#000P0800T1000!#001P1800T2000!#002P2000T1000!#003P2000T1000!#004P1500T1000!#005P1100T1000!}",
"{G0036#000P1500T1500!#001P2000T1500!#002P2300T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}",
};
typedef struct { //舵机结构体变量声明
unsigned int aim = 1500; //舵机目标值
float cur = 1500.0; //舵机当前值
unsignedint time1 = 1000; //舵机执行时间
float inc= 0.0; //舵机值增量，以20ms为周期
}duoji_struct;
duoji_struct servo_do; //用结构体变量声明一个舵机变量组
int servo_bias = {0}; //偏差范围 -128 ~ +127
byte smart_mode = 255;
void(* resetFunc) (void) = 0;
/*************************************************************
函数名称：abs_int(int t)
功能介绍：输入一个数值，返回一个数的绝对值
函数参数：t 要输入的数值
返回值：输入数的绝对值
*************************************************************/
int abs_int(int t) {
if(t > 0)return t;
return (-t);
}
/*************************************************************
函数名称：selection_sort(int *a, int len)
功能介绍：对 a 数进行排序
函数参数：a 要排序是的数
返回值：len 输入数的个数
*************************************************************/
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 > a) {
mi = j;
}
}
if(mi != i) {
t = a;
a = a;
a = t;
}
}
}
/*************************************************************
函数名称：str_contain_str(u8 *str, u8 *str2)
功能介绍：查询str是包含str2，并返回最后一个字符所在str的位置
函数参数：a 要排序是的数
len 输入数的个数
返回值： 返回最后一个字符所在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;
}
/*************************************************************
函数名称：car_to_str(short speed_left, short speed_right)
功能介绍：电机控制函数
函数参数：speed_left左电机速度
speed_right 右电机速度
返回值：无
*************************************************************/
char* car_to_str(short speed_left, short speed_right) {
sprintf(cmd_return, "#006P%04dT0000!#007P%04dT0000!", 1500+speed_left, 1500-speed_right);
return cmd_return;
}
/*************************************************************
函数名称：beep_on_times(u8 times, int frequency)
功能介绍：蜂鸣器控制函数
函数参数：times次数
frequency 频率
返回值：无
*************************************************************/
void beep_on_times(u8 times, int frequency) {
for(byte i = 0; i < times; i++ ) {
digitalWrite(BEEP_PIN, LOW);
delay(frequency);
digitalWrite(BEEP_PIN, HIGH );
delay(frequency);
}
}
/*************************************************************
函数名称：printSerialStr(char str[])
功能介绍：打印字符串数组
函数参数：char str[]-要打印的字符串数组
返回值：无
//strcpy_P()http://www.360doc.com/content/17/0101/12/36498681_619275664.shtml
*************************************************************/
void printSerialStr(char str[]) {
int len = strlen_P(str), i;
char mychar;
for(i=0;i<len;i++) {
mychar = pgm_read_byte_near(str+ i);
Serial.print(mychar);
}
Serial.println("");
}
页:
[1]