From e37d77fd80f5b23ee4443354bb6f41d8872200ba Mon Sep 17 00:00:00 2001 From: Andrea Date: Thu, 27 Feb 2025 16:55:47 +0100 Subject: [PATCH] first commit --- include/README | 39 +++ lib/README | 46 +++ lib/pixy/Pixy.h | 169 +++++++++++ lib/pixy/PixyI2C.h | 78 +++++ lib/pixy/PixySPI_SS.h | 127 ++++++++ lib/pixy/PixyUART.h | 74 +++++ lib/pixy/TPixy.h | 273 ++++++++++++++++++ lib/pixy/examples/hello_world/hello_world.ino | 70 +++++ lib/pixy/examples/i2c/i2c.ino | 63 ++++ lib/pixy/examples/led_cycle/led_cycle.ino | 54 ++++ lib/pixy/examples/pantilt/pantilt.ino | 119 ++++++++ lib/pixy/examples/servo_move/servo_move.ino | 44 +++ lib/pixy/examples/uart/uart.ino | 66 +++++ lib/pixy/keywords.txt | 11 + platformio.ini | 17 ++ src/main.cpp | 116 ++++++++ test/README | 11 + 17 files changed, 1377 insertions(+) create mode 100644 include/README create mode 100644 lib/README create mode 100644 lib/pixy/Pixy.h create mode 100644 lib/pixy/PixyI2C.h create mode 100644 lib/pixy/PixySPI_SS.h create mode 100644 lib/pixy/PixyUART.h create mode 100644 lib/pixy/TPixy.h create mode 100644 lib/pixy/examples/hello_world/hello_world.ino create mode 100644 lib/pixy/examples/i2c/i2c.ino create mode 100755 lib/pixy/examples/led_cycle/led_cycle.ino create mode 100755 lib/pixy/examples/pantilt/pantilt.ino create mode 100755 lib/pixy/examples/servo_move/servo_move.ino create mode 100644 lib/pixy/examples/uart/uart.ino create mode 100644 lib/pixy/keywords.txt create mode 100644 platformio.ini create mode 100644 src/main.cpp create mode 100644 test/README diff --git a/include/README b/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/lib/pixy/Pixy.h b/lib/pixy/Pixy.h new file mode 100644 index 0000000..18c8273 --- /dev/null +++ b/lib/pixy/Pixy.h @@ -0,0 +1,169 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the SPI-related classes. It's called Pixy.h instead +// of Pixy_SPI.h because it's the default/recommended communication method +// with Arduino. This class assumes you are using the ICSP connector to talk to +// Pixy from your Arduino. For more information go to: +// +//http://cmucam.org/projects/cmucam5/wiki/Hooking_up_Pixy_to_a_Microcontroller_(like_an_Arduino) +// + +#ifndef PIXY_H +#define PIXY_H + +#include "TPixy.h" +#include "SPI.h" + + +#define PIXY_SYNC_BYTE 0x5a +#define PIXY_SYNC_BYTE_DATA 0x5b +#define PIXY_BUF_SIZE 16 + +template struct CircularQ +{ + CircularQ() + { + len = 0; + writeIndex = 0; + readIndex = 0; + } + + bool read(BufType *c) + { + if (len) + { + *c = buf[readIndex++]; + len--; + if (readIndex==PIXY_BUF_SIZE) + readIndex = 0; + return true; + } + else + return false; + } + + uint8_t freeLen() + { + return PIXY_BUF_SIZE-len; + } + + bool write(BufType c) + { + if (freeLen()==0) + return false; + + buf[writeIndex++] = c; + len++; + if (writeIndex==PIXY_BUF_SIZE) + writeIndex = 0; + return true; + } + + BufType buf[PIXY_BUF_SIZE]; + uint8_t len; + uint8_t writeIndex; + uint8_t readIndex; +}; + +class LinkSPI +{ + public: + void init() + { + SPI.begin(); + + #ifdef __SAM3X8E__ + // DUE clock divider // + SPI.setClockDivider(84); + #else + // Default clock divider // + SPI.setClockDivider(SPI_CLOCK_DIV16); + #endif + } + + uint16_t getWord() + { + // ordering is different (big endian) because Pixy is sending 16 bits through SPI + // instead of 2 bytes in a 16-bit word as with I2C + uint16_t w; + + if (inQ.read(&w)) + return w; + + return getWordHw(); + } + + uint8_t getByte() + { + return SPI.transfer(0x00); + } + + int8_t send(uint8_t *data, uint8_t len) + { + int i; + + // check to see if we have enough space in our circular queue + if (outQ.freeLen() outQ; + CircularQ inQ; +}; + + +typedef TPixy Pixy; + +#endif diff --git a/lib/pixy/PixyI2C.h b/lib/pixy/PixyI2C.h new file mode 100644 index 0000000..be68ab5 --- /dev/null +++ b/lib/pixy/PixyI2C.h @@ -0,0 +1,78 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the link class for I2C communications. +// +// Note, the PixyI2C class takes an optional argument, which is the I2C address +// of the Pixy you want to talk to. The default address is 0x54 (used when no +// argument is used.) So, for example, if you wished to talk to Pixy at I2C +// address 0x55, declare like this: +// +// PixyI2C pixy(0x55); +// + +#ifndef _PIXYI2C_H +#define _PIXYI2C_H + +#include "TPixy.h" +#include "Wire.h" + +#define PIXY_I2C_DEFAULT_ADDR 0x54 + +class LinkI2C +{ +public: + void init() + { + Wire.begin(); + } + void setArg(uint16_t arg) + { + if (arg==PIXY_DEFAULT_ARGVAL) + addr = PIXY_I2C_DEFAULT_ADDR; + else + addr = arg; + } + uint16_t getWord() + { + uint16_t w; + uint8_t c; + Wire.requestFrom((int)addr, 2); + c = Wire.read(); + w = Wire.read(); + w <<= 8; + w |= c; + return w; + } + uint8_t getByte() + { + Wire.requestFrom((int)addr, 1); + return Wire.read(); + } + + int8_t send(uint8_t *data, uint8_t len) + { + Wire.beginTransmission(addr); + Wire.write(data, len); + Wire.endTransmission(); + return len; + } + +private: + uint8_t addr; +}; + +typedef TPixy PixyI2C; + +#endif diff --git a/lib/pixy/PixySPI_SS.h b/lib/pixy/PixySPI_SS.h new file mode 100644 index 0000000..850400d --- /dev/null +++ b/lib/pixy/PixySPI_SS.h @@ -0,0 +1,127 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the link class for SPI with Slave Select. The +// default communication for Arduino is through the ICSP connector, which uses +// SPI without a slave select. The LinkSPI_SS allows you to use a slave select +// so you can share the SPI port with other devices, or use multiple Pixys. +// +// Note, the PixySPI_SS class takes an optional argument, which is the pin +// number of the slave select signal you wish to use. The default pin is the +// SS pin (used when no argument is used.) So, for example, if you wished to +// use pin 14 for slave select, declare like this: +// +// PixySPI_SS pixy(14); +// + +#ifndef PIXYSPI_SS_H +#define PIXYSPI_SS_H + +#include "TPixy.h" +#include "SPI.h" + + +#define PIXY_SYNC_BYTE 0x5a +#define PIXY_SYNC_BYTE_DATA 0x5b +#define PIXY_OUTBUF_SIZE 6 + +class LinkSPI_SS +{ + public: + void init() + { + outLen = 0; + SPI.begin(); + + #ifdef __SAM3X8E__ + // DUE clock divider // + SPI.setClockDivider(84); + #else + // Default clock divider // + SPI.setClockDivider(SPI_CLOCK_DIV16); + #endif + } + + uint16_t getWord() + { + // ordering is different because Pixy is sending 16 bits through SPI + // instead of 2 bytes in a 16-bit word as with I2C + uint16_t w; + uint8_t c, cout = 0; + + // assert slave select + digitalWrite(ssPin, LOW); + + if (outLen) + { + w = SPI.transfer(PIXY_SYNC_BYTE_DATA); + cout = outBuf[outIndex++]; + if (outIndex==outLen) + outLen = 0; + } + else + w = SPI.transfer(0); + + w <<= 8; + c = SPI.transfer(cout); + w |= c; + + // negate slave select + digitalWrite(ssPin, HIGH); + return w; + } + + uint8_t getByte() // this shouldn't be called normally + // It should only be called if we get out of sync, but with slave select + // we should stay in sync + { + uint8_t c; + // assert slave select + digitalWrite(ssPin, LOW); + c = SPI.transfer(0x00); + // negate slave select + digitalWrite(ssPin, HIGH); + + return c; + } + + int8_t send(uint8_t *data, uint8_t len) + { + if (len>PIXY_OUTBUF_SIZE || outLen!=0) + return -1; + memcpy(outBuf, data, len); + outLen = len; + outIndex = 0; + return len; + } + + void setArg(uint16_t arg) + { + if (arg==PIXY_DEFAULT_ARGVAL) + ssPin = SS; // default slave select pin + else + ssPin = arg; + } + + private: + uint8_t outBuf[PIXY_OUTBUF_SIZE]; + uint8_t outLen; + uint8_t outIndex; + uint16_t ssPin; +}; + + +typedef TPixy PixySPI_SS; + +#endif diff --git a/lib/pixy/PixyUART.h b/lib/pixy/PixyUART.h new file mode 100644 index 0000000..8eb1a4b --- /dev/null +++ b/lib/pixy/PixyUART.h @@ -0,0 +1,74 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the link class for UART communications. +// + +#ifndef _PIXYUART_H +#define _PIXYUART_H + +#include "TPixy.h" +#include "Arduino.h" + +class LinkUART +{ +public: + void init() + { + Serial1.begin(19200); + } + void setArg(uint16_t arg) + { + } + uint16_t getWord() + { + int16_t u, v; + + while(1) + { + u = Serial1.read(); + if (u>=0) + break; + } + while(1) + { + v = Serial1.read(); + if (v>=0) + break; + } + v <<= 8; + v |= u&0xff; + return v; + } + uint8_t getByte() + { + int16_t u; + + while(1) + { + u = Serial1.read(); + if (u>=0) + break; + } + return (uint8_t)u; + } + int8_t send(uint8_t *data, uint8_t len) + { + return Serial1.write(data, len); + } +}; + +typedef TPixy PixyUART; + +#endif diff --git a/lib/pixy/TPixy.h b/lib/pixy/TPixy.h new file mode 100644 index 0000000..f336a67 --- /dev/null +++ b/lib/pixy/TPixy.h @@ -0,0 +1,273 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This file is for defining the Block struct and the Pixy template class. +// (TPixy). TPixy takes a communication link as a template parameter so that +// all communication modes (SPI, I2C and UART) can share the same code. +// + +#ifndef _TPIXY_H +#define _TPIXY_H + +#include "Arduino.h" + +// Communication/misc parameters +#define PIXY_INITIAL_ARRAYSIZE 30 +#define PIXY_MAXIMUM_ARRAYSIZE 130 +#define PIXY_START_WORD 0xaa55 +#define PIXY_START_WORD_CC 0xaa56 +#define PIXY_START_WORDX 0x55aa +#define PIXY_MAX_SIGNATURE 7 +#define PIXY_DEFAULT_ARGVAL 0xffff + +// Pixy x-y position values +#define PIXY_MIN_X 0L +#define PIXY_MAX_X 319L +#define PIXY_MIN_Y 0L +#define PIXY_MAX_Y 199L + +// RC-servo values +#define PIXY_RCS_MIN_POS 0L +#define PIXY_RCS_MAX_POS 1000L +#define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS-PIXY_RCS_MIN_POS)/2) + + +enum BlockType +{ + NORMAL_BLOCK, + CC_BLOCK +}; + +struct Block +{ + // print block structure! + void print() + { + int i, j; + char buf[128], sig[6], d; + bool flag; + if (signature>PIXY_MAX_SIGNATURE) // color code! (CC) + { + // convert signature number to an octal string + for (i=12, j=0, flag=false; i>=0; i-=3) + { + d = (signature>>i)&0x07; + if (d>0 && !flag) + flag = true; + if (flag) + sig[j++] = d + '0'; + } + sig[j] = '\0'; + sprintf(buf, "CC block! sig: %s (%d decimal) x: %d y: %d width: %d height: %d angle %d\n", sig, signature, x, y, width, height, angle); + } + else // regular block. Note, angle is always zero, so no need to print + sprintf(buf, "sig: %d x: %d y: %d width: %d height: %d\n", signature, x, y, width, height); + Serial.print(buf); + } + uint16_t signature; + uint16_t x; + uint16_t y; + uint16_t width; + uint16_t height; + uint16_t angle; +}; + + + +template class TPixy +{ +public: + TPixy(uint16_t arg=PIXY_DEFAULT_ARGVAL); + ~TPixy(); + + uint16_t getBlocks(uint16_t maxBlocks=1000); + int8_t setServos(uint16_t s0, uint16_t s1); + int8_t setBrightness(uint8_t brightness); + int8_t setLED(uint8_t r, uint8_t g, uint8_t b); + void init(); + + Block *blocks; + +private: + boolean getStart(); + void resize(); + + LinkType link; + boolean skipStart; + BlockType blockType; + uint16_t blockCount; + uint16_t blockArraySize; +}; + + +template TPixy::TPixy(uint16_t arg) +{ + skipStart = false; + blockCount = 0; + blockArraySize = PIXY_INITIAL_ARRAYSIZE; + blocks = (Block *)malloc(sizeof(Block)*blockArraySize); + link.setArg(arg); +} + +template void TPixy::init() +{ + link.init(); +} + +template TPixy::~TPixy() +{ + free(blocks); +} + +template boolean TPixy::getStart() +{ + uint16_t w, lastw; + + lastw = 0xffff; + + while(true) + { + w = link.getWord(); + if (w==0 && lastw==0) + { + delayMicroseconds(10); + return false; + } + else if (w==PIXY_START_WORD && lastw==PIXY_START_WORD) + { + blockType = NORMAL_BLOCK; + return true; + } + else if (w==PIXY_START_WORD_CC && lastw==PIXY_START_WORD) + { + blockType = CC_BLOCK; + return true; + } + else if (w==PIXY_START_WORDX) + { + Serial.println("reorder"); + link.getByte(); // resync + } + lastw = w; + } +} + +template void TPixy::resize() +{ + blockArraySize += PIXY_INITIAL_ARRAYSIZE; + blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize); +} + +template uint16_t TPixy::getBlocks(uint16_t maxBlocks) +{ + uint8_t i; + uint16_t w, checksum, sum; + Block *block; + + if (!skipStart) + { + if (getStart()==false) + return 0; + } + else + skipStart = false; + + for(blockCount=0; blockCountblockArraySize) + resize(); + + block = blocks + blockCount; + + for (i=0, sum=0; i=5) // skip + { + block->angle = 0; + break; + } + w = link.getWord(); + sum += w; + *((uint16_t *)block + i) = w; + } + + if (checksum==sum) + blockCount++; + else + Serial.println("cs error"); + + w = link.getWord(); + if (w==PIXY_START_WORD) + blockType = NORMAL_BLOCK; + else if (w==PIXY_START_WORD_CC) + blockType = CC_BLOCK; + else + return blockCount; + } +} + +template int8_t TPixy::setServos(uint16_t s0, uint16_t s1) +{ + uint8_t outBuf[6]; + + outBuf[0] = 0x00; + outBuf[1] = 0xff; + *(uint16_t *)(outBuf + 2) = s0; + *(uint16_t *)(outBuf + 4) = s1; + + return link.send(outBuf, 6); +} + +template int8_t TPixy::setBrightness(uint8_t brightness) +{ + uint8_t outBuf[3]; + + outBuf[0] = 0x00; + outBuf[1] = 0xfe; + outBuf[2] = brightness; + + return link.send(outBuf, 3); +} + +template int8_t TPixy::setLED(uint8_t r, uint8_t g, uint8_t b) +{ + uint8_t outBuf[5]; + + outBuf[0] = 0x00; + outBuf[1] = 0xfd; + outBuf[2] = r; + outBuf[3] = g; + outBuf[4] = b; + + return link.send(outBuf, 5); +} + +#endif diff --git a/lib/pixy/examples/hello_world/hello_world.ino b/lib/pixy/examples/hello_world/hello_world.ino new file mode 100644 index 0000000..59a5414 --- /dev/null +++ b/lib/pixy/examples/hello_world/hello_world.ino @@ -0,0 +1,70 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This sketch is a good place to start if you're just getting started with +// Pixy and Arduino. This program simply prints the detected object blocks +// (including color codes) through the serial console. It uses the Arduino's +// ICSP port. For more information go here: +// +// http://cmucam.org/projects/cmucam5/wiki/Hooking_up_Pixy_to_a_Microcontroller_(like_an_Arduino) +// +// It prints the detected blocks once per second because printing all of the +// blocks for all 50 frames per second would overwhelm the Arduino's serial port. +// + +#include +#include + +// This is the main Pixy object +Pixy pixy; + +void setup() +{ + Serial.begin(9600); + Serial.print("Starting...\n"); + + pixy.init(); +} + +void loop() +{ + static int i = 0; + int j; + uint16_t blocks; + char buf[32]; + + // grab blocks! + blocks = pixy.getBlocks(); + + // If there are detect blocks, print them! + if (blocks) + { + i++; + + // do this (print) every 50 frames because printing every + // frame would bog down the Arduino + if (i%50==0) + { + sprintf(buf, "Detected %d:\n", blocks); + Serial.print(buf); + for (j=0; j +#include + + +PixyI2C pixy; +// PixyI2C pixy(0x55); // You can set the I2C address through PixyI2C object + +void setup() +{ + Serial.begin(9600); + Serial.print("Starting...\n"); + + pixy.init(); +} + + +void loop() +{ + static int i = 0; + int j; + uint16_t blocks; + char buf[32]; + + blocks = pixy.getBlocks(); + + if (blocks) + { + i++; + + // do this (print) every 50 frames because printing every + // frame would bog down the Arduino + if (i%50==0) + { + sprintf(buf, "Detected %d:\n", blocks); + Serial.print(buf); + for (j=0; j +#include + +Pixy pixy; + +void setup() +{ + Serial.begin(9600); + Serial.print("Starting...\n"); + + pixy.init(); +} + +void loop() +{ + uint32_t i=0; + uint8_t r, g, b; + + while(1) + { + // calculate r, g, b such that it cycles through the colors + r = i&0xff; + g = (i*3)&0xff; + b = (i/3)&0xff; + pixy.setLED(r, g, b); + // We need to delay here because serial requests are handled + // every frame period (20ms). If we don't delay, we'll + // overrun Pixy's receive queue. But that's all OK because + // we normally only update the LED once per frame anyway. + delay(20); + + i++; + } +} + + diff --git a/lib/pixy/examples/pantilt/pantilt.ino b/lib/pixy/examples/pantilt/pantilt.ino new file mode 100755 index 0000000..409f304 --- /dev/null +++ b/lib/pixy/examples/pantilt/pantilt.ino @@ -0,0 +1,119 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This sketch is a simple tracking demo that uses the pan/tilt unit. For +// more information, go here: +// +// http://cmucam.org/projects/cmucam5/wiki/Run_the_Pantilt_Demo +// + +#include +#include + +Pixy pixy; + +#define X_CENTER ((PIXY_MAX_X-PIXY_MIN_X)/2) +#define Y_CENTER ((PIXY_MAX_Y-PIXY_MIN_Y)/2) + +class ServoLoop +{ +public: + ServoLoop(int32_t pgain, int32_t dgain); + + void update(int32_t error); + + int32_t m_pos; + int32_t m_prevError; + int32_t m_pgain; + int32_t m_dgain; +}; + + +ServoLoop panLoop(300, 500); +ServoLoop tiltLoop(500, 700); + +ServoLoop::ServoLoop(int32_t pgain, int32_t dgain) +{ + m_pos = PIXY_RCS_CENTER_POS; + m_pgain = pgain; + m_dgain = dgain; + m_prevError = 0x80000000L; +} + +void ServoLoop::update(int32_t error) +{ + long int vel; + char buf[32]; + if (m_prevError!=0x80000000) + { + vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10; + //sprintf(buf, "%ld\n", vel); + //Serial.print(buf); + m_pos += vel; + if (m_pos>PIXY_RCS_MAX_POS) + m_pos = PIXY_RCS_MAX_POS; + else if (m_pos +#include + +Pixy pixy; + +void setup() +{ + + Serial.begin(9600); + Serial.print("Starting...\n"); + + pixy.init(); +} + +void loop() +{ + Serial.println("Moving pan-tilt to max positions"); + pixy.setServos(PIXY_RCS_MAX_POS, PIXY_RCS_MAX_POS); + delay(1000); + + Serial.println("Moving pan-tilt to min positions"); + pixy.setServos(PIXY_RCS_MIN_POS, PIXY_RCS_MIN_POS); + delay(1000); +} + + diff --git a/lib/pixy/examples/uart/uart.ino b/lib/pixy/examples/uart/uart.ino new file mode 100644 index 0000000..46e0684 --- /dev/null +++ b/lib/pixy/examples/uart/uart.ino @@ -0,0 +1,66 @@ +// +// begin license header +// +// This file is part of Pixy CMUcam5 or "Pixy" for short +// +// All Pixy source code is provided under the terms of the +// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). +// Those wishing to use Pixy source code, software and/or +// technologies under different licensing terms should contact us at +// cmucam@cs.cmu.edu. Such licensing terms are available for +// all portions of the Pixy codebase presented here. +// +// end license header +// +// This sketch is like hello_world but uses UART communications. If you're +// not sure what UART is, run the hello_world sketch! +// +// Note, the default baudrate for Pixy's UART communications is 19200. Given +// the slow datarate and Arduino's shallow serial FIFO, this sletch sometimes +// gets checksum errors, when more than 1 block is present. This is because +// printing more than 1 object block to the serial console (as this sketch does) +// causes the Arduino's serial FIFO to overrun, which leads to communication +// errors. +// + +#include "PixyUART.h" + + +PixyUART pixy; + +void setup() +{ + Serial.begin(9600); // 9600 baud for the serial *console* (not for the UART connected to Pixy) + Serial.print("Starting...\n"); + + pixy.init(); +} + +void loop() +{ + static int i = 0; + int j; + uint16_t blocks; + char buf[32]; + + blocks = pixy.getBlocks(); + + if (blocks) + { + i++; + + // do this (print) every 50 frames because printing every + // frame would bog down the Arduino + if (i%50==0) + { + sprintf(buf, "Detected %d:\n", blocks); + Serial.print(buf); + for (j=0; j +#include +#include "Adafruit_BNO08x_RVC.h" +#include +#include "Ultrasonic.h" + +PixyI2C pixy; +Adafruit_BNO08x_RVC rvc = Adafruit_BNO08x_RVC(); +Ultrasonic uSensor1 = Ultrasonic(7); +Ultrasonic uSensor2 = Ultrasonic(10); +Ultrasonic uSensor3 = Ultrasonic(9); + +void setup() +{ + Wire.begin(); + pixy.init(); + + // Wait for serial monitor to open + Serial.begin(115200); + while (!Serial) + delay(10); + + Serial1.begin(115200); // This is the baud rate specified by the datasheet + while (!Serial1) + delay(10); + + if (!rvc.begin(&Serial1)) + { // connect to the sensor over hardware serial + while (1) + delay(10); + } + // Print labels for the values + Serial.print(F("Yaw")); + Serial.print(F("\tPitch")); + Serial.print(F("\tRoll")); + Serial.print(F("\tX")); + Serial.print(F("\tY")); + Serial.println(F("\tZ")); +} + +void loop() +{ + // Get IMU data + Wire.requestFrom(0x10 / 2, 2); + BNO08x_RVC_Data heading; + static int i = 0; + uint16_t blocks; + char buf[32]; + + while (rvc.read(&heading)) + { + int c = Wire.read(); // direction is the first byte + int strength = Wire.read(); + // Print IMU data + Serial.print("IMU - Direction: "); + Serial.print(c); + Serial.print(" Strength: "); + Serial.println(strength); + Serial.print("Heading: "); + Serial.print(heading.yaw); + Serial.print(","); + Serial.print(heading.pitch); + Serial.print(","); + Serial.print(heading.roll); + Serial.println(); + Serial.print("Acceleration: "); + Serial.print(heading.x_accel); + Serial.print(","); + Serial.print(heading.y_accel); + Serial.print(","); + Serial.print(heading.z_accel); + Serial.println(); + + // Get Ultrasonic sensor data + long uS1 = uSensor1.MeasureInCentimeters(); + Serial.print("Distance uS1: "); + Serial.print(uS1); + Serial.println(" cm"); + + long uS2 = uSensor2.MeasureInCentimeters(); + Serial.print("Distance uS2: "); + Serial.print(uS2); + Serial.println(" cm"); + + long uS3 = uSensor3.MeasureInCentimeters(); + Serial.print("Distance uS3: "); + Serial.print(uS3); + Serial.println(" cm"); + + // Get Pixy blocks + blocks = pixy.getBlocks(); + + if (blocks) + { + Serial.println("Detected blocks"); + i++; + // Print every 50 frames to avoid overwhelming serial + if (i % 50 == 0) + { + + // Print Pixy blocks + sprintf(buf, "Detected %d blocks:\n", blocks); + Serial.print(buf); + for (int j = 0; j < blocks; j++) + { + sprintf(buf, " block %d: ", j); + Serial.print(buf); + pixy.blocks[j].print(); + } + Serial.println(); + } + + delay(250); + } + } +} \ No newline at end of file diff --git a/test/README b/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html