source: rtos_arduino/trunk/examples/PIXY_Zumo_Wifi/rca_app.cpp@ 236

Last change on this file since 236 was 236, checked in by ertl-honda, 8 years ago

PIXY関連のファイルの追加
File size: 10.1 KB
Line 
1#include "rca.h"
2#include "ESP8266.h"
3
4#include <Wire.h>
5#include <ZumoShield.h>
6
7#include <SPI.h>
8#include <Pixy.h>
9
10#include "../examples_gdef.h"
11
12#define WMODE_STATION
13
14uint16_t pixy_blocks = 0;
15char pixy_buf[16][128];
16
17//==========================================================================
18//
19// Pixy Pet Robot
20//
21// Adafruit invests time and resources providing this open source code,
22// please support Adafruit and open-source hardware by purchasing
23// products from Adafruit!
24//
25// Written by: Bill Earl for Adafruit Industries
26//
27//==========================================================================
28// begin license header
29//
30// All Pixy Pet source code is provided under the terms of the
31// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
32//
33// end license header
34//
35//==========================================================================
36//
37// Portions of this code are derived from the Pixy CMUcam5 pantilt example code.
38//
39//==========================================================================
40
41#define X_CENTER 160L
42#define Y_CENTER 100L
43#define RCS_MIN_POS 0L
44#define RCS_MAX_POS 1000L
45#define RCS_CENTER_POS ((RCS_MAX_POS-RCS_MIN_POS)/2)
46
47//---------------------------------------
48// Servo Loop Class
49// A Proportional/Derivative feedback
50// loop for pan/tilt servo tracking of
51// blocks.
52// (Based on Pixy CMUcam5 example code)
53//---------------------------------------
54class ServoLoop
55{
56public:
57 ServoLoop(int32_t proportionalGain, int32_t derivativeGain);
58
59 void update(int32_t error);
60
61 int32_t m_pos;
62 int32_t m_prevError;
63 int32_t m_proportionalGain;
64 int32_t m_derivativeGain;
65};
66
67// ServoLoop Constructor
68ServoLoop::ServoLoop(int32_t proportionalGain, int32_t derivativeGain)
69{
70 m_pos = RCS_CENTER_POS;
71 m_proportionalGain = proportionalGain;
72 m_derivativeGain = derivativeGain;
73 m_prevError = 0x80000000L;
74}
75
76// ServoLoop Update
77// Calculates new output based on the measured
78// error and the current state.
79void ServoLoop::update(int32_t error)
80{
81 long int velocity;
82 char buf[32];
83 if (m_prevError!=0x80000000)
84 {
85 velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;
86
87 m_pos += velocity;
88 if (m_pos>RCS_MAX_POS)
89 {
90 m_pos = RCS_MAX_POS;
91 }
92 else if (m_pos<RCS_MIN_POS)
93 {
94 m_pos = RCS_MIN_POS;
95 }
96 }
97 m_prevError = error;
98}
99// End Servo Loop Class
100//---------------------------------------
101
102Pixy pixy; // Declare the camera object
103
104ServoLoop panLoop(200, 200); // Servo loop for pan
105ServoLoop tiltLoop(150, 200); // Servo loop for tilt
106
107void ScanForBlocks();
108void FollowBlock(int trackedBlock);
109int TrackBlock(int blockCount);
110
111//---------------------------------------
112// Setup - runs once at startup
113//---------------------------------------
114void setup()
115{
116 ZumoInit();
117 buzzer.playOn();
118 Serial.begin(115200);
119 Serial.print("Starting...\n");
120 pixy.init();
121
122 button.waitForPress();
123 buzzer.playStart();
124}
125
126uint32_t lastBlockTime = 0;
127
128//---------------------------------------
129// Main loop - runs continuously after setup
130//---------------------------------------
131void loop()
132{
133 uint16_t blocks;
134 blocks = pixy.getBlocks();
135
136 // If we have blocks in sight, track and follow them
137 if (blocks)
138 {
139 int trackedBlock = TrackBlock(blocks);
140 FollowBlock(trackedBlock);
141 lastBlockTime = millis();
142 if(pixy_blocks == 0) {
143 if (blocks > 16) {
144 blocks = 16;
145 }
146 pixy_blocks = blocks;
147 for (int j=0; j<blocks; j++){
148 sprintf(pixy_buf[j], "sig: %d x: %d y: %d width: %d height: %d\n",
149 pixy.blocks[j].signature, pixy.blocks[j].x, pixy.blocks[j].y,
150 pixy.blocks[j].width, pixy.blocks[j].height);
151 }
152 }
153 delay(1);
154 }
155 else if (millis() - lastBlockTime > 100)
156 {
157 motors.setLeftSpeed(0);
158 motors.setRightSpeed(0);
159 ScanForBlocks();
160 }
161}
162
163int oldX, oldY, oldSignature;
164
165//---------------------------------------
166// Track blocks via the Pixy pan/tilt mech
167// (based in part on Pixy CMUcam5 pantilt example)
168//---------------------------------------
169int TrackBlock(int blockCount)
170{
171 int trackedBlock = 0;
172 long maxSize = 0;
173
174 Serial.print("blocks =");
175 Serial.println(blockCount);
176
177 for (int i = 0; i < blockCount; i++)
178 {
179 if ((oldSignature == 0) || (pixy.blocks[i].signature == oldSignature))
180 {
181 long newSize = pixy.blocks[i].height * pixy.blocks[i].width;
182 if (newSize > maxSize)
183 {
184 trackedBlock = i;
185 maxSize = newSize;
186 }
187 }
188 }
189
190 int32_t panError = X_CENTER - pixy.blocks[trackedBlock].x;
191 int32_t tiltError = pixy.blocks[trackedBlock].y - Y_CENTER;
192
193 panLoop.update(panError);
194 tiltLoop.update(tiltError);
195
196 pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
197
198 oldX = pixy.blocks[trackedBlock].x;
199 oldY = pixy.blocks[trackedBlock].y;
200 oldSignature = pixy.blocks[trackedBlock].signature;
201 return trackedBlock;
202}
203
204//---------------------------------------
205// Follow blocks via the Zumo robot drive
206//
207// This code makes the robot base turn
208// and move to follow the pan/tilt tracking
209// of the head.
210//---------------------------------------
211int32_t size = 400;
212void FollowBlock(int trackedBlock)
213{
214 int32_t followError = RCS_CENTER_POS - panLoop.m_pos; // How far off-center are we looking now?
215
216 // Size is the area of the object.
217 // We keep a running average of the last 8.
218 size += pixy.blocks[trackedBlock].width * pixy.blocks[trackedBlock].height;
219 size -= size >> 3;
220
221 // Forward speed decreases as we approach the object (size is larger)
222 int forwardSpeed = constrain(400 - (size/256), -100, 400);
223
224 // Steering differential is proportional to the error times the forward speed
225 int32_t differential = (followError + (followError * forwardSpeed))>>8;
226
227 // Adjust the left and right speeds by the steering differential.
228 int leftSpeed = constrain(forwardSpeed + differential, -400, 400);
229 int rightSpeed = constrain(forwardSpeed - differential, -400, 400);
230
231 // And set the motor speeds
232 motors.setLeftSpeed(leftSpeed);
233 motors.setRightSpeed(rightSpeed);
234}
235
236//---------------------------------------
237// Random search for blocks
238//
239// This code pans back and forth at random
240// until a block is detected
241//---------------------------------------
242int scanIncrement = (RCS_MAX_POS - RCS_MIN_POS) / 150;
243uint32_t lastMove = 0;
244
245void ScanForBlocks()
246{
247 if (millis() - lastMove > 20)
248 {
249 lastMove = millis();
250 panLoop.m_pos += scanIncrement;
251 if ((panLoop.m_pos >= RCS_MAX_POS)||(panLoop.m_pos <= RCS_MIN_POS))
252 {
253 tiltLoop.m_pos = random(RCS_MAX_POS * 0.6, RCS_MAX_POS);
254 scanIncrement = -scanIncrement;
255 if (scanIncrement < 0)
256 {
257 motors.setLeftSpeed(-250);
258 motors.setRightSpeed(250);
259 }
260 else
261 {
262 motors.setLeftSpeed(+180);
263 motors.setRightSpeed(-180);
264 }
265 delay(random(250, 500));
266 }
267
268 pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
269 }
270}
271
272
273bool setup_done = false;
274
275void task1_setup()
276{
277 int ret;
278
279 Serial.println("Echo Server : Start!");
280 ret = WiFi.begin(Serial5, 115200);
281
282 if(ret == 1) {
283 Serial.print("Cannot communicate with ESP8266.");
284 while(1);
285 } else if(ret == 2) {
286 Serial.println("FW Version mismatch.");
287 Serial.print("FW Version:");
288 Serial.println(WiFi.getVersion().c_str());
289 Serial.print("Supported FW Version:");
290 Serial.print(ESP8266_SUPPORT_VERSION);
291 while(1);
292 } else {
293 Serial.print("begin ok\r\n");
294 }
295
296 Serial.print("FW Version:");
297 Serial.println(WiFi.getVersion().c_str());
298
299#ifdef WMODE_STATION
300 if (WiFi.setOprToStation()) {
301 Serial.print("to station ok\r\n");
302 } else {
303 Serial.print("to station err\r\n");
304 }
305
306 if (WiFi.joinAP(STA_SSID, STA_PASSWORD)) {
307 Serial.print("Join AP success\r\n");
308 Serial.print("IP: ");
309 Serial.println(WiFi.getLocalIP().c_str());
310 } else {
311 Serial.print("Join AP failure\r\n");
312 while(1);
313 }
314#else /* !WMODE_STATION */
315 if (WiFi.setOprToSoftAP()) {
316 Serial.print("to softap ok\r\n");
317 } else {
318 Serial.print("to softap err\r\n");
319 }
320 if(WiFi.setSoftAPParam(AP_SSID, AP_PASSWORD, 7, 0)){
321 Serial.print("Set SoftAP success\r\n");
322 Serial.print("IP: ");
323 Serial.println(WiFi.getLocalIP().c_str());
324 }
325 else {
326 Serial.print("Set SoftAP failure\r\n");
327 }
328#endif /* WMODE_STATION */
329
330 if (WiFi.enableMUX()) {
331 Serial.print("multiple ok\r\n");
332 } else {
333 Serial.print("multiple err\r\n");
334 while(1);
335 }
336
337 if (WiFi.startTCPServer(80)) {
338 Serial.print("start tcp server ok\r\n");
339 } else {
340 Serial.print("start tcp server err\r\n");
341 while(1);
342 }
343
344 if (WiFi.setTCPServerTimeout(60)) {
345 Serial.print("set tcp server timout 60 seconds\r\n");
346 } else {
347 Serial.print("set tcp server timout err\r\n");
348 while(1);
349 }
350
351 Serial.print("setup end\r\n");
352
353 setup_done = true;
354}
355
356#define MUX_NULL 0xff
357
358uint8_t mux_id_ptn;
359
360uint8_t mux_id = MUX_NULL;
361uint8_t task1_mux_id = MUX_NULL;
362
363void task1_loop()
364{
365 char buf[128];
366 uint8_t pre_mux_id_ptn;
367
368 /* Check Connection Status */
369 pre_mux_id_ptn = mux_id_ptn;
370
371 if(!WiFi.getMuxCStatus(&mux_id_ptn)) {
372 Serial.println("getMuxCStatus(&mux_id_ptn) : Error!");
373 }
374 else {
375 if (pre_mux_id_ptn != mux_id_ptn) {
376 Serial.print("Connection Status changed! : 0x");
377 Serial.println(mux_id_ptn, HEX);
378 if (mux_id_ptn & 0x01) {
379 mux_id = 0;
380 }
381 if (mux_id_ptn & 0x02) {
382 task1_mux_id = 1;
383 }
384 }
385 }
386
387 if (mux_id == MUX_NULL) {
388 return;
389 }
390
391 if (!WiFi.isConnected(mux_id)) {
392 Serial.print("Echo Server : Port is closed: ");
393 Serial.println(mux_id);
394 mux_id = MUX_NULL;
395 return;
396 }
397
398 if(pixy_blocks != 0) {
399 sprintf(buf, "Detected %d:\n", pixy_blocks);
400 WiFi.send(mux_id, (uint8_t*)buf, strlen(buf));
401 for (int j=0; j<pixy_blocks; j++){
402 WiFi.send(mux_id, (uint8_t*)pixy_buf[j], strlen(pixy_buf[j]));
403 }
404 pixy_blocks = 0;
405 }
406}
Note: See TracBrowser for help on using the repository browser.