enable led headlights
[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
19 void init(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2)
20 {
21     Serial.begin(BAUDRATE);
22     myReceiver.enableIRIn();
23     myReceiver.blink13(true);
24     Serial.println("Infrared Receiver Decoder");
25     led.setpin(13);
26     led.setColorAt(0, r1, g1, b1);
27     led.setColorAt(1, r2, g2, b2);
28     // led.setColorAt(0, 0, 0, 0);
29     // led.setColorAt(1, 0, 0, 0);
30     led.show();
31 }
32
33 void robot_move(uint8_t current_robot)
34 {
35     int left = 0;
36     int right = 0;
37     // IRLib
38     if (myReceiver.getResults())
39     {
40         myDecoder.decode();
41         myDecoder.dumpResults(false);
42         uint8_t power = 230;
43         uint16_t move_ms = 500;
44         uint32_t command = myDecoder.value;
45         Serial.print("Received IR command: ");
46         Serial.print(command, HEX);
47         Serial.print(" ");
48         Serial.println(command, BIN);
49         uint8_t swap = (command & 0x800000) >> 23;
50         uint16_t repeat_delay = ((command & 0x780000) >> 19) * 128;
51         uint8_t repeats = (command & 0x70000) >> 16;
52         uint8_t flip = (command & 0x8000) >> 15;
53         uint8_t robot_id = (command & 0x4000) >> 14;
54         int right_sign = ((command & 0x2000) >> 13) ? -1 : 1;
55         int left_sign = ((command & 0x1000) >> 12) ? -1 : 1;
56         int right = ((command & 0xF00) >> 8) * 16 * right_sign;
57         int left = ((command & 0xF0) >> 4) * 16 * left_sign;
58         uint16_t time = (command & 0xF) * 64;
59         Serial.print("Parsed");
60         Serial.print(" time: ");
61         Serial.print(time, DEC);
62         Serial.print(" left: ");
63         Serial.print(left, DEC);
64         Serial.print(" right: ");
65         Serial.print(right, DEC);
66         Serial.print(" left sign: ");
67         Serial.print(left_sign, DEC);
68         Serial.print(" right sign: ");
69         Serial.println(right_sign, DEC);
70         Serial.print("robot id: ");
71         Serial.print(robot_id, DEC);
72         Serial.print(" flip: ");
73         Serial.print(flip, DEC);
74         Serial.print(" repeats: ");
75         Serial.print(repeats, DEC);
76         Serial.print(" delay: ");
77         Serial.print(repeat_delay, DEC);
78         Serial.print(" swap: ");
79         Serial.println(swap, DEC);
80         if (current_robot == robot_id)
81         {
82             for (int repeat_ii = 0; repeat_ii < repeats + 1; repeat_ii++)
83             {
84                 Serial.print("moving ");
85                 Serial.print(time, DEC);
86                 Serial.print(" ");
87                 Serial.print(left, DEC);
88                 Serial.print(" ");
89                 Serial.print(right, DEC);
90                 Serial.print(" ");
91                 Serial.print(repeat_delay, DEC);
92                 Serial.print(" ");
93                 Serial.println(repeat_ii, DEC);
94                 if (left != 0 || right != 0)
95                 {
96                     motor_left.run(left);
97                     motor_right.run(right);
98                     delay(time);
99                     motor_left.run(0);
100                     motor_right.run(0);
101                 }
102                 if (flip)
103                 {
104                     left = -left;
105                     right = -right;
106                 }
107                 if (swap)
108                 {
109                     int temp = left;
110                     left = right;
111                     right = temp;
112                 }
113                 if (repeat_delay > 0)
114                 {
115                     delay(repeat_delay);
116                 }
117             }
118             myReceiver.enableIRIn();
119         }
120     }
121     if (left == 0 && right == 0)
122     {
123         delay(IR_RECEIVER_SLEEP);
124     }
125 }