[Adruino]XBEE 无线数据传输实际操作

双轮小车制作实例代码

引用:http://hi.baidu.com/dlfla84/item/52b89017a6209c5cf1090e9b

双轮小车制作

2009-6-12 初步完成了串口数据缓存、跨帧读取、协议、LED控制、读取传感器并发送至串口的部分。

但经过试验，12日的程序有问题，遇到了大量的数据丢失等问题。经过两天的调试，终于写出了如下的代码，小车已经可以将发送的命令串接收并返回了。虽然还是有信号丢失的问题，但并不严重，而且经过校验位计算后，会抛弃错误的命令串。当抛弃后，小车可以向上位机发送信号接收失败的信息，让上位机发送新的命令，直到接收成功为止。

整体设计思想：类似一个小游戏，程序的最小循环单位称为帧。在每帧中，完成LED控制、moto控制、传感器读取和指令读取。

其中指令读取是跨帧的，且带有协议。moto指令和LED指令，各有专门的寄存器。 一旦指令读取完毕，就将readState置为CMD_MOTO或CMD_LED的状态，在这种状态下才能激活各自的控制函数。

#define RETRY 20//控制桢速率,这个数值规定的毫秒数为1桢,目前是每秒50桢
#define REREAD 100//每次read命令的时候,跳过-1的次数,目前是每个字符可以重试100次
#define INPUT_SIZE 7
#define LED_SIZE 8
#define MOTO_SIZE 4
#define SENSOR_SIZE 5

#define CMD_MOTO 1
#define CMD_LED 2

#define HEAD 85
#define HEAD_MOTO 170
#define HEAD_LED 187
//LED常用命令,每个数字控制2个灯
//55 bb ff ff ff ff 0c //全部点亮
//55 bb 00 00 00 00 10 //全部熄灭

//moto常用命令
//55 aa 02 02 80 80 03 停止
//55 aa 02 02 ff ff 01 全速前进
//55 aa 02 02 00 00 03 全速后退

int input0,input1,sum,i;//读取串口的字符串,校验和,循环下标
int readState = 0,retryCount = 0,readCount = 0;
int input[INPUT_SIZE];
//led
int ledPort[] = { 3, 4, 2, 5, 13, 10, 12, 11 };//led地址，电路决定。左大-前-下-后，右大-前-下-后
int ledCmd[LED_SIZE];
int ledState[LED_SIZE];//led状态，每个成员可以是0-255，控制LED亮度。
int ledCount;
//sensor
int sensorPort[] = { 0, 1, 2, 4, 3 };//传感器地址,电路决定.accxpin,accypin,acczpin,gyroxpin,gyroypin
int sensorState[SENSOR_SIZE];//传感器的返回值   应该用double吗？
//moto
int E1 = 6;
int E2 = 9;
int M1 = 7;
int M2 = 8;
int motoCmd[MOTO_SIZE];
int motoState[MOTO_SIZE];
/***************** read cmd ***************************************************/
void do_read() {
    while(retryCount < RETRY) {
        if(Serial.available() > 0){
            input1 = Serial.read();
            //完整读取命令并校验，如果读取次数readCount超过REREAD，则放弃
            if(input0 ==HEAD && (input1 == HEAD_MOTO || input1 == HEAD_LED))
            {
                input[0] = input0;
                input[1] = input1;
                for(i = 2;i<INPUT_SIZE;i++){//2:减去了信息头
                    input[i] = Serial.read();
                    if(input[i] == -1 && readCount < REREAD){//跳过-1的
                        i--;
                        readCount++;//不能无限的跳过
                    }else
                        readCount = 0;
                }
                sum=0;//效验和
                for(i=0; i<INPUT_SIZE - 1;i++){
                    sum += input[i];
                }
                sum%=256;
                if(sum == input[INPUT_SIZE-1]){//校验和
                    parseCmd();
                }else
                    outputMsg(0);//输出"error"
            }
            input0 = input1;
        }else{
            delay(1);
            retryCount++;
        }
    }
    retryCount = 0;
}
void parseCmd(){//解析输入命令，转换成moto和led用的命令
    if(input[1] == HEAD_MOTO){
        readState = CMD_MOTO;
        //解析输入数据为moto专用命令
        for(i=0;i<MOTO_SIZE;i++){
            motoCmd[i] = input[i+2];
        }
        //outputMsg(2);//输出moto指令
    }else if(input[1] == HEAD_LED){
        readState = CMD_LED;
        //解析输入数据为led专用命令
        for(i=0;i<LED_SIZE;i+=2){
            ledCmd[i] = input[i/2+2]/16;
            ledCmd[i]*=16;
            ledCmd[i+1] = input[i/2+2]%16;
            ledCmd[i+1]*=16;
        }
        //outputMsg(3);//输出LED指令
    }
    //outputMsg(1);//输出输入指令
}
void outputMsg(int s){
switch(s){
    case 0:
        Serial.println("error");
    break;
    case 1:
        //输出接收到的命令（DEBUG）
        for(i=0;i<INPUT_SIZE;i++){
            Serial.print(input[i]);
            Serial.print(";");
        }
        Serial.println();
    break;
    case 2://输出moto指令
        for(i=0;i<MOTO_SIZE;i++){
            Serial.print(motoCmd[i]);
            Serial.print(";");
        }
        Serial.println();
    break;
    case 3://输出LED指令
        for(i=0;i<LED_SIZE;i++){
            Serial.print(ledCmd[i]);
            Serial.print(";");
        }
        Serial.println();
    break;
}
}
/***************** moto *******************************************************/
void do_moto() {
    if(readState != CMD_MOTO)
        return;
    else
        readState = 0;
   
    if(motoCmd[2]==128&&motoCmd[3]==128){
        stop();
    }else if(motoCmd[2]>128&&motoCmd[3]>128){
        //Serial.print((motoCmd[2]-128)*2);Serial.print(";");Serial.println((motoCmd[3]-128)*2);
        back_off((motoCmd[2]-128)*2,(motoCmd[3]-128)*2);
    }else if(motoCmd[2]<128&&motoCmd[3]>128){
        //Serial.print((128-motoCmd[2])*2);Serial.print(";");Serial.println((motoCmd[3]-128)*2);
        turn_L((128-motoCmd[2])*2,(motoCmd[3]-128)*2);
    }else if(motoCmd[2]<128&&motoCmd[3]<128){
        //Serial.print((128-motoCmd[2])*2);Serial.print(";");Serial.println((128-motoCmd[3])*2);
        advance((128-motoCmd[2])*2,(128-motoCmd[3])*2);
    }else if(motoCmd[2]>128&&motoCmd[3]<128){
        //Serial.print((motoCmd[2]-128)*2);Serial.print(";");Serial.println((128-motoCmd[3])*2);
        turn_R((motoCmd[2]-128)*2,(128-motoCmd[3])*2);
    }
}

