Building an Arduino Mecanum Wheels Robot

1#include <SoftwareSerial.h>
2#include <Wire.h>
3#include <AFMotor.h>
4SoftwareSerial Bluetooth(9, 10); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)
5AF_DCMotor motor(1, MOTOR12_64KHZ); 
6AF_DCMotor motor1(2, MOTOR12_64KHZ); 
7AF_DCMotor motor2(3, MOTOR12_64KHZ); 
8AF_DCMotor motor3(4, MOTOR12_64KHZ);
9int wheelSpeed=100;
10int dataIn, m;
11void setup() {
12  
13  Serial.begin(9600);
14  Bluetooth.begin(9600); 
15  Bluetooth.setTimeout(1);
16  delay(20);
17} 
18void loop() {
19  // Check for incoming data
20  if (Bluetooth.available() > 0) {
21    dataIn = Bluetooth.read();  // Read the data
22    Serial.println(dataIn);
23    if (dataIn == 0) {
24      m = 0;
25    }
26    if (dataIn == 1) {
27      m = 1;
28    }
29    if (dataIn == 2) {
30      m = 2;
31    }
32    if (dataIn == 3) {
33      m = 3;
34    }
35    if (dataIn == 4) {
36      m = 4;
37    }
38    if (dataIn == 5) {
39      m = 5;
40    }
41    if (dataIn == 6) {
42      m = 6;
43    }
44    if (dataIn == 7) {
45      m = 7;
46    }
47    if (dataIn == 8) {
48      m = 8;
49    }
50    if (dataIn == 9) {
51      m = 9;
52    }
53    if (dataIn == 10) {
54      m = 10;
55    }
56    if (dataIn == 11) {
57      m = 11;
58    }
59    if (dataIn == 12) {
60      m = 12;
61    }
62    if (dataIn == 14) {
63      m = 14;
64    }
65    // Set speed
66    if (dataIn >= 16) {
67      wheelSpeed = dataIn;
68      Serial.println(wheelSpeed);
69    }
70  }
71  if (m == 5) {
72    moveSidewaysLeft();
73  }
74  if (m == 4) {
75    moveSidewaysRight();
76  }
77  if (m == 2) {
78    moveForward();
79  }
80  if (m == 7) {
81    moveBackward();
82  }
83  if (m == 1) {
84    moveRightForward();
85  }
86  if (m == 3) {
87    moveLeftForward();
88  }
89  if (m == 6) {
90    moveRightBackward();
91  }
92  if (m == 8) {
93    moveLeftBackward();
94  }
95  if (m == 9) {
96    rotateLeft();
97  }
98  if (m == 10) {
99    rotateRight();
100  }
101  if (m == 0) {
102    stopMoving();
103  }
104  //Serial.println(dataIn);
105  // If button "SAVE" is pressed
106  if (m == 12) {
107
108  }
109  
110
111 
112}
113
114void moveForward() {
115
116  motor.setSpeed(wheelSpeed); 
117  motor1.setSpeed(wheelSpeed); 
118  motor2.setSpeed(wheelSpeed); 
119  motor3.setSpeed(wheelSpeed);
120  motor1.run(FORWARD);
121   motor.run(FORWARD); 
122   motor3.run(FORWARD);
123 motor2.run(FORWARD);
124}
125void moveBackward() {
126
127  motor.setSpeed(wheelSpeed); 
128  motor1.setSpeed(wheelSpeed); 
129  motor2.setSpeed(wheelSpeed); 
130  motor3.setSpeed(wheelSpeed);
131  motor1.run(BACKWARD);
132   motor.run(BACKWARD); 
133   motor3.run(BACKWARD);
134 motor2.run(BACKWARD);
135}
136void moveSidewaysRight() {
137
138  motor.setSpeed(wheelSpeed); 
139  motor1.setSpeed(wheelSpeed); 
140  motor2.setSpeed(wheelSpeed); 
141  motor3.setSpeed(wheelSpeed);
142  motor1.run(BACKWARD);
143   motor.run(FORWARD); 
144   motor3.run(BACKWARD);
145 motor2.run(FORWARD);
146}
147void moveSidewaysLeft() {
148
149  motor.setSpeed(wheelSpeed); 
150  motor1.setSpeed(wheelSpeed); 
151  motor2.setSpeed(wheelSpeed); 
152  motor3.setSpeed(wheelSpeed);
153  motor1.run(FORWARD);
154   motor.run(BACKWARD); 
155   motor3.run(FORWARD);
156 motor2.run(BACKWARD);
157}
158void rotateLeft() {
159
160  motor.setSpeed(wheelSpeed); 
161  motor1.setSpeed(wheelSpeed); 
162  motor2.setSpeed(wheelSpeed); 
163  motor3.setSpeed(wheelSpeed);
164motor1.run(FORWARD);
165   motor.run(FORWARD); 
166   motor3.run(BACKWARD);
167 motor2.run(BACKWARD);
168}
169void rotateRight() {
170
171  motor.setSpeed(wheelSpeed); 
172  motor1.setSpeed(wheelSpeed); 
173  motor2.setSpeed(wheelSpeed); 
174  motor3.setSpeed(wheelSpeed);
175  motor1.run(BACKWARD);
176   motor.run(BACKWARD); 
177   motor3.run(FORWARD);
178 motor2.run(FORWARD);
179}
180void moveRightForward() {
181
182  motor.setSpeed(wheelSpeed); 
183  motor1.setSpeed(0); 
184  motor2.setSpeed(wheelSpeed); 
185  motor3.setSpeed(0);
186  motor3.run(RELEASE);
187   motor1.run(RELEASE);
188   motor.run(FORWARD); 
189   motor2.run(FORWARD);
190}
191void moveRightBackward() {
192
193  motor1.setSpeed(wheelSpeed); 
194  motor.setSpeed(0); 
195  motor3.setSpeed(wheelSpeed); 
196  motor2.setSpeed(0);
197  motor.run(RELEASE);
198   motor2.run(RELEASE);
199   motor1.run(BACKWARD); 
200   motor3.run(BACKWARD);
201   
202}
203void moveLeftForward() {
204
205  motor.setSpeed(0); 
206  motor1.setSpeed(wheelSpeed); 
207  motor2.setSpeed(0); 
208  motor3.setSpeed(wheelSpeed);
209  motor2.run(RELEASE);
210   motor.run(RELEASE);
211   motor1.run(FORWARD); 
212   motor3.run(FORWARD);
213}
214void moveLeftBackward() {
215
216  motor.setSpeed(wheelSpeed); 
217  motor1.setSpeed(0); 
218  motor2.setSpeed(wheelSpeed); 
219  motor3.setSpeed(0);
220  motor3.run(RELEASE);
221   motor1.run(RELEASE);
222   motor.run(BACKWARD); 
223   motor2.run(BACKWARD);
224}
225void stopMoving() {
226  motor.run(RELEASE);
227   motor1.run(RELEASE); 
228   motor2.run(RELEASE);
229   motor3.run(RELEASE);
230   motor1.setSpeed(0);
231   motor2.setSpeed(0);
232   motor3.setSpeed(0);
233   motor.setSpeed(0);
234}