+++ /dev/null
-/*
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU Affero General Public License as
- published by the Free Software Foundation, either version 3 of the
- License, or (at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU Affero General Public License for more details.
-
- You should have received a copy of the GNU Affero General Public License
- along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-/*
- Motor.
- Runs both motors back and forth.
-n
- This example code is in the public domain.
- */
-
-int motor1_enable = 10;
-int motor1a = 9;
-int motor1b = 8;
-int motor2_enable = 3;
-int motor2a = 4;
-int motor2b = 5;
-
-// ping sensors
-int left_trigger = 12;
-int left_echo = 11;
-int right_trigger = 7;
-int right_echo = 6;
-
-// the setup routine runs once when you press reset:
-void setup() {
- Serial.begin(9600);
-
- // initialize the digital pin as an output.
- pinMode(motor1_enable, OUTPUT);
- pinMode(motor1a, OUTPUT);
- pinMode(motor1b, OUTPUT);
- pinMode(motor2_enable, OUTPUT);
- pinMode(motor2a, OUTPUT);
- pinMode(motor2b, OUTPUT);
-
- // ping sensors
- pinMode(left_trigger, OUTPUT);
- pinMode(left_echo, INPUT);
- pinMode(right_trigger, OUTPUT);
- pinMode(right_echo, INPUT);
-
- digitalWrite(motor1_enable, LOW);
- digitalWrite(motor1a, LOW);
- digitalWrite(motor1b, LOW);
- digitalWrite(motor2_enable, LOW);
- digitalWrite(motor2a, LOW);
- digitalWrite(motor2b, LOW);
-}
-
-void
-on(int pin)
-{
- digitalWrite (pin, HIGH);
-}
-
-void
-off(int pin)
-{
- digitalWrite (pin, LOW);
-}
-
-int
-ping(int trigger, int echo)
-{
- int ping_time;
- on(trigger);
- delayMicroseconds(12);
- off(trigger);
- ping_time = pulseIn (echo, HIGH);
- if (ping_time <= 0)
- ping_time = 3000;
- return ping_time;
-}
-
-void motorsRun(int left, int right, int ms_delay) {
- // Set left motor direction:
- if (left > 0) {
- // Set left motor to go forward:
- digitalWrite(motor1a, HIGH);
- digitalWrite(motor1b, LOW);
- } else {
- // Set left motor to go backward:
- digitalWrite(motor1a, LOW);
- digitalWrite(motor1b, HIGH);
- left = -left; // Make left a positive value:
- }
- analogWrite(motor1_enable, left); // Start motor in right direction
-
- // Set left motor direction:
- if (right > 0) {
- // Set right motor to go forward:
- digitalWrite(motor2a, HIGH);
- digitalWrite(motor2b, LOW);
- } else {
- // Set right motor to go backward:
- digitalWrite(motor2a, LOW);
- digitalWrite(motor2b, HIGH);
- right = -right; // Make right a positive value:
- }
- analogWrite(motor2_enable, right); // Start motor in right direction
-
- delay(ms_delay); // Wait the specified amount of time
-}
-
-// the loop routine runs over and over again forever:
-void
-loop()
-{
- int left_speed = 0;
- int right_speed = 0;
-
- int left_ping = ping (left_trigger, left_echo);
- int right_ping = ping (right_trigger, right_echo);
-
- /*
- Serial.print ("left ping = ");
- Serial.print (left_ping);
-
- Serial.print (" right ping = ");
- Serial.println (right_ping);
- */
-
- if (left_ping < 400)
- {
- left_speed = 250;
- }
-
- if (right_ping < 400)
- {
- right_speed = 250;
- }
-
- if (left_speed == 0 && right_speed == 0)
- {
- // backup
- motorsRun(-250, -250, 2000);
- // turn around
- motorsRun(-250, 250, 3200);
- }
- else
- {
- motorsRun(left_speed, right_speed, 0);
- }
-}