first commit

MotorShield_Test/MotorShield_Test.ino

@@ -0,0 +1,137 @@
+#include <AFMotor.h>
+//SMARS Demo 1 with Line sensor
+//This sketch makes the robot move inside a delimitated Area (you can make a perimeter with insulating tape)
+//you'll need an Adafruit Motor shield V1 https://goo.gl/7MvZeo and a IR sensor https://goo.gl/vPWfzx
+AF_DCMotor rightMotor(2);
+AF_DCMotor leftMotor(1);
+// declare variables
+int lineNumber;
+int trigPin = 10;    // Trigger
+int echoPin = 11;    // Echo
+long duration, cm, inches;
+int speed_L = 200;  
+int speed_R = 200;  
+String motorSet = "";
+#define MOTORS_CALIBRATION_OFFSET 0 // this sets offset to allow for differences between the two DC motors
+
+#define MAX_SPEED 200 // sets speed of DC traction motors to 120/256 or about 47% of full speed - to reduce power draining.
+int speedSet = 0;
+
+
+void setup() {
+  Serial.begin(9600);
+    //Define inputs and outputs
+  pinMode(trigPin, OUTPUT);
+  pinMode(echoPin, INPUT);
+  // defines Right motor connector
+  // defines Left motor connector
+  //defines the variable where it will store the
+  // sets up Serial library at 9600 bps
+  //changes the following values to make the robot possible drive as straight as
+
+  
+  leftMotor.setSpeed(speed_L);
+  rightMotor.setSpeed(speed_R); // sets L motor speed
+  
+  // sets R motor speed
+  rightMotor.run(RELEASE);
+  leftMotor.run(RELEASE); //turns L motor on
+  //turns R motor on
+}
+void loop() {
+
+  // the following operations will make the robot goes backward for 2 seconds and turns left for 1.5 seconds
+//  rightMotor.run(RELEASE);
+//  leftMotor.run(RELEASE);
+//  recule();
+//  delay(2000);
+  delay(150);
+  int dist = distance();
+  dist = min(200, dist);
+  if (dist <= 20) {
+    while (dist <= 30) {
+      moveBackward();
+      dist = distance();
+      dist = min(200, dist);
+    }
+    turnRight();
+  } 
+  else {
+    moveForward();
+  }
+   
+  
+}
+void turnRight() {
+  motorSet = "RIGHT";
+  leftMotor.run(FORWARD);      // turn motor 1 forward
+  rightMotor.run(BACKWARD);     // turn motor 2 backward
+  delay(400); // run motors this way for 400ms
+  motorSet = "FORWARD";
+  leftMotor.run(FORWARD);      // set both motors back to forward
+  rightMotor.run(FORWARD);
+}
+//void avance()
+//{
+//    // turning left
+//  rightMotor.run(FORWARD);
+//  leftMotor.run(FORWARD);
+//}
+//void recule()
+//{
+//    //go backward
+//  rightMotor.run(BACKWARD);
+//  leftMotor.run(BACKWARD);
+//}
+
+int distance() {
+  // 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);
+ 
+  // Convert the time into a distance
+  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
+  inches = (duration/2) / 74;   // Divide by 74 or multiply by 0.0135
+  
+  Serial.print(inches);
+  Serial.print("in, ");
+  Serial.print(cm);
+  Serial.print("cm");
+  Serial.println();
+  
+  return cm;
+}
+
+void moveForward() {
+  motorSet = "FORWARD";
+  leftMotor.run(FORWARD);      // turn it on going forward
+  rightMotor.run(FORWARD);      // turn it on going forward
+  for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
+  {
+    leftMotor.setSpeed(speedSet + MOTORS_CALIBRATION_OFFSET);
+    rightMotor.setSpeed(speedSet);
+    delay(5);
+  }
+}
+//-------------------------------------------------------------------------------------------------------------------------------------
+void moveBackward() {
+  motorSet = "BACKWARD";
+  leftMotor.run(BACKWARD);      // turn it on going forward
+  rightMotor.run(BACKWARD);     // turn it on going forward
+  for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
+  {
+    leftMotor.setSpeed(speedSet + MOTORS_CALIBRATION_OFFSET);
+    rightMotor.setSpeed(speedSet);
+    delay(5);
+  }
+}