Merge branch 'master' of makar:/var/www/git/lolcam
[lolcam] / ir_receive.ino
1 #include <MeMCore.h>
2 #include <IRLibRecv.h>
3 #include <IRLibDecodeBase.h>
4 #include <IRLib_P01_NEC.h>
5 #include <IRLibCombo.h>
6 #include "lolcam_globals.h"
7
8 // mCore lib
9 MeDCMotor motor_left(M1);
10 MeDCMotor motor_right(M2);
11
12 // IRLib
13 IRrecv myReceiver(2);
14 IRdecode myDecoder;
15
16 // led
17 MeRGBLed led(0, 30);
18 uint8_t r1 = 0;
19 uint8_t g1 = 0;
20 uint8_t b1 = 0;
21 uint8_t r2 = 0;
22 uint8_t g2 = 0;
23 uint8_t b2 = 0;
24
25 // increase every loop iteration
26 uint32_t t_offset = 0;
27
28 // queued commands
29 uint32_t command_queue[QUEUE_MAX_COUNT];
30 uint8_t command_queue_end_index = 0;
31
32 // set up servo that controls hand
33 MePort port(PORT_1);
34 Servo hand;
35 int16_t hand_pin = port.pin1();
36 uint8_t hand_state = 0;
37
38 // +----------------------------------------------------------------------------+
39 // | Set the RGB values for this robot, initialize Serial and IR receiver class |
40 // +----------------------------------------------------------------------------+
41 void init(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2)
42 {
43     Serial.begin(BAUDRATE);
44     myReceiver.enableIRIn();
45     myReceiver.blink13(true);
46     Serial.println("Infrared Receiver Decoder");
47     led.setpin(13);
48     led.setColorAt(0, r1, g1, b1);
49     led.setColorAt(1, r2, g2, b2);
50     // led.setColorAt(0, 0, 0, 0);
51     // led.setColorAt(1, 0, 0, 0);
52     led.show();
53     hand.attach(hand_pin);
54 }
55
56 // +--------------------------------------------------------------------------------------+
57 // | Extract values packed into a 32-bit command and assign them to the passed references |
58 // +--------------------------------------------------------------------------------------+
59 void extract_values_from_command(uint32_t command, uint16_t& time, int& left, int& right, int& left_sign, int& right_sign, uint8_t& robot_id,
60                                  uint8_t& flip, uint8_t& repeats, uint16_t& repeat_delay, uint8_t& swap, uint8_t& chain, uint8_t& hand_command)
61 {
62     // use bit masking to isolate the values packed into the command
63     hand_command = (command & 0x2000000) >> 25;
64     chain = (command & 0x1000000) >> 24;
65     swap = (command & 0x800000) >> 23;
66     repeat_delay = ((command & 0x780000) >> 19) * 128;
67     repeats = (command & 0x70000) >> 16;
68     flip = (command & 0x8000) >> 15;
69     robot_id = (command & 0x4000) >> 14;
70     right_sign = ((command & 0x2000) >> 13) ? -1 : 1;
71     left_sign = ((command & 0x1000) >> 12) ? -1 : 1;
72     right = ((command & 0xF00) >> 8) * 16 * right_sign;
73     left = ((command & 0xF0) >> 4) * 16 * left_sign;
74     time = (command & 0xF) * 64;
75
76     // print command values for debugging
77     Serial.print("Parsed");
78     Serial.print(" time: ");
79     Serial.print(time, DEC);
80     Serial.print(" left: ");
81     Serial.print(left, DEC);
82     Serial.print(" right: ");
83     Serial.print(right, DEC);
84     Serial.print(" left sign: ");
85     Serial.print(left_sign, DEC);
86     Serial.print(" right sign: ");
87     Serial.println(right_sign, DEC);
88     Serial.print("robot id: ");
89     Serial.print(robot_id, DEC);
90     Serial.print(" flip: ");
91     Serial.print(flip, DEC);
92     Serial.print(" repeats: ");
93     Serial.print(repeats, DEC);
94     Serial.print(" delay: ");
95     Serial.print(repeat_delay, DEC);
96     Serial.print(" swap: ");
97     Serial.print(swap, DEC);
98     Serial.print(" chain: ");
99     Serial.print(chain, DEC);
100     Serial.print(" hand command: ");
101     Serial.println(hand_command, DEC);
102 }
103
104 // +------------------------------------------------------------------------------------------------------------------------+
105 // | The main loop is passed the robot ID from the specific robot's main loop, so we know which robot is being controlled.  |
106 // | Check the IR receiver for a command, extract the values, add the command to the queue, and when we receive a command   |
107 // | that signals the end of the queue, apply each command in the queue to the robot's motors.                              |
108 // +------------------------------------------------------------------------------------------------------------------------+
109 void robot_move(uint8_t current_robot)
110 {
111     // left and right motor values
112     int left = 0;
113     int right = 0;
114     
115     // IRLib
116     if (myReceiver.getResults())
117     {
118         // read infrared value using IRLib
119         myDecoder.decode();
120         myDecoder.dumpResults(false);
121         uint32_t command = myDecoder.value;
122
123         // print as hex and as binary for debugging
124         Serial.print("Received IR command: ");
125         Serial.print(command, HEX);
126         Serial.print(" ");
127         Serial.println(command, BIN);
128
129         // extract the values
130         uint8_t chain, swap, repeats, flip, robot_id, hand_command;
131         uint16_t repeat_delay, time;
132         int right_sign, left_sign, right, left;
133         extract_values_from_command(command, time, left, right, left_sign, right_sign, robot_id, flip, repeats, repeat_delay,
134                                     swap, chain, hand_command);
135
136         // check if the robot this code is running on was indicated
137         if (current_robot == robot_id)
138         {
139             // add command to queue and set end index to next index unless we're already at the end 
140             Serial.println("Queuing received command");
141             command_queue[command_queue_end_index] = command;
142             if (command_queue_end_index < QUEUE_MAX_COUNT - 1)
143             {
144                 command_queue_end_index++;
145             }
146             Serial.print("Command queue end index is ");
147             Serial.println(command_queue_end_index, DEC);
148
149             // if the most recently received command is not chained to the next, run every command in the queue from
150             // the beginning and reset the end index
151             if (chain == 0)
152             {
153                 for (int ii = 0; ii < command_queue_end_index; ii++)
154                 {
155                     extract_values_from_command(command_queue[ii], time, left, right, left_sign, right_sign, robot_id, flip,
156                                                 repeats, repeat_delay, swap, chain, hand_command);
157
158                     // repeat the command depending on the repeat parameter in the command
159                     for (int repeat_ii = 0; repeat_ii < repeats + 1; repeat_ii++)
160                     {
161                         // debug moving
162                         Serial.print("moving ");
163                         Serial.print(time, DEC);
164                         Serial.print(" ");
165                         Serial.print(left, DEC);
166                         Serial.print(" ");
167                         Serial.print(right, DEC);
168                         Serial.print(" ");
169                         Serial.print(repeat_delay, DEC);
170                         Serial.print(" ");
171                         Serial.println(repeat_ii, DEC);
172
173                         // run the motors
174                         if (left != 0 || right != 0)
175                         {
176                             motor_left.run(left);
177                             motor_right.run(right);
178                             delay(time);
179                             motor_left.run(0);
180                             motor_right.run(0);
181                         }
182
183                         // flip the sign of the motor values
184                         if (flip)
185                         {
186                             left = -left;
187                             right = -right;
188                         }
189
190                         // swap the motor values
191                         if (swap)
192                         {
193                             int temp = left;
194                             left = right;
195                             right = temp;
196                         }
197
198                         // delay the next command
199                         if (repeat_delay > 0)
200                         {
201                             delay(repeat_delay);
202                         }
203
204                         // move hand if necessary
205                         if (hand_command != hand_state)
206                         {
207                             if (hand_command == HAND_OPEN)
208                             {
209                                 hand.write(180);
210                                 hand_state = HAND_OPEN;
211                             }
212                             else
213                             {
214                                 hand.write(0);
215                                 hand_state = HAND_CLOSED;
216                             }
217                             delay(500);
218                         }
219                     }
220                 }
221
222                 // reset the queue
223                 command_queue_end_index = 0;
224             }
225         }
226         myReceiver.enableIRIn();
227     }
228
229     // wait before looping if no move was made
230     if (left == 0 && right == 0)
231     {
232         delay(IR_RECEIVER_SLEEP);
233     }
234
235     // increase loop count
236     t_offset++;
237 }