智能锥桶回收

admin

/*----------------------------------------------------------------------------
    关于设置类的都放在这个标签   
------------------------------------------------------------------------------*/
/******************************************************************
宏定义PS2手柄引脚，用有有意义的字符代表相应是引脚，便于使用
******************************************************************/
#define  PS2_CLK    4
#define  PS2_CMD    9
#define  PS2_ATT    2
#define  PS2_DAT    8   

#define  LED_PIN    13              //宏定义工作指示灯引脚
#define  SERVO_NUM  6               //宏定义舵机个数
#define  BEEP_PIN   12
#define  SERVO_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[SERVO_NUM];         //创建一个舵机类
PS2X ps2x;
char buffer[168];                 // 定义一个数组用来存储每小组动作组
byte  servo_pin[SERVO_NUM] = {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[64];
char uart_receive_buf[168];
const char pre_action[PRE_ACTION_GROUPS][100] 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;           //舵机当前值
    unsigned  int time1 = 1000;   //舵机执行时间
    float inc= 0.0;               //舵机值增量，以20ms为周期
}duoji_struct;
duoji_struct servo_do[SERVO_NUM];           //用结构体变量声明一个舵机变量组   
int servo_bias[SERVO_NUM] = {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[mi] > a[j]) {
                mi = j;   
            }   
        }   

        if(mi != i) {
            t = a[mi];
            a[mi] = 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("");
}
global.ino (9.47 KB, 下载次数: 546)

2022-7-6 18:41 上传

点击文件名下载附件