Add more code file

motion/motion.ino

@@ -0,0 +1,72 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include <SoftwareSerial.h>
+#include <MeMCore.h>
+
+
+// declare motors
+MeDCMotor _leftMotor(9);
+MeDCMotor _rightMotor(10);
+MeUltrasonicSensor ultrasonic(PORT_3);
+
+// declare motor speeds
+int _leftMotorSpeed = 0;
+int _rightMotorSpeed = 0;
+
+void setup()
+{
+  Serial.begin(9600);
+  // set no speed for left and right motors 
+  _leftMotor.run((9)==M1?-(0):(0));
+  _rightMotor.run((10)==M1?-(0):(0));
+}
+
+void loop()
+{
+  double distance_in = ultrasonic.distanceInch();
+  double distance_cm = ultrasonic.distanceCm();
+  Serial.print("Distance: ");
+  Serial.print(distance_cm);
+  Serial.print(" cm | ");
+  Serial.print(distance_in);
+  Serial.println(" inches");
+
+
+  // Move forward
+  if (distance_cm > 20) {
+      Serial.println("Move forward");
+      move(1500, 1500, 100);
+  } else{
+  // Move backwards
+      Serial.println("Move backward");
+      move(-100, -100, 500); 
+
+      Serial.println("Turning right");
+      move(100, -100, 500);
+
+
+      delay(100);
+  
+  // Turn right
+  //Serial.println("Turn right");
+  //move(100, -100, 2000);
+  }
+
+  // Turn left
+  //Serial.println("Turn left");
+  //move(-100, 100, 2000);
+ 
+}
+
+void move(int leftMotorSpeed, int rightMotorSpeed, int duration)
+{
+  _leftMotorSpeed = leftMotorSpeed;
+  _rightMotorSpeed = rightMotorSpeed;
+  
+  _leftMotor.run((9)==M1?-(_leftMotorSpeed):(_leftMotorSpeed));
+  _rightMotor.run((10)==M1?-(_rightMotorSpeed):(_rightMotorSpeed));
+  delay(duration);
+  _leftMotor.stop();
+  _rightMotor.stop();
+  
+}