1
2
3
4
5
6#include <Servo.h>
7
8Servo panServo;
9Servo tiltServo;
10
11byte redledPin = 2;
12byte yellowledPin = 3;
13byte greenledPin = 4;
14
15const byte buffSize = 40;
16char inputBuffer[buffSize];
17const char startMarker = '<';
18const char endMarker = '>';
19byte bytesRecvd = 0;
20boolean readInProgress = false;
21boolean newDataFromPC = false;
22
23float panServoAngle = 90.0;
24float tiltServoAngle = 90.0;
25int LED_state = 0;
26
27
28
29void setup() {
30 Serial.begin(115200);
31
32 panServo.attach(8);
33 tiltServo.attach(9);
34
35 pinMode(redledPin, OUTPUT);
36 pinMode(yellowledPin, OUTPUT);
37 pinMode(greenledPin, OUTPUT);
38
39
40 start_sequence();
41
42 delay(200);
43
44 Serial.println("<Hasta la vista baby>");
45}
46
47
48
49void loop() {
50 getDataFromPC();
51 replyToPC();
52 moveServo();
53 setLED();
54}
55
56
57
58void getDataFromPC() {
59
60
61
62 if(Serial.available() > 0) {
63
64 char x = Serial.read();
65
66 if (x == endMarker) {
67 readInProgress = false;
68 newDataFromPC = true;
69 inputBuffer[bytesRecvd] = 0;
70 processData();
71 }
72
73 if(readInProgress) {
74 inputBuffer[bytesRecvd] = x;
75 bytesRecvd ++;
76 if (bytesRecvd == buffSize) {
77 bytesRecvd = buffSize - 1;
78 }
79 }
80
81 if (x == startMarker) {
82 bytesRecvd = 0;
83 readInProgress = true;
84 }
85 }
86}
87
88
89
90void processData()
91{
92 char * strtokIndx;
93
94 strtokIndx = strtok(inputBuffer,",");
95 panServoAngle = atof(strtokIndx);
96
97 strtokIndx = strtok(NULL,",");
98 tiltServoAngle = atof(strtokIndx);
99
100 strtokIndx = strtok(NULL, ",");
101 LED_state = atoi(strtokIndx);
102}
103
104
105
106void replyToPC() {
107
108 if (newDataFromPC) {
109 newDataFromPC = false;
110 Serial.print("<");
111 Serial.print(panServo.read());
112 Serial.print(",");
113 Serial.print(tiltServo.read());
114 Serial.println(">");
115 }
116}
117
118
119
120void moveServo()
121{
122 panServo.write(panServoAngle);
123 tiltServo.write(tiltServoAngle);
124}
125
126void setLED()
127{
128 if(LED_state == 2){
129 digitalWrite(redledPin, LOW);
130 digitalWrite(yellowledPin, HIGH);
131 digitalWrite(greenledPin, LOW);
132 }
133 else if(LED_state == 1){
134 digitalWrite(redledPin, LOW);
135 digitalWrite(yellowledPin, LOW);
136 digitalWrite(greenledPin, HIGH);
137 }
138 else if(LED_state == 0){
139 digitalWrite(redledPin, HIGH);
140 digitalWrite(yellowledPin, LOW);
141 digitalWrite(greenledPin, LOW);
142 }
143 else if(LED_state == 3){
144 digitalWrite(redledPin, HIGH);
145 digitalWrite(yellowledPin, HIGH);
146 digitalWrite(greenledPin, HIGH);
147 }
148 else{
149 digitalWrite(redledPin, LOW);
150 digitalWrite(yellowledPin, LOW);
151 digitalWrite(greenledPin, LOW);
152 }
153
154 }
155
156
157
158 void start_sequence()
159 {
160 panServo.write(90);
161 tiltServo.write(90);
162 delay(300);
163
164
165 digitalWrite(redledPin, HIGH);
166 delay(100);
167 digitalWrite(redledPin, LOW);
168 digitalWrite(yellowledPin, HIGH);
169 delay(100);
170 digitalWrite(yellowledPin, LOW);
171 digitalWrite(greenledPin, HIGH);
172 delay(100);
173
174 digitalWrite(redledPin, LOW);
175 digitalWrite(yellowledPin, LOW);
176 digitalWrite(greenledPin, LOW);
177 delay(100);
178 digitalWrite(redledPin, HIGH);
179 digitalWrite(yellowledPin, HIGH);
180 digitalWrite(greenledPin, HIGH);
181 delay(100);
182 digitalWrite(redledPin, LOW);
183 digitalWrite(yellowledPin, LOW);
184 digitalWrite(greenledPin, LOW);
185 }
186