1#include <AFMotor.h>
2
3AF_DCMotor motor1(1, MOTOR12_1KHZ);
4AF_DCMotor motor2(2, MOTOR12_1KHZ);
5AF_DCMotor motor3(3, MOTOR34_1KHZ);
6AF_DCMotor motor4(4, MOTOR34_1KHZ);
7
8char command;
9void setup()
10{
11 Serial.begin(9600);
12}
13void loop(){
14 if(Serial.available() > 0){
15 command = Serial.read();
16 Stop();
17
18
19 switch(command){
20 case 'F':
21 forward();
22 break;
23 case 'B':
24 back();
25 break;
26 case 'L':
27 left();
28 break;
29 case 'R':
30 right();
31 break;
32 }
33 }
34 void forward()
35{
36 motor1.setSpeed(255);
37 motor1.run(FORWARD);
38 motor2.setSpeed(255);
39 motor2.run(FORWARD);
40 motor3.setSpeed(255);
41 motor3.run(FORWARD);
42 motor4.setSpeed(255);
43 motor4.run(FORWARD);
44}
45void back()
46{
47 motor1.setSpeed(255);
48 motor1.run(BACKWARD);
49 motor2.setSpeed(255);
50 motor2.run(BACKWARD);
51 motor3.setSpeed(255);
52 motor3.run(BACKWARD);
53 motor4.setSpeed(255);
54 motor4.run(BACKWARD);
55}
56void right()
57{
58 motor1.setSpeed(255);
59 motor1.run(BACKWARD);
60 motor2.setSpeed(255);
61 motor2.run(BACKWARD);
62 motor3.setSpeed(255);
63 motor3.run(FORWARD);
64 motor4.setSpeed(255);
65 motor4.run(FORWARD);
66}
67void left()
68{
69 motor1.setSpeed(255);
70 motor1.run(FORWARD);
71 motor2.setSpeed(255);
72 motor2.run(FORWARD);
73 motor3.setSpeed(255);
74 motor3.run(BACKWARD);
75 motor4.setSpeed(255);
76 motor4.run(BACKWARD);
77}
78void Stop()
79{
80 motor1.setSpeed(0);
81 motor1.run(RELEASE);
82 motor2.setSpeed(0);
83 motor2.run(RELEASE);
84 motor3.setSpeed(0);
85 motor3.run(RELEASE);
86 motor4.setSpeed(0);
87 motor4.run(RELEASE);
88}
89