Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <QTRSensors.h>
- QTRSensors qtr;
- const uint8_t SensorCount = 4;
- uint16_t sensorValues[SensorCount];
- const int In1 = A2;
- const int In2 = A3;
- const int In3 = A1;
- const int In4 = A0;
- const int EnA = 11;
- const int EnB = 3;
- void setup()
- {
- qtr.setTypeRC();
- qtr.setSensorPins((const uint8_t[]){4, 5, 6, 7}, SensorCount);
- Serial.begin(9600);
- pinMode(In1, OUTPUT);
- pinMode(In2, OUTPUT);
- pinMode(In3, OUTPUT);
- pinMode(In4, OUTPUT);
- pinMode(EnA, OUTPUT);
- pinMode(EnB, OUTPUT);
- }
- void loop() {
- qtr.read(sensorValues);
- int leftValue = map(sensorValues[0], 0, 1500, 0, 20);
- int leftCenterValue = map(sensorValues[1], 0, 1500, 0, 20);
- int rightCenterValue = map(sensorValues[2], 0, 1500, 0, 20);
- int rightValue = map(sensorValues[3], 0, 1500, 0, 20);
- if (leftValue - 3 > rightValue)
- {
- goRight();
- }
- else if (leftValue < rightValue - 3)
- {
- goLeft();
- }
- else
- {
- goStraight();
- }
- Serial.print(leftValue);
- Serial.print('\t');
- Serial.print(leftCenterValue);
- Serial.print('\t');
- Serial.print(rightCenterValue);
- Serial.print('\t');
- Serial.print(rightValue);
- Serial.print('\t');
- Serial.println();
- delay(200);
- delay(250);
- }
- void goStraight()
- {
- digitalWrite(In1, HIGH);
- digitalWrite(In2, LOW);
- digitalWrite(In3, HIGH);
- digitalWrite(In4, LOW);
- analogWrite(EnA, 100);
- analogWrite(EnB, 100);
- delay(50);
- Stop();
- }
- void Stop()
- {
- digitalWrite(In1, LOW);
- digitalWrite(In2, LOW);
- digitalWrite(In3, LOW);
- digitalWrite(In4, LOW);
- analogWrite(EnA, 0);
- analogWrite(EnB, 0);
- }
- void goRight()
- {
- digitalWrite(In1, HIGH);
- digitalWrite(In2, LOW);
- digitalWrite(In3, LOW);
- digitalWrite(In4, HIGH);
- analogWrite(EnA, 100);
- analogWrite(EnB, 100);
- delay(50);
- Stop();
- }
- void goLeft()
- {
- digitalWrite(In1, LOW);
- digitalWrite(In2, HIGH);
- digitalWrite(In3, HIGH);
- digitalWrite(In4, LOW);
- analogWrite(EnA, 100);
- analogWrite(EnB, 100);
- delay(50);
- Stop();
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement