Code for operating IR and ultrasonic sensors.

The code here reads distances from the ultrasonic sensor and GP2D12 IR
sensor. Also some work (not yet working) on driving with the cameras
mounted on panning servos.

Ultrasonic_sensor/ultrasonic_driving_1/ultrasonic_driving_1/ultrasonic_driving_1.ino

@@ -0,0 +1,70 @@
+/* Code for using the ultrasonic sensor while driving
+   Sensor has a minimum range of 30 cm!
+*/
+
+#include <Servo.h>
+
+int pin = 8; //Attached to A5
+float distance = 0;
+Servo motor_left;
+Servo motor_right;
+int motor_left_speed = 90;
+int motor_right_speed = 90;
+
+void setup() {
+  motor_left.attach(3);
+  motor_right.attach(2);
+  motor_left.write(motor_left_speed);
+  motor_right.write(motor_right_speed);
+  drive();
+}
+
+void loop() {
+  distance = read_ul_sensor_range();
+  if (distance <= 40) {
+    turn_left();
+  }
+  else {
+    drive();
+  }
+  delay(100);
+}
+
+float read_ul_sensor_range(void) {
+  int tmp = 0;
+  float range = 0;
+  tmp = analogRead(pin);
+  //Min val ~ 56 (dist. <= 30cm), ~58 at 30cm
+  //ADC value increases by ~20 per 10 cm (from rough testing; check with Julian)
+  if (tmp <= 58) {
+    range = 30.0;
+  }
+  else {
+    //Works; slight underestimate though
+    range = 30.0 + (float(tmp) - 56.0)/2.0;
+  }
+  return range;
+}
+
+void turn_left(void) {
+  motor_left.write(125);
+  motor_right.write(55);
+  delay(2000);
+}
+
+void turn_right(void) {
+  motor_left.write(55);
+  motor_right.write(125);
+  delay(2000);
+}
+
+void reverse(void) {
+  motor_left.write(125);
+  motor_right.write(125);
+  delay(1000);
+}
+
+void drive(void) {
+  motor_left.write(55);
+  motor_right.write(55);
+}    

Ultrasonic_sensor/ultrasonic_sensor_test_1/ultrasonic_sensor_test_1.ino

@@ -0,0 +1,34 @@
+/* Code for using the ultrasonic sensor
+   Sensor has a minimum range of 30 cm!
+*/
+
+int pin = 8; //Attached to A5
+float distance = 0;
+
+void setup() {
+  Serial.begin(9600);
+
+}
+
+void loop() {
+  distance = read_ul_sensor_range();
+  Serial.println(distance);
+  delay(500);
+}
+
+float read_ul_sensor_range(void) {
+  int tmp = 0;
+  float range = 0;
+  tmp = analogRead(pin);
+  //Min val ~ 56 (dist. <= 30cm), ~58 at 30cm
+  //ADC value increases by ~20 per 10 cm (from rough testing; check with Julian)
+  if (tmp <= 58) {
+    range = 30.0;
+  }
+  else {
+    //Works; slight underestimate though
+    range = 30.0 + (float(tmp) - 56.0)/2.0;
+  }
+  return range;
+}
+

ir_sensor/ir_2d12_driving_test/ir_2d12_driving_test/ir_2d12_driving_test.ino

@@ -0,0 +1,78 @@
+//Test using GP2D12 with driving to avoid obstacles
+//GP2D12 is on analog port A6
+//DC motors on servo port S1
+//IT WORKS!
+
+#include <Servo.h>
+
+float distance = 0;
+int pin = 10;
+static const float ADC_ratio = 5.0/1023.0;
+Servo servo_left;
+Servo servo_right;
+//Speeds for both motors
+static int left_speed = 55;
+static int right_speed = 55;
+
+void setup() {
+  servo_left.attach(3);
+  servo_right.attach(2);
+  servo_left.write(left_speed);
+  servo_right.write(right_speed);
+  Serial.begin(9600);
+}
+
+void loop() {
+  distance = read_gp2d12_range(pin);
+  /*if (distance == -1) {
+    distance = read_gp2d12_range(pin);
+    if (distance == -1) {
+      reverse();
+      turn_left();
+      drive();
+    }
+  }*/
+  Serial.println(distance);
+  if ((distance <= 40) && (distance != -1)) {
+    turn_left();
+  }
+  else {
+    drive();
+  }
+  delay(100);
+}
+
+// This code seems to work fairly well, although the range is slightly off (close enough though)
+float read_gp2d12_range(int pin) {
+  int tmp = 0;
+  float range = 0;
+  tmp = analogRead(pin);
+  range = (6787.0 /((float)tmp - 3.0)) - 4.0;
+  if (tmp < 3) {
+    range =  -1; // Error value
+  }
+  return range;
+}
+
+void turn_left(void) {
+  servo_left.write(125);
+  servo_right.write(55);
+  delay(2000);
+}
+
+void turn_right(void) {
+  servo_left.write(55);
+  servo_right.write(125);
+  delay(2000);
+}
+
+void reverse(void) {
+  servo_left.write(125);
+  servo_right.write(125);
+  delay(1000);
+}
+
+void drive(void) {
+  servo_left.write(55);
+  servo_right.write(55);
+}

