adding pink trombone, rfid arduino code, camera views rotater, !go and !turn commands
[lolcam] / ir_receive.ino
index 1ea4fad..d44bca6 100644 (file)
@@ -1,5 +1,5 @@
 #include <MeMCore.h>
-#include <IRLibRecv.h>
+#include <IRLibRecvPCI.h>
 #include <IRLibDecodeBase.h>
 #include <IRLib_P01_NEC.h>
 #include <IRLibCombo.h>
@@ -10,10 +10,11 @@ MeDCMotor motor_left(M1);
 MeDCMotor motor_right(M2);
 
 // IRLib
-IRrecv myReceiver(2);
+// IRrecvPCI may be more accurate than IRrecv (see 2.2.2 of IRLib manual)
+IRrecvPCI myReceiver(2);
 IRdecode myDecoder;
 
-// led
+// LED
 MeRGBLed led(0, 30);
 uint8_t r1 = 0;
 uint8_t g1 = 0;
@@ -33,7 +34,7 @@ uint8_t command_queue_end_index = 0;
 MePort port(PORT_1);
 Servo hand;
 int16_t hand_pin = port.pin1();
-uint8_t hand_state = 0;
+uint8_t hand_state = HAND_OPEN;
 
 // +----------------------------------------------------------------------------+
 // | Set the RGB values for this robot, initialize Serial and IR receiver class |
