34 lines
788 B
C++
34 lines
788 B
C++
#include "MeMCore.h"
|
|
|
|
MeLineFollower lineFollower(PORT_1);
|
|
MeDCMotor motor1(9);
|
|
MeDCMotor motor2(10);
|
|
|
|
const uint8_t motorSpeed = 100;
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
void loop() {
|
|
uint8_t sensorValue = lineFollower.readSensors();
|
|
Serial.println(sensorValue);
|
|
//S1_IN_S2_IN
|
|
if (sensorValue == 0) { // Both sensors are on the black line
|
|
// Move forward
|
|
motor1.run(-motorSpeed);
|
|
motor2.run(motorSpeed);
|
|
} else if (sensorValue == 2) {
|
|
// Turn slightly right
|
|
motor1.run(-motorSpeed);
|
|
motor2.run(motorSpeed / 200);
|
|
} else if (sensorValue == 1) {
|
|
// Turn slightly left
|
|
motor1.run(-motorSpeed / 200);
|
|
motor2.run(motorSpeed);
|
|
} else { // Both sensors are on the white surface (lost the line)
|
|
motor1.stop();
|
|
motor2.stop();
|
|
}
|
|
}
|