RC wifi bil Robot kamera använder Arduino och openWRT (4 / 4 steg)
Steg 4: kod
1. #include
2. #include
3.
4. //UART PROTOKOLL / / /
5. #define UART_FLAG 0XFF
6. //Moto////////////////////////////////
7. / / PROTO: FLAGGA DEV RIKTNING TOM FLAGGA
8. #define MOTO 0X00
9. #define framåt 0X01 //MOTO kommandot
10. #define bakåt 0X02
11. #define SVÄNGVÄNSTER 0X03
12. #define SVÄNGHÖGER 0X04
13. #define CARSTOP 0X00
14. //Servo///////////////////////////////
15. / / PROTO: FLAGGA DEV SERVONUM POS FLAGGA
16. #define SERVO 0X01
17. //Moto hastighet / / /
18. / / PROTO: FLAGGA DEV MOTOSIDE HASTIGHET FLAGGA
19. #define MOTOSPEED 0X02
20. //////////////////////////////////////
21. int n = 1;
22. int flagCount = 0;
23. int tempData = 0;
24. int UARTReveived = 0;
25. int rxData [5].
26. //-------------------define motor----------------------------------------------//
27. AF_DCMotor motorL(3,MOTOR12_8KHZ); ansluta till M3
28. AF_DCMotor motorR(4,MOTOR12_8KHZ); ansluta till M4
29. int motor_speed = 200. [modifid] varvtal 150-200,---min:100; max: 255
30. int motor_delay = 400. [modifid] fördröjningstiden i steg
31.
32. //-------------------define servo----------------------------------------------//
33. servo hand_t_servo; skapa objekt för att styra en servo servo
34. servo hand_d_servo; skapa objekt för att styra en servo servo
35. int hand_t_pos = 90. //
36. int hand_d_pos = 90. //
37. int hand_delay = 1; [modifid] hastighet av hand
38.
39. //------------------main program-----------------------------------------------//
40. void loop()
41. {
42. if(Serial.available())
43. {
44. tempData = Serial.read();
45. delay(3);
46. if(tempData == UART_FLAG && flagCount < 2)
47. {
48. rxData [0] = tempData;
49. flagCount ++;
50.}
51. annat
52. {
53. rxData [n] = tempData;
54. n ++;
55.}
56. if(flagCount == 2)
57. {
58. rxData [4] == UART_FLAG;
59. UARTReveived = 1;
60. n = 1;
61. flagCount = 0;
62. tempData = 0;
63. Serial.flush();
64.}
65.}
66. if(UARTReveived == 1)
67. {
68. Serial.print("rxData:");
69. Serial.print(rxData[0]);
70. Serial.println(rxData[1]);
71. if(rxData[1] == MOTO)
72. {
73. switch(rxData[2])
74. {
75. mål framåt:
76. carGoFwd();
77. semester.
78. mål bakåt:
79. carGoBwd();
80. semester.
81. mål SVÄNGVÄNSTER:
82. carTurnL();
83. semester.
84. mål SVÄNGHÖGER:
85. carTurnR();
86. semester.
87. mål CARSTOP:
88. carStop();
89. semester.
90.}
91. UARTReveived = 0;
92.}
93. annars om (rxData [1] == SERVO)
94. {
95. servoSet(rxData[2], rxData[3]);
96. UARTReveived = 0;
97.}
98. annars om (rxData [1] == MOTOSPEED)
99. {
100. CHNSpeed(rxData[2], rxData[3]);
101. UARTReveived = 0;
102.}
103.}
104.}
105.
106. //CAR RÖRELSER
107. void carGoFwd()
108. {
109. motorL.setSpeed(motor_speed);
110. motorR.setSpeed(motor_speed);
111. motorL.run(FORWARD);
112. motorR.run(FORWARD);
113. Serial.print("forward");
114. delay(motor_delay);
115.}
116. void carGoBwd()
117. {
118. motorL.setSpeed(motor_speed);
119. motorR.setSpeed(motor_speed);
120. motorL.run(BACKWARD);
121. motorR.run(BACKWARD);
122. Serial.print("Backward");
123. delay(motor_delay);
124.}
125. void carTurnL()
126. {
127. motorL.setSpeed(motor_speed);
128. motorR.setSpeed(motor_speed);
129. motorL.run(BACKWARD);
130. motorR.run(FORWARD);
131. delay(motor_delay);
132. Serial.print("TurnL");
133.}
134. void carTurnR()
135. {
136. motorL.setSpeed(motor_speed);
137. motorR.setSpeed(motor_speed);
138. motorL.run(FORWARD);
139. motorR.run(BACKWARD);
140. delay(motor_delay);
141. Serial.print("TurnR");
142.}
143. void carStop()
144. {
145. b_motor_stop();
146. Serial.print("carStop");
147. delay(5);
148.}
149. //CAR HASTIGHET
150. void CHNSpeed (int wheelDIR, int wheelSpeed)
151. {
152. if(wheelDIR == 0X01) //LEFT hjul
153. {
154. motorL.setSpeed(wheelSpeed);
155.}
156. annars if(wheelDIR == 0X02) //RIGHT hjul
157. {
158. motorR.setSpeed(wheelSpeed);
159.}
160.}
161. //SERVO TUR
162. void servoSet (int servoNum, int pos)
163. {
164. if(pos > 180) pos = 160;
165. annars om (pos < 0) pos = 0;
166. switch(servoNum)
167. {
168. mål 0X07:
169. hand_t_servo.write(pos);
170. Serial.print("X");
171. Serial.print(pos);
172. semester.
173. mål 0X08:
174. hand_d_servo.write(pos);
175. Serial.print("Y");
176. Serial.print(pos);
177. semester.
178.}
179.}
180. void setup()
181. {
182. Serial.begin(9600);
183. b_motor_stop();
184. b_servo_ini();
185. / / delay(2000); väntetid
186. Serial.println("Hello! WiFi bil");
187.}
188.
189. void b_motor_stop() {
190. motorL.run(RELEASE);
191. motorR.run(RELEASE);
192.}
193.
194. void b_servo_ini() {
195. hand_t_servo.attach(9); tillmäter objektet servo servo på pin 9
196. hand_d_servo.attach(10); tillmäter objektet servo servo på stift 10
197. hand_t_servo.write(hand_t_pos);
198. hand_d_servo.write(hand_d_pos);
199.}