47 lines
690 B
C++
47 lines
690 B
C++
|
|
#include <MeMCore.h>
|
|
#include <MeUltrasonicSensor.h>
|
|
// The two motor objects
|
|
MeDCMotor motor1(9);
|
|
MeDCMotor motor2(10);
|
|
MeUltrasonicSensor sensor1(3);
|
|
uint8_t Lspeed = 200;
|
|
uint8_t Rspeed = 200;
|
|
void setup()
|
|
{
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
|
|
devant();
|
|
int distance = sensor1.distanceCm();
|
|
Serial.println(distance);
|
|
if (distance < 20){
|
|
turnright();
|
|
delay(600);
|
|
}
|
|
|
|
}
|
|
void turnright(){
|
|
motor1.run(Lspeed);
|
|
motor2.run(Rspeed);
|
|
}
|
|
void turnleft(){
|
|
motor1.run(-Lspeed);
|
|
motor2.run(-Rspeed);
|
|
}
|
|
void devant(){
|
|
motor1.run(-Lspeed);
|
|
motor2.run(Rspeed);
|
|
}
|
|
void derriere(){
|
|
motor1.run(Lspeed);
|
|
motor2.run(-Rspeed);
|
|
}
|
|
void stop(){
|
|
motor1.run(0);
|
|
motor2.run(0);
|
|
}
|