panning_driving/panning_driving/panning_driving.ino

@@ -0,0 +1,127 @@
+/* Driving with panning GP2D12 and ultrasonic sensor
+*/
+
+#include <Servo.h>
+
+Servo servoIR;
+Servo servoUS;
+int IRPos = 0;
+int USPos = 0;
+float USdistance = 0;
+float IRdistance = 0;
+Servo motor_left;
+Servo motor_right;
+int motor_left_speed = 90;
+int motor_right_speed = 90;
+int i = 0;
+int USpin = 8;
+int GP2D12pin = 10;
+int sensing_dist = 30;
+
+void setup() {
+  servoIR.attach(13);
+  servoUS.attach(12);
+  motor_left.attach(3);
+  motor_right.attach(2);
+  servoIR.write(90);
+  servoUS.write(90);
+  motor_left.write(motor_left_speed);
+  motor_right.write(motor_right_speed);
+  drive();
+}
+
+void loop() {
+  for (i = 0; i =< 90; i++) {
+    IRPos = i;
+    USPos = 180 - i;
+    servoIR.write(IRPos);
+    servoUS.write(USPos);
+    USdistance = read_ul_sensor_range();
+    IRdistance = read_gp2d12_range();
+    if (((USdistance <= sensing_dist) && (USPos > 90)) || ((IRdistance <= sensing_dist) && (IRPos > 90))) {
+      turn_left();
+    }
+    else if (((USdistance <= sensing_dist) && (USPos < 90)) || ((IRdistance <= sensing_dist) && (IRPos < 90))) {
+      turn_right();
+    }
+    else if (((USdistance <= sensing_dist) && (USPos == 90)) || ((IRdistance <= sensing_dist) && (IRPos == 90))) {
+     reverse();
+     delay(100);
+     turn_left();
+    } 
+    else {
+      drive();
+    }
+    delay(10);
+  }
+  for (i = 90; i >= 0; i--) {
+    IRPos = i;
+    USPos = 90 - i;
+    servoIR.write(IRPos);
+    servoUS.write(USPos);
+    USdistance = read_ul_sensor_range();
+    IRdistance = read_gp2d12_range();
+    if (((USdistance <= sensing_dist) && (USPos > 90)) || ((IRdistance <= sensing_dist) && (IRPos > 90))) {
+      turn_left();
+    }
+    else if (((USdistance <= sensing_dist) && (USPos < 90)) || ((IRdistance <= sensing_dist) && (IRPos < 90))) {
+      turn_right();
+    }
+    else if (((USdistance <= sensing_dist) && (USPos == 90)) || ((IRdistance <= sensing_dist) && (IRPos == 90))) {
+     reverse();
+     delay(100);
+     turn_left();
+    }
+   else {
+    drive();
+   } 
+    delay(10);
+  }
+}
+
+float read_ul_sensor_range(void) {
+  int tmp = 0;
+  float range = 0;
+  tmp = analogRead(USpin);
+  //Min val ~ 56 (dist. <= 30cm), ~58 at 30cm
+  //ADC value increases by ~20 per 10 cm (from rough testing; check with Julian)
+  if (tmp <= 58) {
+    range = 30.0;
+  }
+  else {
+    //Works; slight underestimate though
+    range = 30.0 + (float(tmp) - 56.0)/2.0;
+  }
+  return range;
+}
+
+float read_gp2d12_range(void) {
+  int tmp = 0;
+  float range = 0;
+  tmp = analogRead(GP2D12pin);
+  range = (6787.0 /((float)tmp - 3.0)) - 4.0;
+  if (tmp < 3) {
+    range =  -1; // Error value
+  }
+  return range;
+}
+
+void turn_left(void) {
+  motor_left.write(125);
+  motor_right.write(55);
+}
+
+void turn_right(void) {
+  motor_left.write(55);
+  motor_right.write(125);
+}
+
+void reverse(void) {
+  motor_left.write(125);
+  motor_right.write(125);
+}
+
+void drive(void) {
+  motor_left.write(55);
+  motor_right.write(55);
+}