Mitsubachi Arduino code
- 1. #include <Servo.h> //Servo library Servo servo_test; //Servo setup int angle = 0; // Servo setup // motors int in1 = 4; int in2 = 5; int in3 = 6; int in4 = 7; unsigned long previousMillis = 0; // will store last time servos were spinning unsigned long currentMillis = 0; const long interval = 5000; // interval between each round of spin int trigPin = 3; // Trigger for ultrasonic sensor int echoPin = 2; // Echo for ultrasonic sensor long duration, cm, inches; // ultrasonic sensor setup void setup() { Serial.begin (9600); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); servo_test.attach(9); pinMode(in1, OUTPUT); pinMode(in2, OUTPUT); pinMode(in3, OUTPUT); pinMode(in4, OUTPUT); } void loop() { // The sensor is triggered by a HIGH pulse of 10 or more microseconds. // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: digitalWrite(trigPin, LOW); delayMicroseconds(5); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); // Read the signal from the sensor: a HIGH pulse whose duration is the time (in microseconds) // from the sending of the ping to the reception of its echo off of an object. pinMode(echoPin, INPUT); duration = pulseIn(echoPin, HIGH); cm = (duration/2) / 29.1; // Convert time into distance delay(250); currentMillis = millis();
- 2. if (cm < 15) { digitalWrite(in1, LOW); digitalWrite(in2, LOW); digitalWrite(in3, LOW); digitalWrite(in4, LOW); } else { // Turn on motors in FORWARD motions digitalWrite(in1, HIGH); digitalWrite(in2, LOW); digitalWrite(in3, HIGH); digitalWrite(in4, LOW); if (currentMillis - previousMillis >= interval) { previousMillis = currentMillis; // save the last time spinning the servo digitalWrite(in1, LOW); // stop all motors digitalWrite(in2, LOW); digitalWrite(in3, LOW); digitalWrite(in4, LOW); for (angle = 0; angle < 360; angle += 5) { // turn servo 1000 degrees servo_test.write(angle); delay(5); } delay(400); for (angle = 360; angle>=1; angle-=5) { // reverse servo back to 0 degrees servo_test.write(angle); delay(5); } delay(1000); } } }