void stop(void) //停止
{
    digitalWrite(E1, LOW);
    digitalWrite(E2, LOW);
    set_led(0,0,255,0,0,0,255,0);
}
void advance(char a, char b) //前进
{
    analogWrite(E1, a); //PWM调速
    digitalWrite(M1, LOW);
    analogWrite(E2, b);
    digitalWrite(M2, LOW);
    set_led(255,255,0,0,255,255,0,0);
}
void back_off(char a, char b) //后退
{
    analogWrite(E1, a);
    digitalWrite(M1, HIGH);
    analogWrite(E2, b);
    digitalWrite(M2, HIGH);
    set_led(255,0,0,255,255,0,0,255);
}
void turn_L(char a, char b) //左转
{
    analogWrite(E1, a);
    digitalWrite(M1, LOW);
    analogWrite(E2, b);
    digitalWrite(M2, HIGH);
    set_led(255,255,0,0,255,0,0,255);
}
void turn_R(char a, char b) //右转
{
    analogWrite(E1, a);
    digitalWrite(M1, HIGH);
    analogWrite(E2, b);
    digitalWrite(M2, LOW);
    set_led(255,0,0,255,255,255,0,0);
}
/***************** LED *******************************************************/
void do_led() {
    if(readState != CMD_LED)
        return;
    else
        readState = 0;
   
    for (i = 0; i < LED_SIZE; i++) {
        //在8个LED中循环，对比LED的历史状态和命令，
        if(ledCmd[i] == ledState[i] || ledCmd[i] == -1)//如果命令不变，就下一个
            continue;
        if(ledState[i] == 0) {//如果历史命令是关闭，则加电，并写入亮度
            digitalWrite(ledPort[i], HIGH);
            analogWrite(ledPort[i], ledCmd[i]);
        }
        else if(ledCmd[i] == 0) {//如果新命令是关闭，则关闭
            digitalWrite(ledPort[i], LOW);
        }
        else {//到这里只可能是改变亮度的命令
            analogWrite(ledPort[i], ledCmd[i]);
        }
        ledState[i] = ledCmd[i];//将命令写入LED状态
        //Serial.print(ledState[i]);
        //Serial.print(";");
    }
}
void set_led(int v0,int v1,int v2,int v3,int v4,int v5,int v6,int v7){
    readState = CMD_LED;
    ledCmd[0] = v0;ledCmd[1] = v1;ledCmd[2] = v2;ledCmd[3] = v3;
    ledCmd[4] = v4;ledCmd[5] = v5;ledCmd[6] = v6;ledCmd[7] = v7;
}
/*************** sensor *********************************************/
void do_sensor() {
    //读取原始数据, 1代表3.2mv 1023=3300mV
    //计算出电压偏置值, 300mv=1g 加速度    0g读数为1.5v,换算成466, 参考ADXL330 Mannual
    //计算出电压偏置值 , 20mv= 10度/s 的角速度   静止读数为1.5v,换算成466, 参考IDG330 Mannual
    //加速度和角速度的公式是一样的，所以就整合成一条
    for (i = 0; i < SENSOR_SIZE; i++) {
        //sensorState[i] = analogRead(sensorPort[i]) - 466;
        sensorState[i] = analogRead(sensorPort[i]);//在上位机实现归零
    }
    for (i = 0; i < SENSOR_SIZE; i++) {
        Serial.print(sensorState[i]);
        Serial.print(";");
    }
    Serial.println();
}
/***************** setup-loop *************************************************/
void setup() {
    //init LEDs
    for (i = 0; i < LED_SIZE; i++) {
        pinMode(ledPort[i], OUTPUT);
        digitalWrite(ledPort[i], LOW);
    }
    //init motos
    for (i = 6; i <= 9; i++) {
        pinMode(i, OUTPUT);
    }
    Serial.begin(115200);
    analogReference(EXTERNAL); //设置模拟输入为外部参考3.3V
    //Serial.println("Ready");
}

void loop() {
    do_moto();//控制电机
    do_led();//通过m_state变量，控制LED灯
    do_sensor();//读取五轴传感器的值
    do_read();//读取命令,顺便延时至结束.
}