简单的电机控制实验代码

apache

Arduino程序
sample.ino

1. /** 示例
   
2.     By Apache([email protected]) 2012/11/30
   
3. */
   
4. #include "proto.h"
   
5. 
   
6. #define SET_LEFT_PWM(SPEED) analogWrite(PWM_ENB_PIN, (SPEED))
   
7. #define SET_RIGHT_PWM(SPEED) analogWrite(PWM_ENA_PIN, (SPEED))
   
8. #define SET_DIR(DIR, XPIN, YPIN) \
   
9.     do { \
   
10.         if(DIR != STOP_DIR) {\
    
11.             digitalWrite(XPIN, DIR); \
    
12.             digitalWrite(YPIN, !DIR); \
    
13.         }else {\
    
14.             digitalWrite(XPIN, 0); \
    
15.             digitalWrite(YPIN, 0); \
    
16.         } \
    
17.     }while(0)
    
18.    
    
19. #define SET_LEFT_DIR(DIR) SET_DIR(DIR, PWM_IN3_PIN, PWM_IN4_PIN)
    
20. #define SET_RIGHT_DIR(DIR) SET_DIR(DIR, PWM_IN1_PIN, PWM_IN2_PIN)
    
21. 
    
22. #define SET_MOTOR(LSPD, LDIR, RSPD, RDIR) \
    
23.     do { \
    
24.         SET_LEFT_PWM((LSPD)); \
    
25.         SET_RIGHT_PWM((RSPD)); \
    
26.         SET_LEFT_DIR((LDIR)); \
    
27.         SET_RIGHT_DIR((RDIR)); \
    
28.     }while(0)
    
29. 
    
30. #define DEBUG(msg) Serial.println(msg)
    
31. 
    
32. static uint8_t waiting_head = 1;
    
33. static Head head;
    
34. 
    
35. void init_pin()
    
36. {
    
37.     uint8_t outputs[] = {
    
38.         PWM_ENA_PIN, PWM_IN1_PIN, PWM_IN2_PIN,
    
39.         PWM_ENB_PIN, PWM_IN3_PIN, PWM_IN4_PIN };
    
40.     uint8_t inputs[] = {};
    
41.     uint8_t i, ocnt = sizeof(outputs), icnt = sizeof(inputs);
    
42.    
    
43.     for(i = 0; i < ocnt; i++) {
    
44.         pinMode(outputs[i], OUTPUT);
    
45.     }
    
46.     for(i = 0; i < icnt; i++) {
    
47.         pinMode(inputs[i], INPUT);
    
48.     }
    
49. }
    
50. void setup()
    
51. {
    
52.     Serial.begin(9600);
    
53.     Serial.flush();
    
54.     init_pin();
    
55. }
    
56. 
    
57. inline void on_line_move(uint8_t *buffer)
    
58. {
    
59.     LineMove linemove;
    
60.    
    
61.     deserialize_LineMove(buffer, &linemove, 0);
    
62.     SET_MOTOR(linemove.speed, linemove.dir, linemove.speed, linemove.dir);
    
63. }
    
64. 
    
65. void on_move(uint8_t *buffer)
    
66. {
    
67.     Move move;
    
68.    
    
69.     deserialize_Move(buffer, &move, 0);
    
70.     SET_MOTOR(move.left_speed, move.left_dir, move.right_speed, move.right_dir);
    
71. }
    
72. 
    
73. inline void proto_process(uint32_t cmd, uint8_t *buffer)
    
74. {
    
75.     switch(cmd) {
    
76.     case LINE_MOVE_CMD:
    
77.         on_line_move(buffer);
    
78.         break;
    
79.     case MOVE_CMD:
    
80.         on_move(buffer);
    
81.         break;
    
82.     default:
    
83.         break;
    
84.     }
    
85.     Serial.flush();
    
86. }
    
87. void recv_package()
    
