mirror of
https://github.com/KevinMidboe/Arduino.git
synced 2025-10-29 09:30:12 +00:00
129 lines
2.5 KiB
C++
129 lines
2.5 KiB
C++
/*
|
|
Drive forward and turn left or right when border is detected
|
|
- Only reflectionsensor 0 and 5 are used.
|
|
*/
|
|
#include <ZumoMotors.h>
|
|
#include <Pushbutton.h>
|
|
#include <QTRSensors.h>
|
|
#include <ZumoReflectanceSensorArray.h>
|
|
#include <NewPing.h>
|
|
|
|
#define LED 13
|
|
|
|
// this might need to be tuned for different lighting conditions, surfaces, etc.
|
|
#define lightThreshold 1800 //
|
|
|
|
#define reverseSpeed 400
|
|
#define turnSpeed 400
|
|
#define forwardSpeed 400
|
|
#define revDuration 300
|
|
#define turnDuration 200
|
|
#define triggerPin 3
|
|
#define echoPin 2
|
|
#define maxDistance 200
|
|
|
|
ZumoMotors motors;
|
|
Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12
|
|
#define NUM_SENSORS 6
|
|
unsigned int sensor_values[NUM_SENSORS];
|
|
ZumoReflectanceSensorArray sensors;
|
|
|
|
NewPing sonar(triggerPin, echoPin, maxDistance);
|
|
|
|
unsigned int pingSpeed = 100;
|
|
unsigned long pingTimer;
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);
|
|
sensors.init();
|
|
pingTimer = millis();
|
|
//button.waitForButton();
|
|
}
|
|
|
|
boolean findLine()
|
|
{
|
|
sensors.read(sensor_values);
|
|
if (sensor_values[0] < lightThreshold || sensor_values[5] < lightThreshold)
|
|
{
|
|
if (sensor_values[0] < lightThreshold)
|
|
{
|
|
turnLeft();
|
|
return true;
|
|
}
|
|
else if (sensor_values[5] < lightThreshold)
|
|
{
|
|
turnRight();
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void turnLeft()
|
|
{
|
|
motors.setSpeeds(-reverseSpeed, -reverseSpeed);
|
|
delay(revDuration);
|
|
motors.setSpeeds(turnSpeed, -turnSpeed);
|
|
delay(turnDuration);
|
|
}
|
|
|
|
void turnRight()
|
|
{
|
|
motors.setSpeeds(-reverseSpeed, -reverseSpeed);
|
|
delay(revDuration);
|
|
motors.setSpeeds(-turnSpeed, turnSpeed);
|
|
delay(turnDuration);
|
|
}
|
|
/*
|
|
void search()
|
|
{
|
|
time = sonar.ping();
|
|
distance = sonar.convert_cm(time);
|
|
if (distance < 40)
|
|
motors.setSpeeds(310, 400);
|
|
else
|
|
{
|
|
motors.setSpeeds(400, 400);
|
|
delay(20);
|
|
time = sonar.ping();
|
|
distance = sonar.convert_cm(time);
|
|
if (distance < 40)
|
|
motors.setSpeeds(310, 400);
|
|
}
|
|
}*/
|
|
|
|
void echoCheck()
|
|
{
|
|
if (sonar.check_timer())
|
|
{
|
|
if (sonar.ping_result / US_ROUNDTRIP_CM < 20)
|
|
{
|
|
Serial.println(sonar.ping_result / US_ROUNDTRIP_CM);
|
|
motors.setSpeeds(310, 400);
|
|
}
|
|
else
|
|
{
|
|
Serial.println("Nothing here!");
|
|
motors.setSpeeds(400, 400);
|
|
delay(20);
|
|
}
|
|
}
|
|
}
|
|
|
|
void scan()
|
|
{
|
|
// Init at start, find the target.
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
findLine();
|
|
motors.setSpeeds(310, 400);
|
|
if (millis() >= pingTimer)
|
|
{
|
|
pingTimer += pingSpeed;
|
|
sonar.ping_timer(echoCheck);
|
|
}
|
|
}
|