Merge pull request #1 from himalayanelixir/reset-code

Reset code

Author: himalayanelixir

arduino_controller/Port Mapping.txt

@@ -0,0 +1,4 @@
+
+    1               2           3           4           5           6               7           8           9               10          11              12          13              14           15             16            17            18          19              20              21              22            
+
+{{11, 12, 13}, {8, 9, 10}, {5, 6, 7}, {2, 3, 4}, {14, 15, 16}, {17, 18, 19}, {20, 21, 22}, {23, 24, 25}, {29, 30, 31}, {35, 36, 37}, {41, 42, 43}, {47, 48, 49}, {26, 27, 28}, {32, 33, 34}, {38, 39, 40}, {44, 45, 46}, {50, 51, 52}, {53, 54, 55}, {56, 57, 58}, {59, 60, 61}, {63, 64, 65}, {62, 66, 67}};

arduino_controller/arduino_controller.ino

@@ -5,9 +5,11 @@
 #define DEBOUNCE_TIME .4
 #define SAMPLE_FREQUENCY 20
 #define MAXIMUM (DEBOUNCE_TIME * SAMPLE_FREQUENCY)
-#define NUMBER_MOTORS 1
+#define NUMBER_MOTORS 3
+#define TIMEOUT 10000
 
 // relay pin
+//int relayPin = 69;
 int relayPin = 53;
 
 // function declarations`
@@ -24,18 +26,17 @@ bool new_data = false;
 // initialize motors
 Servo my_servo[NUMBER_MOTORS];
 // create a array of ports with the order: motor, counter, reset
-int ports[NUMBER_MOTORS][3] = {{21, 19, 20}};
-// int ports[NUMBER_MOTORS][3] = {{2, 3, 4}, {5, 6, 7}, {8, 9, 10}, {11, 12, 13}, {22, 23, 24}, {25, 26, 27}, {28, 29, 30}, {31, 32, 33}, {34, 35, 36}};
-// int ports[NUMBER_MOTORS][3] = {{2, 3, 4}, {5, 6, 7}, {8, 9, 10}, {11, 12, 13}, {22, 23, 24}, {25, 26, 27}};
+// int ports[NUMBER_MOTORS][3] = {{62, 66, 67}};
+int ports[NUMBER_MOTORS][3] = {{2, 3, 4},{5, 6, 7},{8, 9, 10}};
 // integer array that contains the direction and number of rotations a motor
 // needs to go
 int motor_commands[NUMBER_MOTORS][2] = {0};
 
 // array of new switch values
-byte motor_sensor_counter1[NUMBER_MOTORS] = {1};
+byte motor_sensor_counter1[NUMBER_MOTORS] = {0};
 
 // array of old switch values
-byte motor_sensor_counter2[NUMBER_MOTORS] = {1};
+byte motor_sensor_counter2[NUMBER_MOTORS] = {0};
 
 // 0 or 1 depending on the input signal
 byte input[NUMBER_MOTORS] = {0};
@@ -51,7 +52,7 @@ void setup()
 
   // initialize relay pin
   pinMode(relayPin, OUTPUT);
-  
+
   // initialize all motor ports
   Serial.println("Begining Initialization");
   Serial.print("Motor Ports: ");

arduino_controller/arduino_controller_functions.ino

@@ -9,28 +9,41 @@
 
 void ProcessData()
 {
+  // Added this because reset code is different from regular movement code
+  bool is_reset = false;
+  bool go = true;
+  int total_turns = 0;
+  long timeout_counter = 0;
 
   PopulateArray();
+
   // initialize all motors and get them moving
   for (int i = 0; i < NUMBER_MOTORS; i++)
   {
     if (motor_commands[i][0] == 0)
     {
       my_servo[i].write(100);
       Serial.println("Reached Up");
+      is_reset = false;
     }
     else if (motor_commands[i][0] == 1)
     {
       my_servo[i].write(80);
       Serial.println("Reached Down");
+      is_reset = false;
     }
     else if (motor_commands[i][0] == 2)
     {
       my_servo[i].write(90);
       Serial.println("Reached Stopped");
+      is_reset = false;
     }
     else if (motor_commands[i][0] == 3)
     {
+      // Start moving pieces up
+      my_servo[i].write(100);
+      Serial.println("Reached Reset");
+      is_reset = true;
       // call reset function (still needs to be written)
     }
     else
@@ -39,59 +52,119 @@ void ProcessData()
     }
   }
 
