Cute Bot

1#include <AFMotor.h> 
2#include <Servo.h>    
3#include <NewPing.h>
4
5Servo  servo;
6NewPing sonar(A0, A1, 1000);
7AF_DCMotor leftMotor(2, MOTOR12_8KHZ);  
8AF_DCMotor rightMotor(1, MOTOR12_8KHZ); 
9
10int pos = 0; 
11int dist =  0;
12int leftdist = 0;
13int rightdist = 0;
14int c = 0;
15
16int leftred=  0;
17int rightred=1;
18
19void setup() {
20  Serial.begin(115200);
21  servo.attach(7);  
22  servo.write(70);
23  moveFront();
24
25  pinMode(0, OUTPUT);
26  pinMode(1,  OUTPUT);
27}
28
29void loop() {
30  dist = readPing();
31  if (dist<20) {
32    moveBack();
33    delay(500);
34    rightdist = lookRight();
35    delay(1000);
36    leftdist = lookLeft();
37    delay(1000);
38    if (leftdist >= rightdist){
39      moveLeft();
40    }else{
41      moveRight();
42    }
43  }
44  moveFront();
45}
46
47int  readPing() {
48  delay(70);
49  unsigned int uS = sonar.ping();
50  int cm =  uS/US_ROUNDTRIP_CM;
51  return cm;
52}
53
54void moveFront() {
55    leftMotor.run(FORWARD);
56    rightMotor.run(FORWARD);
57    for (c = 0; c < 150; c +=2) {
58      leftMotor.setSpeed(c);
59      rightMotor.setSpeed(c);
60
61     }
62    delay(300);
63}
64
65void  moveBack() {
66    leftMotor.run(BACKWARD);
67    rightMotor.run(BACKWARD);
68  
69    delay(200);
70}  
71
72void moveRight() {
73  leftMotor.run(FORWARD);
74  rightMotor.run(BACKWARD);
75
76  delay(300);
77}
78
79void moveLeft() {
80  leftMotor.run(BACKWARD);
81  rightMotor.run(FORWARD);
82
83  delay(300);
84}  
85
86void moveStop() {
87  leftMotor.run(RELEASE);
88  rightMotor.run(RELEASE);
89  
90  delay(500);
91}
92
93int lookLeft() {
94  int rightdist = 0;
95  moveStop();  
96  for(pos = 70; pos >= 0; pos-=10){
97    servo.write(pos);
98    delay(50);
99  }
100  delay(1000);
101  rightdist = readPing();
102  servo.write(70);
103  return  rightdist;  
104}
105
106int lookRight() {
107  int leftdist = 0;
108  moveStop();
109  for(pos = 70; pos <= 140; pos+=10){
110    servo.write(pos);
111    delay(50);
112  }
113  delay(1000);
114  leftdist = readPing();
115  servo.write(70);
116  return  leftdist;  
117}
118