60 lines
805 B
C++
60 lines
805 B
C++
#include "MeMCore.h"
|
|
|
|
MeDCMotor leftMotor(9);
|
|
MeDCMotor rightMotor(10);
|
|
MeUltrasonicSensor ultraSensor(PORT_3);
|
|
|
|
uint8_t motorSpeed = 100;
|
|
|
|
float dist;
|
|
|
|
void setup() {
|
|
|
|
Serial.begin(9600);
|
|
Serial.println("Initializing... \n");
|
|
leftMotor.run(0);
|
|
rightMotor.run(0);
|
|
|
|
}
|
|
|
|
void loop() {
|
|
|
|
dist = ultraSensor.distanceCm();
|
|
Serial.println(dist);
|
|
|
|
if (dist > 10){
|
|
goForward();
|
|
}
|
|
else if (ultraSensor.distanceCm() < 10){
|
|
|
|
goBackward();
|
|
delay(1000);
|
|
goLeft();
|
|
delay(1000);
|
|
}
|
|
}
|
|
|
|
void goForward(){
|
|
|
|
leftMotor.run(-motorSpeed);
|
|
rightMotor.run(motorSpeed);
|
|
delay(1000);
|
|
|
|
}
|
|
|
|
void goBackward(){
|
|
|
|
leftMotor.run(motorSpeed);
|
|
rightMotor.run(-motorSpeed);
|
|
delay(100);
|
|
|
|
}
|
|
|
|
void goLeft(){
|
|
|
|
leftMotor.run(motorSpeed);
|
|
rightMotor.run(motorSpeed);
|
|
delay(100);
|
|
|
|
}
|