91 lines
1.8 KiB
C++
91 lines
1.8 KiB
C++
#include <MeMCore.h>
|
|
|
|
|
|
|
|
MeDCMotor motorL(M1); //positive go backward
|
|
|
|
MeDCMotor motorR(M2); //positive go forward
|
|
|
|
MeUltrasonicSensor ultraSensor(PORT_3);
|
|
|
|
uint8_t motorSpeed = 255;
|
|
bool turning = false;
|
|
unsigned long turnTimer = 0;
|
|
int ranMin = 200;//in ms
|
|
int ranMax = 2000;
|
|
unsigned long turnTimerDuration = random(ranMin, ranMax);
|
|
bool directionLeft = true;
|
|
int MinDistance = 20;//in cm
|
|
|
|
/*
|
|
* if turning, keep turning until turn timer is over and kill turning
|
|
* if not turning, check sensor and carry on moving forward
|
|
* if sensor within threshold, set turn timer random, direction random, set turning
|
|
*
|
|
*
|
|
*/
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(9600);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
if (turning){
|
|
//timercheck
|
|
if(millis()- turnTimer >= turnTimerDuration){
|
|
//reset timer
|
|
turning = false;
|
|
motorL.run(-motorSpeed);
|
|
motorR.run(motorSpeed);
|
|
}else{
|
|
//keep on turning
|
|
}
|
|
}else{
|
|
//not turning
|
|
if(ultraSensor.distanceCm() < MinDistance)//sensorcheck
|
|
{
|
|
turnTimerDuration = random(ranMin, ranMax);
|
|
directionLeft = random(2);
|
|
|
|
if(directionLeft){
|
|
motorL.run(motorSpeed);
|
|
motorR.run(motorSpeed);
|
|
}else{
|
|
motorL.run(-motorSpeed);
|
|
motorR.run(-motorSpeed);
|
|
}
|
|
|
|
turning = true;
|
|
turnTimer = millis();
|
|
}
|
|
|
|
|
|
}
|
|
|
|
/*
|
|
Serial.print("Distance : ");
|
|
Serial.print(ultraSensor.distanceCm() );
|
|
Serial.println(" cm");
|
|
//delay(100);//docs say at least that timer
|
|
*/
|
|
/*motorL.run(motorSpeed); // value: between -255 and 255.
|
|
/motorR.run(motorSpeed); // value: between -255 and 255.
|
|
|
|
delay(2000);
|
|
motorL.stop();
|
|
motorR.stop();
|
|
|
|
delay(100);
|
|
motorL.run(-motorSpeed);
|
|
motorR.run(-motorSpeed);
|
|
|
|
delay(2000);
|
|
motorL.stop();
|
|
motorR.stop();
|
|
|
|
delay(2000);
|
|
*/
|
|
}
|