88. {
    
89.     int nbytes;
    
90.     uint8_t buffer[MAX_BUFFER_SIZE] = "";
    
91.    
    
92.     nbytes = Serial.available();
    
93.     if(waiting_head) {
    
94.         if(nbytes >= HEAD_SIZE) {
    
95.             Serial.readBytes((char *)buffer, HEAD_SIZE);
    
96.             deserialize_Head(buffer, &head, 0);
    
97.             waiting_head = 0;
    
98.         }
    
99.     }
    
100.     else {
     
101.         if(nbytes >= head.body_len) {
     
102.             Serial.readBytes((char *)buffer, head.body_len);
     
103.             proto_process(head.cmd, buffer);
     
104.             waiting_head = 1;
     
105.         }
     
106.     }
     
107. }
     
108. void loop()
     
109. {
     
110.     recv_package();
     
111. }

复制代码

proto.h

1. /*This head file is generated by code. Please do not edit!*/
   
2. #ifndef proto_H
   
3. #define proto_H
   
4. #ifdef __cplusplus
   
5. extern "C" {
   
6. #endif
   
7. 
   
8. #define HEAD_SIZE 3
   
9. #define MAX_BUFFER_SIZE 60
   
10. #define FORWARD_DIR 0
    
11. #define BACKWARD_DIR 1
    
12. #define STOP_DIR 2
    
13. #define PWM_ENA_PIN 3
    
14. #define PWM_ENB_PIN 5
    
15. #define PWM_IN1_PIN 8
    
16. #define PWM_IN2_PIN 7
    
17. #define PWM_IN3_PIN 12
    
18. #define PWM_IN4_PIN 11
    
19. #define LINE_MOVE_CMD 100
    
20. #define MOVE_CMD 101
    
21. 
    
22. #pragma pack(1)
    
23. typedef struct {
    
24.     unsigned char dir;
    
25.     unsigned char speed;
    
26. }LineMove;
    
27. extern inline int serialize_LineMove(LineMove *linemove, void *buffer, const int offset);
    
28. extern inline int deserialize_LineMove(void *buffer, LineMove *linemove, const int offset);
    
29. 
    
30. typedef struct {
    
31.     unsigned char left_dir;
    
32.     unsigned char left_speed;
    
33.     unsigned char right_dir;
    
34.     unsigned char right_speed;
    
35. }Move;
    
36. extern inline int serialize_Move(Move *move, void *buffer, const int offset);
    
37. extern inline int deserialize_Move(void *buffer, Move *move, const int offset);
    
38. 
    
39. typedef struct {
    
40.     unsigned char cmd;
    
41.     unsigned short body_len;
    
42. }Head;
    
43. extern inline int serialize_Head(Head *head, void *buffer, const int offset);
    
44. extern inline int deserialize_Head(void *buffer, Head *head, const int offset);
    
45. 
    
46. static int serialize(void *stru, void *buffer, int size, const int offset);
    
47. static int deserialize(void *buffer, void *stru, int size, const int offset);
    
48. 
    
49. #pragma pack()
    
50. #ifdef __cplusplus
    
51. }
    
52. #endif
    
53. #endif
    

复制代码

proto.c

1. /*This source file is generated by code. Please do not edit!*/
   
2. #include "proto.h"
   
3. 
   
4. static int serialize(void *stru, void *buffer, int size, const int offset)
   
5. {
   
6.     char *p_stru = (char *)stru;
   
7.     char *p_buffer = (char *)(buffer + offset);
   
8.    
   
9.     while(*p_buffer++ = *p_stru++, size--);
   
10.    
    
11.     return size + offset;
    
12. }
    
13. static int deserialize(void *buffer, void *stru, int size, const int offset)
    
14. {
    
15.     char *p_stru = (char *)stru;
    
16.     char *p_buffer = (char *)(buffer + offset);
    
17. 
    
18.     while(*p_stru++ = *p_buffer++, size--);
    
19.    
    
20.     return size + offset;
    
21. }
    
22. 
    
23. inline int serialize_LineMove(LineMove *linemove, void *buffer, const int offset)
    
24. {
    
25.     return serialize(linemove, buffer, sizeof(*linemove), offset);
    
26. }
    
27. 
    
28. inline int deserialize_LineMove(void *buffer, LineMove *linemove, const int offset)
    
29. {
    
30.     return deserialize(buffer, linemove, sizeof(*linemove), offset);
    
31. }
    
