void loop() {
if (HC06.available()) {
char command = HC06.read();
Serial.println(command);
switch (command) {
case 'F':
moveForward();
break;
case 'B':
moveBackward();
break;
case 'L':
turnLeft();
break;
case 'R':
turnRight();
break;
case 'S':
StopMotors();
break;
}
}
}
void moveForward() {
Serial.println("Moving forward");
analogWrite(motor1_INL, 255);
analogWrite(motor1_INR, 0);
analogWrite(motor2_INL, 255);
analogWrite(motor2_INR, 0);
analogWrite(motor3_INL, 255);
analogWrite(motor3_INR, 0);
analogWrite(motor4_INL, 255);
analogWrite(motor4_INR, 0);
}
void moveBackward() {
Serial.println("Moving backward");
analogWrite(motor1_INL, 0);
analogWrite(motor1_INR, 255);
analogWrite(motor2_INL, 0);
analogWrite(motor2_INR, 255);
analogWrite(motor3_INL, 0);
analogWrite(motor3_INR, 255);
analogWrite(motor4_INL, 0);
analogWrite(motor4_INR, 255);
}
void turnLeft() {
Serial.println("Turning left");
analogWrite(motor1_INL, 255);
analogWrite(motor1_INR, 0);
analogWrite(motor3_INL, 255);
analogWrite(motor3_INR, 0);
analogWrite(motor2_INL, 0);
analogWrite(motor2_INR, 255);
analogWrite(motor4_INL, 0);
analogWrite(motor4_INR, 255);
}
void turnRight() {
Serial.println("Turning right");
analogWrite(motor1_INL, 0);
analogWrite(motor1_INR, 255);
analogWrite(motor3_INL, 0);
analogWrite(motor3_INR, 255);
analogWrite(motor2_INL, 255);
analogWrite(motor2_INR, 0);
analogWrite(motor4_INL, 255);
analogWrite(motor4_INR, 0);
}
void StopMotors() {
//Serial.println("Stopping motors");
analogWrite(motor1_INL, 0);
analogWrite(motor1_INR, 0);
analogWrite(motor2_INL, 0);
analogWrite(motor2_INR, 0);
analogWrite(motor3_INL, 0);
analogWrite(motor3_INR, 0);
analogWrite(motor4_INL, 0);
analogWrite(motor4_INR, 0);
}
Remember to do a forward declaration of all your functions before setup.
void moveForward();
void moveBackward();
void turnLeft();
void turnRight();
void StopMotors();
void setup(){.......
1 Like