add code for phase0 through phase3

[challenge-bot] / phase2 / phase2.ino

diff --git a/phase2/phase2.ino b/phase2/phase2.ino
new file mode 100644 (file)
index 0000000..687f2e7
--- /dev/null
+++ b/phase2/phase2.ino
@@ -0,0 +1,160 @@
+/*
+ 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.
+ */
+
+// Pin 13 has an LED connected on most Arduino boards.
+// give it a name:
+int led = 13;
+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(led, 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(led, HIGH);
+  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);
+    }
+}