115 line
2.7 KiB
C++
115 line
2.7 KiB
C++
|
|
#include <QTRSensors.h>
|
|
|
|
QTRSensors qtr;
|
|
|
|
const uint8_t SensorCount = 8;
|
|
uint16_t sensorValues[SensorCount];
|
|
|
|
|
|
int MODE = 8;
|
|
|
|
int left_motor_in = 6;
|
|
int left_motor_out = 9;
|
|
|
|
int right_motor_in = 3;
|
|
int right_motor_out = 5;
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
Serial.println("Start");
|
|
init_motor();
|
|
|
|
qtr.setTypeRC();
|
|
qtr.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount);
|
|
qtr.setEmitterPin(7);
|
|
|
|
delay(500);
|
|
pinMode(LED_BUILTIN, OUTPUT);
|
|
digitalWrite(LED_BUILTIN, HIGH);// turn on Arduino's LED to indicate we are in calibration mode
|
|
Serial.println("Starting Calibration");
|
|
|
|
// 2.5 ms RC read timeout (default) * 10 reads per calibrate() call
|
|
// = ~25 ms per calibrate() call.
|
|
// Call calibrate() 400 times to make calibration take about 10 seconds.
|
|
for (uint16_t i = 0; i < 400; i++)
|
|
{
|
|
qtr.calibrate();
|
|
}
|
|
digitalWrite(LED_BUILTIN, LOW); // turn off Arduino's LED to indicate we are through with calibration
|
|
Serial.println("Calibration finished");
|
|
|
|
// print the calibration minimum values measured when emitters were on
|
|
Serial.begin(9600);
|
|
for (uint8_t i = 0; i < SensorCount; i++)
|
|
{
|
|
Serial.print(qtr.calibrationOn.minimum[i]);
|
|
Serial.print(' ');
|
|
}
|
|
Serial.println();
|
|
|
|
// print the calibration maximum values measured when emitters were on
|
|
for (uint8_t i = 0; i < SensorCount; i++)
|
|
{
|
|
Serial.print(qtr.calibrationOn.maximum[i]);
|
|
Serial.print(' ');
|
|
}
|
|
Serial.println();
|
|
Serial.println();
|
|
delay(1000);
|
|
}
|
|
|
|
void loop() {
|
|
uint16_t position = qtr.readLineBlack(sensorValues);
|
|
|
|
for (uint8_t i = 0; i < SensorCount; i++)
|
|
{
|
|
Serial.print(sensorValues[i]);
|
|
Serial.print('\t');
|
|
}
|
|
Serial.println(position);
|
|
|
|
delay(250);
|
|
|
|
if (position >= 3000 and position <= 4300) {
|
|
left_motor_go(200);
|
|
right_motor_go(200);
|
|
Serial.print(position);Serial.println("center");
|
|
}
|
|
|
|
if (position < 3000) {
|
|
left_motor_go(200);
|
|
right_motor_go(0);
|
|
Serial.print(position);Serial.println("left");
|
|
}
|
|
|
|
if (position > 4300) {
|
|
left_motor_go(0);
|
|
right_motor_go(200);
|
|
Serial.print(position);Serial.println("right");
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void init_motor(){
|
|
pinMode(left_motor_in, OUTPUT);
|
|
pinMode(left_motor_out, OUTPUT);
|
|
|
|
pinMode(right_motor_in, OUTPUT);
|
|
pinMode(right_motor_out, OUTPUT);
|
|
|
|
pinMode(MODE, OUTPUT);
|
|
digitalWrite(MODE, HIGH);
|
|
}
|
|
|
|
//range:0<value<255
|
|
void left_motor_go(int motor_speed){
|
|
analogWrite(left_motor_in, motor_speed);
|
|
digitalWrite(left_motor_out, LOW);
|
|
}
|
|
//range:0<value<255
|
|
void right_motor_go(int motor_speed){
|
|
analogWrite(right_motor_in, motor_speed);
|
|
digitalWrite(right_motor_out, LOW);
|
|
}
|