Line_Following-2022/test/test.ino

116 lines
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);
}