Stap 2: Arduino schets
Voor de Arduino, het is noodzakelijk een kleine schets dat alleen de gegevens, ontvangt draait op de motor (voorwaarts als achteruit) en elke 300 ms controleert de afstand van de muur:
void loop() { if (Serial.available()) { val = Serial.parseInt(); if (val == 21) { //Off goTo(0, 0); offOn = false; } else if (val < 20) { myservo.write(constrain(map(val, 0, 20, 140, 40), 40, 140)); } else if (val == 22) { //LED On digitalWrite(LED, HIGH); } else if (val == 23) { //LED Off digitalWrite(LED, LOW); } else if ((val >= 1500) && (val <= 1755)) { //Backwards val = val - 1500; goTo(2, val); offOn = true; } else if ((val >= 1000) && (val <= 1255)) { //On val = val - 1000; goTo(1, val); offOn = true; } }
unsigned long now = millis(); if (now - lastime >= 300) { //Check the distance every 300 ms lastime = now; if (distanza() < 40) { //Near a wall... delay(30);
unsigned long distance = distanza(); if (distance < 40) { //Check another time the distance (to be sure...) if (offOn == true) { //If turned on, the car go backwards goTo(2, 210); }
byte pulses = map(distance, 2, 40, 20, 1); unsigned int msPerPulse = 1000 / (pulses * 2); for (byte index = 1; index <= pulses; index++) {//Play a frequency based on the distance tone(12, 2000); delay(msPerPulse); noTone(12); delay(msPerPulse); }
if (offOn == true) { goTo(0, 0); delay(100); } } } } }
long distanza() { //Proximity long distanza;
digitalWrite(trig, LOW); delayMicroseconds(2); digitalWrite(trig, HIGH); delayMicroseconds(10); digitalWrite(trig, LOW);
distanza = pulseIn(echo, HIGH); distanza = distanza / 58;
Serial.println(distanza); return distanza; }
void goTo(byte dir, byte Speed) { //Go backwards and forwards if (dir == 1) { analogWrite(BMpin, 0); digitalWrite(enablePin, HIGH); analogWrite(Mpin, Speed); } else if (dir == 2) { analogWrite(Mpin, 0); digitalWrite(enablePin, HIGH); analogWrite(BMpin, Speed); } else if (dir == 0) { digitalWrite(enablePin, LOW); analogWrite(Mpin, 0); analogWrite(BMpin, 0); } }