Cyberduck is alive!

Co-Authored-By: Carlos Garcia Saura <[email protected]>

Cyberduck-Robot.ino

@@ -1,3 +1,5 @@
+#include <Servo.h>
+
 // L298n pins (For motors)
 #define right_in1 2
 #define right_in2 3
@@ -7,22 +9,16 @@
 #define enableRight 11 //ENA
 
 // Motors
-#define stdSpeed 100
+#define stdSpeed 75
 
-enum DirectionTaken{
-  FWD,
-  RIGHT,
-  LEFT,
-  NONE
-};
+// Servo
+Servo neck;
+#define servoNeck A0
 
 // Sensors
 const int sensorCount = 5;
 int sensorPins[] = {8, 7, 6, 12, 13}; // From left to right (travelwise)
 int sensorValues[sensorCount];
-int previous_error = 0;
-int error = 0;
-DirectionTaken dir = NONE;
 
 void setup(){
     pinMode(left_in3, OUTPUT);
@@ -32,6 +28,8 @@ void setup(){
     pinMode(enableRight, OUTPUT);
     pinMode(enableLeft, OUTPUT);
 
+    neck.attach(servoNeck);
+
     for(int i = 0; i < sensorCount; i++){
       pinMode(sensorPins[i], INPUT);
     }
@@ -40,73 +38,52 @@ void setup(){
 }
 
 void loop(){
-  control_robot();
+  //control_robot();
+
+  //test_motor();
+
+  test_servo();
 }
 
 // @brief This method will update the array sensorValues with the current digital read of all the sensors in the array
 void readSensors(){
   for(int i = 0; i < sensorCount; i++){
     sensorValues[i] = digitalRead(sensorPins[i]);
   }
+
 }
 
 void control_robot(){
   readSensors();
-  int sensorCum =0;
 
-  // Calculate the accomulative sensors value
-  for(int i = 0; i < sensorCount; i++){
-    sensorCum += sensorValues[i] * (1 << sensorCount-1-i); // pow is intended for floating point whereas with logical shifts we can work with integers without any interference
-  }
+  //int left2 = sensorValues[4]; // 13
+  int left1 = sensorValues[3]; // 12
+  //int central = sensorValues[2]; // 6
+  int right1 = sensorValues[1]; // 7
+  //int right2 = sensorValues[0]; // 8
 
+  Serial.print("LEFT: ");
+  Serial.print(sensorValues[3]);
+  Serial.print(" RIGHT: ");
+  Serial.print(sensorValues[1]);
   Serial.println();
-  Serial.println(sensorCum, BIN);
+
+  if(left1 == 0 and right1 == 0){
+    forwardMovement(stdSpeed, stdSpeed);
+  }else if(left1 == 0 and right1 == 1){
+    turnRight(stdSpeed);
+  }else if(left1 == 1 and right1 ==0){
+    turnLeft(stdSpeed);
+  }else{
+    forwardMovement(stdSpeed, stdSpeed);
+  }
 
-    //delay(500);
+
 
-  switch(sensorCum){
-    case 0b10000:
-    case 0b01000:
-    case 0b11000:
-    case 0b00111:
-      turnRight(stdSpeed);
-      Serial.print("Going right");
-      dir = RIGHT;
-      break;
-
-    case 0b00001:
-    case 0b00010:
-    case 0b00011:
-    case 0b11100:
-      turnLeft(stdSpeed);
-      Serial.print("Going left");
-      dir = LEFT;
-      break;
-
-    // e.g.: 01011, which is an unlikely case, but we ought to consider it too 
-    // In either case we want to continue moving straight
-    case 0b01100:
-    case 0b00110:
-    case 0b00100:
-    case 0b01110:
-    case 0b11111:
-      forwardMovement(stdSpeed, stdSpeed);
-      Serial.print("Going forward");
-      dir = FWD;
-      break;
-
-    default:
-      // Leftwards of the track
-      if(dir == LEFT) turnLeft(stdSpeed);
-      else if(dir == RIGHT) turnRight(stdSpeed);
-      // Rightwards of the track
-      else forwardMovement(stdSpeed, stdSpeed);
-      break;
-  }
-
-  forwardMovement(stdSpeed, stdSpeed);
 }
 
+
+
 void forwardMovement(int speedLeft, int speedRight){
   // Left wheel
   digitalWrite(left_in3, LOW);
@@ -121,8 +98,9 @@ void forwardMovement(int speedLeft, int speedRight){
 
 void turnLeft(int speedRight){
   // Left wheel
-  digitalWrite(left_in3, LOW);
+  digitalWrite(left_in3, HIGH);
   digitalWrite(left_in4, LOW);
+  analogWrite(enableLeft, speedRight/3);
 
   // Right wheel
   digitalWrite(right_in1, HIGH);
@@ -138,10 +116,17 @@ void turnRight(int speedLeft){
 
   // Right wheel
   digitalWrite(right_in1, LOW);
-  digitalWrite(right_in2, LOW);
+  digitalWrite(right_in2, HIGH);
+  analogWrite(enableRight, speedLeft/3);
 }
 
 void test_motor(){
+  digitalWrite(right_in1, LOW);
+  digitalWrite(right_in2, LOW);
+  digitalWrite(left_in3, LOW);
+  digitalWrite(left_in4, LOW);
+  delay(1000);
+
   forwardMovement(stdSpeed, stdSpeed);
   delay(3000);
   turnLeft(stdSpeed);
@@ -157,3 +142,14 @@ void test_sensorIR(){
   }
   Serial.println();
 }
+
+void test_servo(){
+  neck.write(60);
+  delay(500);
+  neck.write(90);
+  delay(500);
+  neck.write(120);
+  delay(500);  
+  neck.write(90);
+  delay(500);
+}