Hardware:
Robot Shield
Arduino Uno R3
Black Tape
QRD1113 IR Sensors (x2)
Resistors(2x10k, 2×220)
Code:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 |
#include Servo left; Servo right; const int leftSensor=A2; const int rightSensor=A3; int i=0; int FL=1700; // Left Servo Forward int FR=1300; //Right Servo Forward int R=0; int L=0; void setup() { Serial.begin(9600); left.attach(12); //left servo motor right.attach(13); //right servo motor pinMode(leftSensor, INPUT); pinMode(rightSensor, INPUT); } void loop() { int leftSens; int rightSens; leftSens = ReadSens_L(); rightSens = ReadSens_R(); Serial.println("Beginning"); Serial.println(leftSens); Serial.println(rightSens); if(leftSens<400&&rightSens<400) { left.writeMicroseconds(FL); right.writeMicroseconds(FR); Serial.println("Forward!"); delay(40); } else { if(leftSens>400&&rightSens<400) { left.writeMicroseconds(1450); right.writeMicroseconds(1400); Serial.println("Turn Left!"); delay(10); Serial.println(leftSens); delay(10); Serial.println(rightSens); delay(10); int leftSens = ReadSens_L(); delay(10); int rightSens = ReadSens_R(); delay(10); } else { if(leftSens<400&&rightSens>400) { left.writeMicroseconds(1600); right.writeMicroseconds(1550); Serial.println("Turn Right!"); delay(10); Serial.println(leftSens); delay(10); Serial.println(rightSens); delay(10); int leftSens = ReadSens_L(); delay(10); int rightSens = ReadSens_R(); delay(10); } else{ }}} } int ReadSens_L(){ int i; int L = 0; for (i = 0; i < 5; i++){ L = L + analogRead(A2); // sensor on analog pin A2 } L = L / 5; // average return L; } int ReadSens_R(){ int j; int R = 0; for (j = 0; j < 5; j++){ R = R + analogRead(A3); // sensor on analog pin A3 } R = R / 5; // avarage return R; } |