-  bool go = true;
-  int total_turns = 0;
-
-  while (go == true)
+  if (is_reset == false)
   {
-    // subtract from motor rotations when a rotation is detected
-    for (int i = 0; i < NUMBER_MOTORS; i++)
+    while (go == true)
     {
-      motor_sensor_counter2[i] = motor_sensor_counter1[i];
-      motor_sensor_counter1[i] = CheckSwitch(i, ports[i][1]);
-      
-      if (motor_sensor_counter1[i] == 1 && motor_sensor_counter2[i] == 0)
+      // subtract from motor rotations when a rotation is detected
+      for (int i = 0; i < NUMBER_MOTORS; i++)
       {
-        motor_commands[i][1] = motor_commands[i][1] - 1;
+        motor_sensor_counter2[i] = motor_sensor_counter1[i];
+        motor_sensor_counter1[i] = CheckSwitch(i, ports[i][1]);
+
+        if (motor_sensor_counter1[i] == 1 && motor_sensor_counter2[i] == 0)
+        {
+          motor_commands[i][1] = motor_commands[i][1] - 1;
+        }
+
+        if (motor_commands[i][1] < 0)
+        {
+          motor_commands[i][1] = 0;
+        }
       }
+      // stop motors that have reached 0
+      for (int i = 0; i < NUMBER_MOTORS; i++)
+      {
+        if (motor_commands[i][1] <= 0)
+        {
+          my_servo[i].write(90);
+        }
+      }
+      // see how many turns are left in the array
+      for (int i = 0; i < NUMBER_MOTORS; i++)
+      {
+        total_turns += motor_commands[i][1];
+        // Serial.print("Total number of turns in array: ");
+        // Serial.println(total_turns);
+      }
+      // print the total number of turns left for each motor
+      for (int i = 0; i < NUMBER_MOTORS; i++)
+      {
+        Serial.print("Motor ");
+        Serial.print(i);
+        Serial.print(": ");
+        Serial.print(motor_commands[i][1]);
+        Serial.print(" --  ");
+      }
+      Serial.println("");
 
-      if (motor_commands[i][1] < 0)
+      // exit loop if there are no more motor rotations remaining
+      if (total_turns <= 0)
       {
-        motor_commands[i][1] = 0;
+        go = false;
       }
-    }
-    // stop motors that have reached 0
-    for (int i = 0; i < NUMBER_MOTORS; i++)
-    {
-      if (motor_commands[i][1] <= 0)
+      total_turns = 0;
+
+      // time out loop if stall
+      if (timeout_counter >= TIMEOUT)
       {
-        my_servo[i].write(90);
+        go = false;
+        for (int i = 0; i < NUMBER_MOTORS; i++)
+        {
+          my_servo[i].write(90);
+          Serial.println("Timeout");
+        }
       }
+      timeout_counter = timeout_counter + 1;
+      Serial.println(timeout_counter);
     }
-    // see how many turns are left in the array
-    for (int i = 0; i < NUMBER_MOTORS; i++)
-    {
-      total_turns += motor_commands[i][1];
-      // Serial.print("Total number of turns in array: ");
-      // Serial.println(total_turns);
-    }
-    // print the total number of turns left for each motor
-    for (int i = 0; i < NUMBER_MOTORS; i++)
-    {
-      Serial.print("Motor ");
-      Serial.print(i);
-      Serial.print(": ");
-      Serial.print(motor_commands[i][1]);
-      Serial.print(" --  ");
+  }
+  else
+  {
+    int reset_counter[NUMBER_MOTORS] = {0};
+
+    for (int i = 0; i < NUMBER_MOTORS; ++i){
+      reset_counter[i] = 1;
     }
-      Serial.println("");
 
-    // exit loop if there are no more motor rotations remaining
-    if (total_turns <= 0)
+    while (go == true)
     {
-      go = false;
+      // stop motors that have reached 0
+      for (int i = 0; i < NUMBER_MOTORS; i++)
+      {
+        if (digitalRead(ports[i][2]) == 0)
+        {
+          my_servo[i].write(90);
+          reset_counter[i] = 0;
+        }
+      }
+      // see how many turns are left in the array
+      for (int i = 0; i < NUMBER_MOTORS; i++)
+      {
+        total_turns += reset_counter[i];
+      }
+
+      Serial.print("Total Turns: ");
+      Serial.println(total_turns);
+
+      if (total_turns <= 0)
+      {
+        go = false;
+      }
+      total_turns = 0;
+
+      if (timeout_counter >= TIMEOUT)
+      {
+        go = false;
+        for (int i = 0; i < NUMBER_MOTORS; i++)
+        {
+          my_servo[i].write(90);
+          Serial.println("Timeout");
+        }
+      }
+      timeout_counter = timeout_counter + 1;
+      Serial.println(timeout_counter);
     }
-    total_turns = 0;
   }
   // Send Finished Signal
   Finished();