32. 
    
33. inline int serialize_Move(Move *move, void *buffer, const int offset)
    
34. {
    
35.     return serialize(move, buffer, sizeof(*move), offset);
    
36. }
    
37. 
    
38. inline int deserialize_Move(void *buffer, Move *move, const int offset)
    
39. {
    
40.     return deserialize(buffer, move, sizeof(*move), offset);
    
41. }
    
42. 
    
43. inline int serialize_Head(Head *head, void *buffer, const int offset)
    
44. {
    
45.     return serialize(head, buffer, sizeof(*head), offset);
    
46. }
    
47. 
    
48. inline int deserialize_Head(void *buffer, Head *head, const int offset)
    
49. {
    
50.     return deserialize(buffer, head, sizeof(*head), offset);
    
51. }
    

复制代码

上位机程序
testproto.py

1. #-*- coding: utf-8 -*-
   
2. #示例, 测试LineMove协议
   
3. #By Apache([email protected]) 2012/11/20
   
4. 
   
5. import sys
   
6. try:
   
7.     import serial
   
8. except ImportError:
   
9.     print "Import serial error"
   
10.     sys.exit(1)
    
11. import proto
    
12. from time import sleep
    
13. 
    
14. COM = "COM3"
    
15. BAUND=9600
    
16. 
    
17. print u"正在打开串口..."
    
18. try:
    
19.     ser = serial.Serial(COM, BAUND)
    
20.     sleep(2)
    
21. except Exception, e:
    
22.     print e
    
23.     sys.exit(1)
    
24. print u"已打开串口 COM: %s, BAUND: %d" % (COM, BAUND)
    
25. 
    
26. class TestLineMove:
    
27.     def __init__(self):
    
28.         self.head = proto.Head(cmd=proto.LINE_MOVE_CMD)
    
29.         self.body = proto.LineMove()
    
30.    
    
31.     def __sendpkg(self):
    
32.         body_buffer = self.body.serialize()
    
33.         self.head.body_len = len(body_buffer)
    
34.         head_buffer = self.head.serialize()
    
35.         ser.write("%s%s" % (head_buffer, body_buffer))
    
36.         
    
37.     def __line_move(self, msg, dir=0, speed=100, t=2):
    
38.         self.body.dir = dir
    
39.         self.body.speed = speed
    
40.         print msg, u"%s 秒" % t
    
41.         self.__sendpkg()
    
42.         sleep(t)
    
43.         
    
44.     def test(self):
    
45.         print u"**LineMove协议测试开始"
    
46.         for i in xrange(5):
    
47.             self.__line_move(u"高速前进", dir=0, speed=255, t=0.5)
    
48.             self.__line_move(u"高速后退", dir=1, speed=230, t=0.5)
    
49.             self.__line_move(u"停止", dir=2, speed=0, t=0.5)
    
50.             self.__line_move(u"低速前进", dir=0, speed=170, t=2)
    
51.             self.__line_move(u"高速前进", dir=0, speed=255, t=3)
    
52.             self.__line_move(u"低速后退", dir=1, speed=170, t=2)
    
53.             self.__line_move(u"高速后退", dir=1, speed=230, t=3)
    
54.             self.__line_move(u"停止", dir=2, speed=0, t=1)
    
55.         print u"**LineMove协议测试结束"
    
56. if __name__ == "__main__":
    
57.     TestLineMove().test()
    
58.     ser.close()

复制代码

proto.py

1. #-*- coding: utf-8 -*-
   
2. # This script is generated by code. Please do not edit!
   
3. 
   
4. from struct import pack, unpack
   
5. 
   
6. PACK_TYPE_DICT = {
   
7.     "char": ("b", 1),
   
8.     "uchar": ("B", 1),
   
9.     "short": ("h", 2),
   
10.     "ushort": ("H", 2),
    
11.     "int": ("i", 4),
    
12.     "uint": ("I", 4),
    
13.     "long": ("l", 4),
    
14.     "ulong": ("L", 4),
    
15.     "longlong": ("q", 8),
    
16.     "ulonglong": ("Q", 8),
    
17.     "float": ("f", 4),
    
18.     "double": ("d", 8),
    
19. }
    