@@ -44,6 +45,7 @@ void init(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2
     myReceiver.enableIRIn();
     myReceiver.blink13(true);
     Serial.println("Infrared Receiver Decoder");
+    print_separator();
     led.setpin(13);
     led.setColorAt(0, r1, g1, b1);
     led.setColorAt(1, r2, g2, b2);
@@ -51,6 +53,32 @@ void init(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2
     // led.setColorAt(1, 0, 0, 0);
     led.show();
     hand.attach(hand_pin);
+    set_hand(HAND_OPEN);
+}
+
+// +-------------------------------------------------+
+// | Print a horizontal rule to separate information |
+// +-------------------------------------------------+
+void print_separator()
+{
+    Serial.println("#######################################");
+}
+
+// +----------------------------------------------+
+// | Set hand servo to fully open or fully closed |
+// +----------------------------------------------+
+void set_hand(uint8_t state)
+{
+    if (state == HAND_OPEN)
+    {
+        hand.write(0);
+        hand_state = HAND_OPEN;
+    }
+    else
+    {
+        hand.write(180);
+        hand_state = HAND_CLOSED;
+    }
 }
 
 // +--------------------------------------------------------------------------------------+
@@ -60,7 +88,7 @@ void extract_values_from_command(uint32_t command, uint16_t& time, int& left, in
                                  uint8_t& flip, uint8_t& repeats, uint16_t& repeat_delay, uint8_t& swap, uint8_t& chain, uint8_t& hand_command)
 {
     // use bit masking to isolate the values packed into the command
-    hand_command = (command & 0x4000000) >> 25;
+    hand_command = (command & 0x2000000) >> 25;
     chain = (command & 0x1000000) >> 24;
     swap = (command & 0x800000) >> 23;
     repeat_delay = ((command & 0x780000) >> 19) * 128;
@@ -115,113 +143,112 @@ void robot_move(uint8_t current_robot)
     // IRLib
     if (myReceiver.getResults())
     {
-        // read infrared value using IRLib
-        myDecoder.decode();
-        myDecoder.dumpResults(false);
-        uint32_t command = myDecoder.value;
-
-        // print as hex and as binary for debugging
-        Serial.print("Received IR command: ");
-        Serial.print(command, HEX);
-        Serial.print(" ");
-        Serial.println(command, BIN);
-
-        // extract the values
-        uint8_t chain, swap, repeats, flip, robot_id, hand_command;
-        uint16_t repeat_delay, time;
-        int right_sign, left_sign, right, left;
-        extract_values_from_command(command, time, left, right, left_sign, right_sign, robot_id, flip, repeats, repeat_delay,
-                                    swap, chain, hand_command);
-
-        // check if the robot this code is running on was indicated
-        if (current_robot == robot_id)
+        // read infrared value using IRLib, decode will return true if it recognizes the protocol
+        if (myDecoder.decode() && myDecoder.protocolNum == NEC)
         {
-            // add command to queue and set end index to next index unless we're already at the end 
-            Serial.println("Queuing received command");
-            command_queue[command_queue_end_index] = command;
-            if (command_queue_end_index < QUEUE_MAX_COUNT - 1)
-            {
-                command_queue_end_index++;
-            }
-            Serial.print("Command queue end index is ");
-            Serial.println(command_queue_end_index, DEC);
+            myDecoder.dumpResults(false);
+            uint32_t command = myDecoder.value;
+
+            // print as hex and as binary for debugging
+            Serial.print("Received IR command: 0x");
+            Serial.print(command, HEX);
+            Serial.print(" 0b");
+            Serial.println(command, BIN);
+
+            // extract the values
+            uint8_t chain, swap, repeats, flip, robot_id, hand_command;
+            uint16_t repeat_delay, time;
+            int right_sign, left_sign, right, left;
+            extract_values_from_command(command, time, left, right, left_sign, right_sign, robot_id, flip, repeats, repeat_delay,
+                                        swap, chain, hand_command);
 
-            // if the most recently received command is not chained to the next, run every command in the queue from
-            // the beginning and reset the end index
-            if (chain == 0)
+            // check if the robot this code is running on was indicated
+            if (current_robot == robot_id)
             {
-                for (int ii = 0; ii < command_queue_end_index; ii++)
+                // add command to queue and set end index to next index unless we're already at the end 
+                Serial.println("Queuing received command");
+                command_queue[command_queue_end_index] = command;
+                if (command_queue_end_index < QUEUE_MAX_COUNT - 1)
                 {
-                    extract_values_from_command(command_queue[ii], time, left, right, left_sign, right_sign, robot_id, flip,
-                                                repeats, repeat_delay, swap, chain, hand_command);
+                    command_queue_end_index++;
+                }
+                Serial.print("Command queue end index is ");
+                Serial.println(command_queue_end_index, DEC);
 
-                    // repeat the command depending on the repeat parameter in the command
-                    for (int repeat_ii = 0; repeat_ii < repeats + 1; repeat_ii++)
+                // if the most recently received command is not chained to the next, run every command in the queue from
+                // the beginning and reset the end index
+                if (chain == 0)
+                {
+                    for (int ii = 0; ii < command_queue_end_index; ii++)
                     {
-                        // debug moving
-                        Serial.print("moving ");
-                        Serial.print(time, DEC);
-                        Serial.print(" ");
-                        Serial.print(left, DEC);
-                        Serial.print(" ");
-                        Serial.print(right, DEC);
-                        Serial.print(" ");
-                        Serial.print(repeat_delay, DEC);
-                        Serial.print(" ");
-                        Serial.println(repeat_ii, DEC);
-
-                        // run the motors
-                        if (left != 0 || right != 0)
-                        {
-                            motor_left.run(left);
-                            motor_right.run(right);
-                            delay(time);
-                            motor_left.run(0);
-                            motor_right.run(0);
-                        }
+                        extract_values_from_command(command_queue[ii], time, left, right, left_sign, right_sign, robot_id, flip,
+                                                    repeats, repeat_delay, swap, chain, hand_command);
 
-                        // flip the sign of the motor values
-                        if (flip)
+                        // repeat the command depending on the repeat parameter in the command
+                        for (int repeat_ii = 0; repeat_ii < repeats + 1; repeat_ii++)
                         {
-                            left = -left;
-                            right = -right;
-                        }
+                            // debug moving
+                            Serial.print("moving ");
+                            Serial.print(time, DEC);
+                            Serial.print(" ");
+                            Serial.print(left, DEC);
+                            Serial.print(" ");
+                            Serial.print(right, DEC);
+                            Serial.print(" ");
+                            Serial.print(repeat_delay, DEC);
+                            Serial.print(" ");
+                            Serial.println(repeat_ii, DEC);
 
-                        // swap the motor values
-                        if (swap)
-                        {
-                            int temp = left;
-                            left = right;
-                            right = temp;
-                        }
+                            // run the motors
+                            if (left != 0 || right != 0)
+                            {
+                                motor_left.run(left);
+                                motor_right.run(right);
+                                delay(time);
+                                motor_left.run(0);
+                                motor_right.run(0);
+                            }
 
-                        // delay the next command
-                        if (repeat_delay > 0)
-                        {
-                            delay(repeat_delay);
-                        }
+                            // flip the sign of the motor values
+                            if (flip)
+                            {
+                                left = -left;
+                                right = -right;
+                            }
 
-                        // move hand if necessary
-                        if (hand_command != hand_state)
-                        {
-                            if (hand_command == HAND_OPEN)
+                            // swap the motor values
+                            if (swap)
                             {
-                                hand.write(180);
-                                hand_state = HAND_OPEN;
+                                int temp = left;
+                                left = right;
+                                right = temp;
                             }
-                            else
+
+                            // delay the next command
+                            if (repeat_delay > 0)
+                            {
+                                delay(repeat_delay);
+                            }
+
+                            // switch hand state
+                            if (hand_command == HAND_TOGGLE)
                             {
-                                hand.write(0);
-                                hand_state = HAND_CLOSED;
+                                set_hand(!hand_state);
+                                delay(500);
                             }
                         }
                     }
-                }
 
-                // reset the queue
-                command_queue_end_index = 0;
+                    // reset the queue
+                    command_queue_end_index = 0;
+                }
             }
         }
+        else
+        {
+            Serial.println("Received an unknown protocol");
+        }
+        print_separator();
         myReceiver.enableIRIn();
     }