本帖最后由 apache 于 2012-12-23 22:24 编辑
前端时间发过一个帖子，初步介绍了下小车。这几天基本把程序部分写好了。这里贴出协议和小车源码，希望对和我一样喜欢DIY小车的同学有所帮助。
先说协议部分。协议使用了谷歌的Protocol Buffers进行定义。这样做有如下优缺点：
优点：
1、将数据处理与逻辑分离，使开发者不用关心繁琐的打解包过程，将更多注意力专注到业务逻辑上。
2、谷歌提供了通过proto定义文件生成多种语言的IO类的工具（protoc）
缺点：
1、略微优点浪费存储空间- -!
协议定义文件如下：
[pre lang="pb" line="1" file="Protocol.proto"]enum CTRL_CMD {
LINE_MOVE_CMD = 100;
MOVE_CMD = 101;
SERVO_CTRL_CMD = 102;
LED_CTRL_CMD = 103;
}
enum MOTOR_DIR {
FORWARD_DIR = 0;
BACKWARD_DIR = 1;
STOP_DIR = 2; // 已停止
}
enum SELECT_SERVO {
HORIZONTAL_SERVO = 0;
VERTICAL_SERVO = 1;
BOTH_SERVO = 2;
}
enum SET_LED {
LED_ON = 0;
LED_OFF = 1;
}
message Head {
required fixed32 cmd = 1;
required fixed32 body_len = 2;
}
message LineMove {
required int32 dir = 1;
required int32 speed = 2;
}
message Move {
required int32 left_dir = 1;
required int32 left_speed = 2;
required int32 right_dir = 3;
required int32 right_speed = 4;
}
message ServoCtrl {
required int32 select = 1;
required int32 angle = 2;
}
message LedCtrl {
required int32 flag = 1;
}[/code]
分别通过protoc.jar和protoc.exe生成Arduino使用的c文件和上位机程序使用的Python IO类
下面是wificar.h和wificar.c的源码
[pre lang="C" line="1" file="wificar.h"]#ifndef WIFI_CAR
#define WIFI_CAR
#include "Arduino.h"
extern "C" {
#include "protocol.h"
}
typedef struct Head Head;
typedef struct LineMove LineMove;
typedef struct Move Move;
#define YES 1
#define NO 0
#define HEAD_SIZE 11
#define MAX_BUFFER_SIZE 64
#define PWM_ENA_PIN 3
#define PWM_ENB_PIN 5
#define PWM_IN1_PIN 7
#define PWM_IN2_PIN 8
#define PWM_IN3_PIN 11
#define PWM_IN4_PIN 12
#define WAVE_TRIG_PIN 4
#define WAVE_ECHO_PIN 2
#define LED_PIN 13
#define V_SERVO_PIN 9
#define H_SERVO_PIN 10
#define V_REF_ANGLE 100
#define H_REF_ANGLE 86
#define MAX_HSERVO_ANGLE (180 - V_REF_ANGLE)
#define MAX_VSERVO_ANGLE (180 - H_REF_ANGLE)
#define MIN_HSERVO_ANGLE (-V_REF_ANGLE)
#define MIN_VSERVO_ANGLE (-H_REF_ANGLE)
#define SET_PINS_MODE(PINS, MODE) \
do { \
for(int i = 0; i < sizeof(PINS); i++) \
pinMode(PINS, MODE); \
}while(0)
#define SET_LEFT_PWM(SPEED) analogWrite(PWM_ENB_PIN, (SPEED))
#define SET_RIGHT_PWM(SPEED) analogWrite(PWM_ENA_PIN, (SPEED))
#define SET_DIR(DIR, XPIN, YPIN) \
do { \
if(DIR != _STOP_DIR) {\
digitalWrite(XPIN, DIR); \
digitalWrite(YPIN, !DIR); \
}else {\
digitalWrite(XPIN, 0); \
digitalWrite(YPIN, 0); \
} \
}while(0)
#define SET_LEFT_DIR(DIR) SET_DIR(DIR, PWM_IN3_PIN, PWM_IN4_PIN)
#define SET_RIGHT_DIR(DIR) SET_DIR(DIR, PWM_IN1_PIN, PWM_IN2_PIN)
#define SET_MOTOR(LSPD, LDIR, RSPD, RDIR) \
do { \
SET_LEFT_PWM((LSPD)); \
SET_RIGHT_PWM((RSPD)); \
SET_LEFT_DIR((LDIR)); \
SET_RIGHT_DIR((RDIR)); \
}while(0)
#define ROTATE_HSERVO(ANGLE) hservo.write(H_REF_ANGLE + (ANGLE))
#define ROTATE_VSERVO(ANGLE) vservo.write(V_REF_ANGLE + (ANGLE))
#define DEBUG
#ifdef DEBUG
#define DBG(msg) Serial.println(msg)
#else
#define DBG(msg)
#endif
#endif
[/code]
[pre lang="arduino" line="1" file="wificar.ino"]#include <Servo.h>
#include "wificar.h"
static Servo hservo, vservo;
static int g_hcur_angle = 0, g_vcur_angle = 0;
static uint8_t g_waiting_head = YES;
static Head g_head;
void init_pin()
{
uint8_t opins[] = {
PWM_ENA_PIN, PWM_IN1_PIN, PWM_IN2_PIN,
PWM_ENB_PIN, PWM_IN3_PIN, PWM_IN4_PIN,
WAVE_TRIG_PIN, LED_PIN
};
uint8_t ipins[] = {
WAVE_ECHO_PIN
};
SET_PINS_MODE(opins, OUTPUT);
SET_PINS_MODE(ipins, INPUT);
pinMode(WAVE_TRIG_PIN, OUTPUT);
pinMode(WAVE_ECHO_PIN, INPUT);
}
void init_servo()
{
vservo.attach(V_SERVO_PIN);
hservo.attach(H_SERVO_PIN);
vservo.write(V_REF_ANGLE);
hservo.write(H_REF_ANGLE);
}
void setup()
{
Serial.begin(9600);
Serial.flush();
init_pin();
init_servo();
}
inline void on_line_move(uint8_t *buffer)
{
LineMove lnmove;
LineMove_read_delimited_from(buffer, &lnmove, 0);
SET_MOTOR(lnmove._speed, lnmove._dir, lnmove._speed, lnmove._dir);
}
inline void on_move(uint8_t *buffer)
{
Move move;
Move_read_delimited_from(buffer, &move, 0);
SET_MOTOR(move._left_speed, move._left_dir, move._right_speed, move._right_dir);
}
inline void on_servo_ctrl(uint8_t *buffer)
{
ServoCtrl servo_ctrl;
ServoCtrl_read_delimited_from(buffer, &servo_ctrl, 0);
if(servo_ctrl._select == _HORIZONTAL_SERVO || servo_ctrl._select == _BOTH_SERVO)
{
g_hcur_angle += servo_ctrl._angle;
if(g_hcur_angle > MAX_HSERVO_ANGLE || g_hcur_angle < MIN_HSERVO_ANGLE)
g_hcur_angle -= servo_ctrl._angle;
ROTATE_HSERVO(g_hcur_angle);
}
if(servo_ctrl._select == _VERTICAL_SERVO || servo_ctrl._select == _BOTH_SERVO)
{
g_vcur_angle += servo_ctrl._angle;
if(g_vcur_angle > MAX_VSERVO_ANGLE || g_vcur_angle < MIN_VSERVO_ANGLE)
g_vcur_angle -= servo_ctrl._angle;
ROTATE_VSERVO(g_vcur_angle);
}
}
inline void on_led_ctrl(uint8_t *buffer)
{
LedCtrl led_ctrl;
LedCtrl_read_delimited_from(buffer, &led_ctrl, 0);
if(led_ctrl._flag == _LED_ON)
{
digitalWrite(LED_PIN, HIGH);
}
else
{
digitalWrite(LED_PIN, LOW);
}
}
inline void proto_master(uint8_t *buffer)
{
switch(g_head._cmd)
{
case _LINE_MOVE_CMD:
on_line_move(buffer);
break;
case _MOVE_CMD:
on_move(buffer);
break;
case _SERVO_CTRL_CMD:
on_servo_ctrl(buffer);
break;
case _LED_CTRL_CMD:
on_led_ctrl(buffer);
break;
default:
break;
}
Serial.flush();
}
inline void recv_package()
{
int nbytes;
uint8_t buffer[MAX_BUFFER_SIZE];
nbytes = Serial.available();
if(g_waiting_head == YES)
{
if(nbytes >= HEAD_SIZE)
{
Serial.readBytes((char *)buffer, HEAD_SIZE);
Head_read_delimited_from(buffer, &g_head, 0);
g_waiting_head = NO;
}
}
else
{
if(nbytes >= g_head._body_len)
{
Serial.readBytes((char *)buffer, g_head._body_len);
proto_master(buffer);
g_waiting_head = YES;
}
}
}
void loop()
{
recv_package();
}
[/code]
完整代码见附件：
上位机程序使用Python+PyQt4+Python OpenCv开发，因为通过Py2exe转成exe程序后比较大，这里暂时不发（后续贴出网盘下载路径）
|
发表于 2012-12-23 22:22:20
