andrea/elbestia-robot
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

first commit

Browse source
This commit is contained in:
Andrea Moro 2025-02-27 16:55:47 +01:00
parent c5c86b57df
commit e37d77fd80
17 changed files with 1377 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

39
include/README Normal file
View file

@ -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

46
lib/README Normal file
View file

@ -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 <Foo.h>
#include <Bar.h>
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

169
lib/pixy/Pixy.h Normal file
View file

@ -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 <class BufType> 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()<len)
return -1;
for (i=0; i<len; i++)
outQ.write(data[i]);
flushSend();
return len;
}
void setArg(uint16_t arg)
{
}
private:
uint16_t getWordHw()
{
// 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;
uint8_t c, cout = 0;
if (outQ.read(&cout))
w = SPI.transfer(PIXY_SYNC_BYTE_DATA);
else
w = SPI.transfer(PIXY_SYNC_BYTE);
w <<= 8;
c = SPI.transfer(cout);
w |= c;
return w;
}
void flushSend()
{
uint16_t w;
while(outQ.len)
{
w = getWordHw();
inQ.write(w);
}
}
// we need a little circular queues for both directions
CircularQ<uint8_t> outQ;
CircularQ<uint16_t> inQ;
};
typedef TPixy<LinkSPI> Pixy;
#endif

78
lib/pixy/PixyI2C.h Normal file
View file

@ -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<LinkI2C> PixyI2C;
#endif

127
lib/pixy/PixySPI_SS.h Normal file
View file

@ -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<LinkSPI_SS> PixySPI_SS;
#endif

74
lib/pixy/PixyUART.h Normal file
View file

@ -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<LinkUART> PixyUART;
#endif

273
lib/pixy/TPixy.h Normal file
View file

@ -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 LinkType> 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 <class LinkType> TPixy<LinkType>::TPixy(uint16_t arg)
{
skipStart = false;
blockCount = 0;
blockArraySize = PIXY_INITIAL_ARRAYSIZE;
blocks = (Block *)malloc(sizeof(Block)*blockArraySize);
link.setArg(arg);
}
template <class LinkType> void TPixy<LinkType>::init()
{
link.init();
}
template <class LinkType> TPixy<LinkType>::~TPixy()
{
free(blocks);
}
template <class LinkType> boolean TPixy<LinkType>::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 <class LinkType> void TPixy<LinkType>::resize()
{
blockArraySize += PIXY_INITIAL_ARRAYSIZE;
blocks = (Block *)realloc(blocks, sizeof(Block)*blockArraySize);
}
template <class LinkType> uint16_t TPixy<LinkType>::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; blockCount<maxBlocks && blockCount<PIXY_MAXIMUM_ARRAYSIZE;)
{
checksum = link.getWord();
if (checksum==PIXY_START_WORD) // we've reached the beginning of the next frame
{
skipStart = true;
blockType = NORMAL_BLOCK;
//Serial.println("skip");
return blockCount;
}
else if (checksum==PIXY_START_WORD_CC)
{
skipStart = true;
blockType = CC_BLOCK;
return blockCount;
}
else if (checksum==0)
return blockCount;
if (blockCount>blockArraySize)
resize();
block = blocks + blockCount;
for (i=0, sum=0; i<sizeof(Block)/sizeof(uint16_t); i++)
{
if (blockType==NORMAL_BLOCK && 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 <class LinkType> int8_t TPixy<LinkType>::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 <class LinkType> int8_t TPixy<LinkType>::setBrightness(uint8_t brightness)
{
uint8_t outBuf[3];
outBuf[0] = 0x00;
outBuf[1] = 0xfe;
outBuf[2] = brightness;
return link.send(outBuf, 3);
}
template <class LinkType> int8_t TPixy<LinkType>::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

70
lib/pixy/examples/hello_world/hello_world.ino Normal file
View file

@ -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 <SPI.h>
#include <Pixy.h>
// 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<blocks; j++)
{
sprintf(buf, " block %d: ", j);
Serial.print(buf);
pixy.blocks[j].print();
}
}
}
}

63
lib/pixy/examples/i2c/i2c.ino Normal file
View file

@ -0,0 +1,63 @@
//
// 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 I2C communications. If you're
// not sure what I2C is, run the hello_world sketch!
//
#include <Wire.h>
#include <PixyI2C.h>
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<blocks; j++)
{
sprintf(buf, " block %d: ", j);
Serial.print(buf);
pixy.blocks[j].print();
}
}
}
}

54
lib/pixy/examples/led_cycle/led_cycle.ino Executable file
View file

@ -0,0 +1,54 @@
//
// 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 demonstrates the setLED() function. Running this sketch
// will cycle the Pixy's RGB LED through its colors.
//
#include <SPI.h>
#include <Pixy.h>
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++;
}
}

119
lib/pixy/examples/pantilt/pantilt.ino Executable file
View file

@ -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 <SPI.h>
#include <Pixy.h>
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<PIXY_RCS_MIN_POS)
m_pos = PIXY_RCS_MIN_POS;
}
m_prevError = error;
}
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];
int32_t panError, tiltError;
blocks = pixy.getBlocks();
if (blocks)
{
panError = X_CENTER-pixy.blocks[0].x;
tiltError = pixy.blocks[0].y-Y_CENTER;
panLoop.update(panError);
tiltLoop.update(tiltError);
pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
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<blocks; j++)
{
sprintf(buf, " block %d: ", j);
Serial.print(buf);
pixy.blocks[j].print();
}
}
}
}

44
lib/pixy/examples/servo_move/servo_move.ino Executable file
View file

@ -0,0 +1,44 @@
//
// 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 demonstrates the setServos() function. Running this sketch
// will move the servos to their limits, back and forth, back and forth.
//
#include <SPI.h>
#include <Pixy.h>
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);
}

66
lib/pixy/examples/uart/uart.ino Normal file
View file

@ -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<blocks; j++)
{
sprintf(buf, " block %d: ", j);
Serial.print(buf);
pixy.blocks[j].print();
}
}
}
}

11
lib/pixy/keywords.txt Normal file
View file

@ -0,0 +1,11 @@
Pixy KEYWORD1
PixyI2C KEYWORD1
PixyUART KEYWORD1
PixySPI_SS KEYWORD1
Block KEYWORD1
getBlocks KEYWORD2
init KEYWORD2
print KEYWORD2
setServos KEYWORD2
setBrightness KEYWORD2
setLED KEYWORD2

17
platformio.ini Normal file
View file

@ -0,0 +1,17 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:uno_r4_minima]
platform = renesas-ra
board = uno_r4_minima
framework = arduino
lib_deps =
adafruit/Adafruit BNO08x RVC@^1.0.2
seeed-studio/Grove Ultrasonic Ranger@^1.0.1

116
src/main.cpp Normal file
View file

@ -0,0 +1,116 @@
#include <Arduino.h>
#include <Wire.h>
#include "Adafruit_BNO08x_RVC.h"
#include <PixyI2C.h>
#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);
}
}
}

11
test/README Normal file
View file

@ -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