Chemin de compensation Wall-E (4 / 4 étapes)

Étape 4: Le code

 #include 

 const int left1 = 11;// left motor pin 1 const int left2 = 10;// left motor pin 2 const int right1 = 13; //right motor pin 1 const int right2 = 12;//right motor 2 Servo handServo; const int Trig = 4;// the HC-SR04 ultrasonic sensor's trigger pin const int Echo = 3;// and it's read pin 

 void setup() { // set the motor pins as output pinMode(left1, OUTPUT); pinMode(left2, OUTPUT); pinMode(right1, OUTPUT); pinMode(right2, OUTPUT); pinMode(Echo, INPUT); pinMode(Trig, OUTPUT); handServo.attach(5);// connect the servo to pin 5 } 

 void loop() { long duration, cm;// set variable for duration of ping and the distance units //make the motors rotate in sync(robot goes forward) handServo.write(60);//initialize the hand position digitalWrite(left1, HIGH); digitalWrite(left2, LOW); digitalWrite(right1, HIGH); digitalWrite(right2, LOW); delay(10000); digitalWrite(left2, HIGH); digitalWrite(left1, LOW); digitalWrite(right1, HIGH); digitalWrite(right2, LOW); delay(1000); digitalWrite(left1, HIGH); digitalWrite(left2, LOW); digitalWrite(right1, HIGH); digitalWrite(right2, LOW); delay(10000); //look to see if theres anything in front digitalWrite(Trig,LOW);//give a low on the trigger for a clean high delayMicroseconds(2); digitalWrite(Trig,HIGH);//now send a high pulse delayMicroseconds(5); digitalWrite(Trig,LOW);//and low again duration = pulseIn(Echo,HIGH);//read the echo pin for a high //the ultrasonic sensor I used (the HC-SR04) has 4 pins two for power and ground, //one for sending a pulse and the last one for reading the pulse sent //you calculate the distance an object is at by calculating the time //difference between the pulse sent and when it was read. cm = microsecondsToCentimeters(duration); //if pulse read was below a certain limit move the servos if (cm < 18) { handServo.write(180); delay(30); } else{ handServo.write(60);//hands go back up } } long microsecondsToCentimeters(long microseconds) { //sounds speed is 29 microseconds per centimeter. //the pulse makes a round trip, so the distance is divided by 2 return microseconds/ 29 / 2; }