简单的电机控制实验代码
Arduino程序sample.ino/** 示例
By Apache([email protected]) 2012/11/30
*/
#include "proto.h"
#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 DEBUG(msg) Serial.println(msg)
static uint8_t waiting_head = 1;
static Head head;
void init_pin()
{
uint8_t outputs[] = {
PWM_ENA_PIN, PWM_IN1_PIN, PWM_IN2_PIN,
PWM_ENB_PIN, PWM_IN3_PIN, PWM_IN4_PIN };
uint8_t inputs[] = {};
uint8_t i, ocnt = sizeof(outputs), icnt = sizeof(inputs);
for(i = 0; i < ocnt; i++) {
pinMode(outputs, OUTPUT);
}
for(i = 0; i < icnt; i++) {
pinMode(inputs, INPUT);
}
}
void setup()
{
Serial.begin(9600);
Serial.flush();
init_pin();
}
inline void on_line_move(uint8_t *buffer)
{
LineMove linemove;
deserialize_LineMove(buffer, &linemove, 0);
SET_MOTOR(linemove.speed, linemove.dir, linemove.speed, linemove.dir);
}
void on_move(uint8_t *buffer)
{
Move move;
deserialize_Move(buffer, &move, 0);
SET_MOTOR(move.left_speed, move.left_dir, move.right_speed, move.right_dir);
}
inline void proto_process(uint32_t cmd, uint8_t *buffer)
{
switch(cmd) {
case LINE_MOVE_CMD:
on_line_move(buffer);
break;
case MOVE_CMD:
on_move(buffer);
break;
default:
break;
}
Serial.flush();
}
void recv_package()
{
int nbytes;
uint8_t buffer = "";
nbytes = Serial.available();
if(waiting_head) {
if(nbytes >= HEAD_SIZE) {
Serial.readBytes((char *)buffer, HEAD_SIZE);
deserialize_Head(buffer, &head, 0);
waiting_head = 0;
}
}
else {
if(nbytes >= head.body_len) {
Serial.readBytes((char *)buffer, head.body_len);
proto_process(head.cmd, buffer);
waiting_head = 1;
}
}
}
void loop()
{
recv_package();
}proto.h/*This head file is generated by code. Please do not edit!*/
#ifndef proto_H
#define proto_H
#ifdef __cplusplus
extern "C" {
#endif
#define HEAD_SIZE 3
#define MAX_BUFFER_SIZE 60
#define FORWARD_DIR 0
#define BACKWARD_DIR 1
#define STOP_DIR 2
#define PWM_ENA_PIN 3
#define PWM_ENB_PIN 5
#define PWM_IN1_PIN 8
#define PWM_IN2_PIN 7
#define PWM_IN3_PIN 12
#define PWM_IN4_PIN 11
#define LINE_MOVE_CMD 100
#define MOVE_CMD 101
#pragma pack(1)
typedef struct {
unsigned char dir;
unsigned char speed;
}LineMove;
extern inline int serialize_LineMove(LineMove *linemove, void *buffer, const int offset);
extern inline int deserialize_LineMove(void *buffer, LineMove *linemove, const int offset);
typedef struct {
unsigned char left_dir;
unsigned char left_speed;
unsigned char right_dir;
unsigned char right_speed;
}Move;
extern inline int serialize_Move(Move *move, void *buffer, const int offset);
extern inline int deserialize_Move(void *buffer, Move *move, const int offset);
typedef struct {
unsigned char cmd;
unsigned short body_len;
}Head;
extern inline int serialize_Head(Head *head, void *buffer, const int offset);
extern inline int deserialize_Head(void *buffer, Head *head, const int offset);
static int serialize(void *stru, void *buffer, int size, const int offset);
static int deserialize(void *buffer, void *stru, int size, const int offset);
#pragma pack()
#ifdef __cplusplus
}
#endif
#endif
proto.c/*This source file is generated by code. Please do not edit!*/
#include "proto.h"
static int serialize(void *stru, void *buffer, int size, const int offset)
{
char *p_stru = (char *)stru;
char *p_buffer = (char *)(buffer + offset);
while(*p_buffer++ = *p_stru++, size--);
return size + offset;
}
static int deserialize(void *buffer, void *stru, int size, const int offset)
{
char *p_stru = (char *)stru;
char *p_buffer = (char *)(buffer + offset);
while(*p_stru++ = *p_buffer++, size--);
return size + offset;
}
inline int serialize_LineMove(LineMove *linemove, void *buffer, const int offset)
{
return serialize(linemove, buffer, sizeof(*linemove), offset);
}
inline int deserialize_LineMove(void *buffer, LineMove *linemove, const int offset)
{
return deserialize(buffer, linemove, sizeof(*linemove), offset);
}
inline int serialize_Move(Move *move, void *buffer, const int offset)
{
return serialize(move, buffer, sizeof(*move), offset);
}
inline int deserialize_Move(void *buffer, Move *move, const int offset)
{
return deserialize(buffer, move, sizeof(*move), offset);
}
inline int serialize_Head(Head *head, void *buffer, const int offset)
{
return serialize(head, buffer, sizeof(*head), offset);
}
inline int deserialize_Head(void *buffer, Head *head, const int offset)
{
return deserialize(buffer, head, sizeof(*head), offset);
}
上位机程序
testproto.py#-*- coding: utf-8 -*-
#示例, 测试LineMove协议
#By Apache([email protected]) 2012/11/20
import sys
try:
import serial
except ImportError:
print "Import serial error"
sys.exit(1)
import proto
from time import sleep
COM = "COM3"
BAUND=9600
print u"正在打开串口..."
try:
ser = serial.Serial(COM, BAUND)
sleep(2)
except Exception, e:
print e
sys.exit(1)
print u"已打开串口 COM: %s, BAUND: %d" % (COM, BAUND)
class TestLineMove:
def __init__(self):
self.head = proto.Head(cmd=proto.LINE_MOVE_CMD)
self.body = proto.LineMove()
def __sendpkg(self):
body_buffer = self.body.serialize()
self.head.body_len = len(body_buffer)
head_buffer = self.head.serialize()
ser.write("%s%s" % (head_buffer, body_buffer))
def __line_move(self, msg, dir=0, speed=100, t=2):
self.body.dir = dir
self.body.speed = speed
print msg, u"%s 秒" % t
self.__sendpkg()
sleep(t)
def test(self):
print u"**LineMove协议测试开始"
for i in xrange(5):
self.__line_move(u"高速前进", dir=0, speed=255, t=0.5)
self.__line_move(u"高速后退", dir=1, speed=230, t=0.5)
self.__line_move(u"停止", dir=2, speed=0, t=0.5)
self.__line_move(u"低速前进", dir=0, speed=170, t=2)
self.__line_move(u"高速前进", dir=0, speed=255, t=3)
self.__line_move(u"低速后退", dir=1, speed=170, t=2)
self.__line_move(u"高速后退", dir=1, speed=230, t=3)
self.__line_move(u"停止", dir=2, speed=0, t=1)
print u"**LineMove协议测试结束"
if __name__ == "__main__":
TestLineMove().test()
ser.close()proto.py#-*- coding: utf-8 -*-
# This script is generated by code. Please do not edit!
from struct import pack, unpack
PACK_TYPE_DICT = {
"char": ("b", 1),
"uchar": ("B", 1),
"short": ("h", 2),
"ushort": ("H", 2),
"int": ("i", 4),
"uint": ("I", 4),
"long": ("l", 4),
"ulong": ("L", 4),
"longlong": ("q", 8),
"ulonglong": ("Q", 8),
"float": ("f", 4),
"double": ("d", 8),
}
class Codec:
@classmethod
def serialize(self, obj):
__buffer = []
for field_pktype in obj._field_pktype:
field, pktype = field_pktype, PACK_TYPE_DICT]
value = obj.__dict__.get(field)
if not value: value = 0
__buffer.append(pack(pktype, value))
return ''.join(__buffer)
@classmethod
def deserialize(self, obj, buffer):
offset = 0
for field_pktype in obj._field_pktype:
field = field_pktype
pktype, size = PACK_TYPE_DICT]
obj.__dict__ = unpack(pktype, buffer)
offset += size
return offset
class MetaClass(type):
def __init__(cls, name, bases, dict):
def _init(cls, **_dict):
for field, val in _dict.items():
setattr(cls, field, val)
def _serialize(cls, **args):
return dict["_codec"].serialize(cls, **args)
def _deserialize(cls, buffer, **args):
return dict["_codec"].deserialize(cls, buffer, **args)
for field, _ in dict["_fields"]:
setattr(cls, field, None)
setattr(cls, "__init__", _init)
setattr(cls, "_field_pktype", dict["_fields"])
setattr(cls, "serialize", _serialize)
setattr(cls, "deserialize", _deserialize)
delattr(cls, "_fields")
delattr(cls, "_codec")
super(MetaClass, cls).__init__(name, bases, dict)
HEAD_SIZE = 3
MAX_BUFFER_SIZE = 60
FORWARD_DIR = 0
BACKWARD_DIR = 1
STOP_DIR = 2
PWM_ENA_PIN = 3
PWM_ENB_PIN = 5
PWM_IN1_PIN = 8
PWM_IN2_PIN = 7
PWM_IN3_PIN = 12
PWM_IN4_PIN = 11
LINE_MOVE_CMD = 100
MOVE_CMD = 101
LineMove = MetaClass(
name="LineMove",
bases=(),
dict=dict(
_codec=Codec,
_fields=(
(u'dir', u'uchar'),
(u'speed', u'uchar')
)
)
)
Move = MetaClass(
name="Move",
bases=(),
dict=dict(
_codec=Codec,
_fields=(
(u'left_dir', u'uchar'),
(u'left_speed', u'uchar'),
(u'right_dir', u'uchar'),
(u'right_speed', u'uchar')
)
)
)
Head = MetaClass(
name="Head",
bases=(),
dict=dict(
_codec=Codec,
_fields=(
(u'cmd', u'uchar'),
(u'body_len', u'ushort')
)
)
)
好复杂，还自定义了协议{:3_57:}，看着很完善很强大，等看图片或视频介绍 也不给个电路 这种只是发代码，没有接线和电路的东西，很难看懂啊 可、能上图
页:
[1]