20. 
    
21. class Codec:
    
22.     @classmethod
    
23.     def serialize(self, obj):
    
24.         __buffer = []
    
25.         for field_pktype in obj._field_pktype:
    
26.             field, pktype = field_pktype[0], PACK_TYPE_DICT[field_pktype[1]][0]
    
27.             value = obj.__dict__.get(field)
    
28.             if not value: value = 0
    
29.             __buffer.append(pack(pktype, value))
    
30.         
    
31.         return ''.join(__buffer)
    
32. 
    
33.     @classmethod
    
34.     def deserialize(self, obj, buffer):
    
35.         offset = 0
    
36.         for field_pktype in obj._field_pktype:
    
37.             field = field_pktype[0]
    
38.             pktype, size = PACK_TYPE_DICT[field_pktype[1]]
    
39.             obj.__dict__[field] = unpack(pktype, buffer[offset: offset + size])[0]
    
40.             offset += size
    
41.             
    
42.         return offset
    
43. 
    
44. 
    
45. class MetaClass(type):
    
46.     def __init__(cls, name, bases, dict):
    
47.         def _init(cls, **_dict):
    
48.             for field, val in _dict.items():
    
49.                 setattr(cls, field, val)
    
50.         def _serialize(cls, **args):
    
51.             return dict["_codec"].serialize(cls, **args)
    
52.         def _deserialize(cls, buffer, **args):
    
53.             return dict["_codec"].deserialize(cls, buffer, **args)
    
54.         
    
55.         for field, _ in dict["_fields"]:
    
56.             setattr(cls, field, None)
    
57.             
    
58.         setattr(cls, "__init__", _init)
    
59.         setattr(cls, "_field_pktype", dict["_fields"])
    
60.         setattr(cls, "serialize", _serialize)
    
61.         setattr(cls, "deserialize", _deserialize)
    
62.         
    
63.         delattr(cls, "_fields")
    
64.         delattr(cls, "_codec")
    
65.         
    
66.         super(MetaClass, cls).__init__(name, bases, dict)
    
67. 
    
68. HEAD_SIZE = 3
    
69. MAX_BUFFER_SIZE = 60
    
70. FORWARD_DIR = 0
    
71. BACKWARD_DIR = 1
    
72. STOP_DIR = 2
    
73. PWM_ENA_PIN = 3
    
74. PWM_ENB_PIN = 5
    
75. PWM_IN1_PIN = 8
    
76. PWM_IN2_PIN = 7
    
77. PWM_IN3_PIN = 12
    
78. PWM_IN4_PIN = 11
    
79. LINE_MOVE_CMD = 100
    
80. MOVE_CMD = 101
    
81. 
    
82. LineMove = MetaClass(
    
83. name="LineMove",
    
84. bases=(),
    
85. dict=dict(
    
86.     _codec=Codec,
    
87.     _fields=(
    
88.             (u'dir', u'uchar'),
    
89.             (u'speed', u'uchar')
    
90.         )
    
91. )
    
92. )
    
93. 
    
94. Move = MetaClass(
    
95. name="Move",
    
96. bases=(),
    
97. dict=dict(
    
98.     _codec=Codec,
    
99.     _fields=(
    
100.             (u'left_dir', u'uchar'),
     
101.             (u'left_speed', u'uchar'),
     
102.             (u'right_dir', u'uchar'),
     
103.             (u'right_speed', u'uchar')
     
104.         )
     
105. )
     
106. )
     
107. 
     
108. Head = MetaClass(
     
109. name="Head",
     
110. bases=(),
     
111. dict=dict(
     
112.     _codec=Codec,
     
113.     _fields=(
     
114.             (u'cmd', u'uchar'),
     
115.             (u'body_len', u'ushort')
     
116.         )
     
117. )
     
118. )
     

复制代码

幻生幻灭

好复杂，还自定义了协议{:3_57:}，看着很完善很强大，等看图片或视频介绍

太行摄狼

也不给个电路

追梦男儿

这种只是发代码，没有接线和电路的东西，很难看懂啊

宋征宇

可、能上图