android 控制485舵机舵机控制指令

转载

mob64ca140b466e 2024-01-27 23:24:43

文章标签 android 控制485舵机 Arduino 串口舵机 i++ 文章分类 Android 移动开发

简单来说，控制舵机速度就是改变舵机旋转角度的时长，在PWM层面上讲就是舵机PWM增量的不同直接导致舵机角速度的不同，例如以20ms为周期，要求在1000ms时间内舵机角度从0°转到180°，即舵机PWM值由500改变到2500，则一个周期内PWM增量为40，每经过一次周期时间舵机就转动一个PWM增量，这就间接体现了舵机的速度。

舵机控制指令

因为要六个舵机，因此指令格式不能乱。控制板商家提供的源码将舵机控制指令定位如下格式：
单舵机控制指令：#indexPpwmTtime!(index-舵机序号000,001,002…;pwm-舵机PWM值0500-2500之间;time-舵机执行时间0-65535ms)多舵机控制指令：{#index1Ppwm1Ttime1！#index2Ppwm2Ttime2！…}动作组指令，多个单舵机指令合并在一起，然后加个大括号,也可以不加 另外，定义一个结构体数组，分别存储六个舵机的各种信息。

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];

指令解析

这里就存在串口指令解析的问题。这里的指令解析，简单来说就是划分字符串，在识别到特定字符时，循环读取特定字符后的数字，不断地读取赋值，直到遇到结束符。代码如下：

void parseInStringCmd(){
    static unsigned int index, time1, pwm1, i;//声明三个变量分别用来存储解析后的舵机序号，舵机执行时间，舵机PWM
    static unsigned int len;                 //存储字符串长度  
    if(inString.length() > 0) {   //判断串口有数据      
        if((inString[0] == '#') || (inString[0] == '{')) {   //解析以“#”或者以“{”开头的指令
            char len = inString.length();       //获取串口接收数据的长度
            index=0; pwm1=0; time1=0;           //3个参数初始化
            for(i = 0; i < len; i++) {          //
                if(inString[i] == '#') {        //判断是否为起始符“#”
                    i++;                        //下一个字符
                    while((inString[i] != 'P') && (i<len)) {     //判断是否为#之后P之前的数字字符
                        index = index*10 + (inString[i] - '0');  //记录P之前的数字
                        i++;
                    }
                    i--;                          //因为上面i多自增一次，所以要减去1个
                } else if(inString[i] == 'P') {   //检测是否为“P”
                    i++;
                    while((inString[i] != 'T') && (i<len)) {  //检测P之后T之前的数字字符并保存
                        pwm1 = pwm1*10 + (inString[i] - '0');
                        i++;
                    }
                    i--;
                } else if(inString[i] == 'T') {  //判断是否为“T”
                    i++;
                    while((inString[i] != '!') && (i<len)) {//检测T之后!之前的数字字符并保存
                        time1 = time1*10 + (inString[i] - '0'); //将T后面的数字保存
                        i++;
                    }
                    if((index >= SERVO_NUM) || (pwm1 > 2500) ||(pwm1<500)) {  //如果舵机号和PWM数值超出约定值则跳出不处理 
                        break;
                    }
                    //检测完后赋值
                    servo_do[index].aim = pwm1;         //舵机PWM赋值
                    servo_do[index].time1 = time1;      //舵机执行时间赋值
                    float pwm_err = servo_do[index].aim - servo_do[index].cur;
                    servo_do[index].inc = (pwm_err*1.00)/(servo_do[index].time1/SERVO_TIME_PERIOD); //根据时间计算舵机PWM增量
                    index = pwm1 = time1 = 0; //赋值完成后清零
                }
            }
        } else if(strcmp(inString.c_str(),"$DST!")==0) { //.c_str把C++中的字符串转换为C中的字符串,然后和字符串"$DST!"作比较
                                                         //使用格式：类型 strcmp（参数1，参数2）
                                                         //功 能: 比较参数1和参数（1、若参数1>参数2，返回正数；2、若参数1<参数2，返回负数；3、若参数1=参数2，返回0；）   
            for(i = 0; i < SERVO_NUM; i++) {
                servo_do[i].aim =  (int)servo_do[i].cur;
            }           
        } 
    inString = "";
    } 
}

时间处理

开头就说明了舵机速度控制存在周期问题，则在启动时首先需要经过一个周期，舵机才会有一个增量，没到一个周期时间程序就得返回，重新循环进入舵机使能函数。时间处理函数如下：

bool handleTimePeriod( unsigned long *ptr_time, unsigned int time_period) {
    if((millis() - *ptr_time) < time_period) {//millis()返回Arduino开始运行当前程序以来经历的毫秒数
        return 1;  
    } else{
         *ptr_time = millis();
         return 0;
    }
}

舵机增量处理

每隔一个设定周期，舵机就转动一个PWM增量角度。如果此时舵机目标位置与初始位置之间的PWM差值小于PWM增量，则可说明此时舵机运动非常慢，让舵机直接运动到目标位置即可；如果大于增量，则按增量进行运动。代码如下：

void handleServo() {
    static unsigned long systick_ms_bak = 0;
    if(handleTimePeriod(&systick_ms_bak, SERVO_TIME_PERIOD))return;  //20ms处理一次，不到20ms则返回不处理
    for(byte i = 0; i < SERVO_NUM; i++) {
        if(abs( servo_do[i].aim - servo_do[i].cur) <= abs (servo_do[i].inc) ) {//这里就体现了这个程序的精度，SERVO_TIME_PERIOD越小精度越高
             myservo[i].writeMicroseconds(servo_do[i].aim);      
             servo_do[i].cur = servo_do[i].aim;
             
        } else {
             servo_do[i].cur +=  servo_do[i].inc;
             myservo[i].writeMicroseconds((int)servo_do[i].cur); 
        }    
    }
}