Stap 3: De code
Gedetailleerde werking van de ultrasone afstandssensor kon worden gevonden in het HC_SR04- gegevensblad.
De code die wordt gebruikt in deze robot is echt eenvoudig. In de hoofdlus (loop()), het meten van de afstand door het aanroepen van de functie getDistance().
Als de afstand tussen de robot en het obstakel (in dit geval, mijn handen) te sluiten (zeg 10cm), de robot in terugwaartse richting voor 300 milliseconden en willekeurig zet links of rechts voor een andere 200 milliseconden.
void loop() { distance = getDistance(); Serial.print("Distance = "); Serial.print(distance); Serial.println("cm"); if (distance < 10) { robotBackward(300); if (random(2)==0) { robotRight(200); } else { robotLeft(200); } } robotStop(50); } Hier is hoe de 'getDistance()' werk functioneren:
float getDistance() { digitalWrite(TRIGGER_PIN, LOW); delayMicroseconds(2); digitalWrite(TRIGGER_PIN, HIGH); delayMicroseconds(10); digitalWrite(TRIGGER_PIN, LOW); return ( pulseIn(ECHO_PIN, HIGH) *0.017); } Het stuurt een stijgende rand op "Trig" pin door het eerste verzenden van lage en gevolgd door HIGH. Na 10 micro-seconden verlaagt het het "Trig" signaal opnieuw. Dit maakt de sensor afgeven van ultrasone pulsen. Later controleert het de echo weerspiegeld door de obstakels. Als het obstakel dichter is, kost het minder tijd om te ontvangen van de echos. Aan de andere kant, als het obstakel is ver weg, duurt het meer tijd om te wachten op de echos. De sensor een hoge puls aan "Echo" pin afgeven en de pulsbreedte gelijk aan de vertraging van de heen-en terugreis van de ultrasone pulsen (in microseconden).
Aangezien de snelheid van het geluid 340 meter/seconde is, de afstand kan worden berekend door D = 0. 5 * pulse_width * 340 m/s * 1e-6
Het volledige programma staat hier:
pinnen controller motoren #define motor1_pos 3 #define motor1_neg 10 #define motor2_pos 6 #define motor2_neg 9 #define motor_en A2 #define TRIGGER_PIN 15 / / (A1) Arduino pin gebonden aan pin op de ultrasone sensor te activeren. #define ECHO_PIN 14 / / (A0) Arduino pin gebonden aan echo pin op de ultrasone sensor. float afstand; void setup {Serial.begin(57600); setupMotor();} // pins controller motors #define motor1_pos 3 #define motor1_neg 10 #define motor2_pos 6 #define motor2_neg 9 #define motor_en A2 #define TRIGGER_PIN 15 // (A1) Arduino pin tied to trigger pin on the ultrasonic sensor. #define ECHO_PIN 14 // (A0) Arduino pin tied to echo pin on the ultrasonic sensor. float distance; void setup() { Serial.begin(57600); setupMotor(); } void loop() { distance = getDistance(); Serial.print("Distance = "); Serial.print(distance); Serial.println("cm"); if (distance < 10) { robotBackward(300); if (random(2)==0) { robotRight(200); } else { robotLeft(200); } } robotStop(50); } void setupMotor() { pinMode(motor1_pos,OUTPUT); pinMode(motor1_neg,OUTPUT); pinMode(motor2_pos,OUTPUT); pinMode(motor2_neg,OUTPUT); pinMode(motor_en,OUTPUT); enableMotor(); robotStop(50); } //----------------------------------------------------------------------------------------------------- // Ultrasonic Distance Sensor //----------------------------------------------------------------------------------------------------- float getDistance() { digitalWrite(TRIGGER_PIN, LOW); delayMicroseconds(2); digitalWrite(TRIGGER_PIN, HIGH); delayMicroseconds(10); digitalWrite(TRIGGER_PIN, LOW); return ( pulseIn(ECHO_PIN, HIGH) *0.017); } //----------------------------------------------------------------------------------------------------- // motor //----------------------------------------------------------------------------------------------------- void enableMotor() { //Turn on the motor driver chip : L293D digitalWrite(motor_en, HIGH); } void disableMotor() { //Turn off the motor driver chip : L293D digitalWrite(motor_en, LOW); } void robotStop(int ms){ digitalWrite(motor1_pos, LOW); digitalWrite(motor1_neg, LOW); digitalWrite(motor2_pos, LOW); digitalWrite(motor2_neg, LOW); delay(ms); } void robotForward(int ms){ digitalWrite(motor1_pos, HIGH); digitalWrite(motor1_neg, LOW); digitalWrite(motor2_pos, HIGH); digitalWrite(motor2_neg, LOW); delay(ms); } void robotBackward(int ms){ digitalWrite(motor1_pos, LOW); digitalWrite(motor1_neg, HIGH); digitalWrite(motor2_pos, LOW); digitalWrite(motor2_neg, HIGH); delay(ms); } void robotRight(int ms){ digitalWrite(motor1_pos, LOW); digitalWrite(motor1_neg, HIGH); digitalWrite(motor2_pos, HIGH); digitalWrite(motor2_neg, LOW); delay(ms); } void robotLeft(int ms){ digitalWrite(motor1_pos, HIGH); digitalWrite(motor1_neg, LOW); digitalWrite(motor2_pos, LOW); digitalWrite(motor2_neg, HIGH); delay(ms); } VOID setupMotor() {pinMode(motor1_pos,OUTPUT); pinMode(motor1_neg,OUTPUT); pinMode(motor2_pos,OUTPUT); pinMode(motor2_neg,OUTPUT); pinMode(motor_en,OUTPUT); enableMotor(); robotStop(50);} / /---/ / ultrasone afstandssensor / /---
