81 lines
1.3 KiB
C++
81 lines
1.3 KiB
C++
#include "MeMCore.h"
|
|
|
|
MeDCMotor leftMotor(9);
|
|
MeDCMotor rightMotor(10);
|
|
MeLineFollower lineFinder(PORT_2); /* Line Finder module can only be connected to PORT_3, PORT_4, PORT_5, PORT_6 of base shield. */
|
|
|
|
uint8_t motorSpeed = 50;
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
int sensorState = lineFinder.readSensors();
|
|
goForward();
|
|
switch(sensorState)
|
|
{
|
|
case S1_IN_S2_IN:
|
|
Serial.println("Sensor 1 and 2 are inside of black line");
|
|
goForward();
|
|
break;
|
|
case S1_IN_S2_OUT:
|
|
Serial.println("Sensor 2 is outside of black line");
|
|
goLeft();
|
|
goForward();
|
|
break;
|
|
case S1_OUT_S2_IN:
|
|
Serial.println("Sensor 1 is outside of black line");
|
|
goRight();
|
|
goForward();
|
|
break;
|
|
case S1_OUT_S2_OUT:
|
|
Serial.println("Sensor 1 and 2 are outside of black line");
|
|
stop();
|
|
break;
|
|
default: break;
|
|
}
|
|
delay(200);
|
|
}
|
|
|
|
void goForward(){
|
|
|
|
leftMotor.run(-motorSpeed);
|
|
rightMotor.run(motorSpeed);
|
|
delay(1000);
|
|
|
|
}
|
|
|
|
void goBackward(){
|
|
|
|
leftMotor.run(motorSpeed);
|
|
rightMotor.run(-motorSpeed);
|
|
delay(1000);
|
|
|
|
}
|
|
|
|
void goLeft(){
|
|
|
|
leftMotor.run(motorSpeed);
|
|
rightMotor.run(motorSpeed);
|
|
delay(1000);
|
|
|
|
}
|
|
|
|
void goRight(){
|
|
|
|
leftMotor.run(-motorSpeed);
|
|
rightMotor.run(-motorSpeed);
|
|
delay(1000);
|
|
|
|
}
|
|
|
|
void stop(){
|
|
|
|
leftMotor.stop();
|
|
rightMotor.stop();
|
|
delay(1000);
|
|
|
|
} |