Prometheus / PixyLib

PixyLib

Dependents:   Prom_Roebi

Pixy.cpp@1:7cc7d76ae814, 2017-05-20 (annotated)

Committer:
ZHAW_Prometheus
Date:
Sat May 20 21:46:24 2017 +0000
Revision:
1:7cc7d76ae814
Parent:
0:168eb7f8ec2e 
Vers. 20.05.2017 23:45

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ZHAW_Prometheus 0:168eb7f8ec2e 1
ZHAW_Prometheus 0:168eb7f8ec2e 2 #include "Pixy.h"
ZHAW_Prometheus 0:168eb7f8ec2e 3
ZHAW_Prometheus 0:168eb7f8ec2e 4 Pixy::Pixy(Serial& _cam) : cam(_cam)
ZHAW_Prometheus 0:168eb7f8ec2e 5 {
ZHAW_Prometheus 0:168eb7f8ec2e 6 //cam.baud( 230400 );
ZHAW_Prometheus 0:168eb7f8ec2e 7 cam.baud( 460800 );
ZHAW_Prometheus 0:168eb7f8ec2e 8 cam.attach(this, &Pixy::rxCallback, Serial::RxIrq);
ZHAW_Prometheus 0:168eb7f8ec2e 9 }
ZHAW_Prometheus 0:168eb7f8ec2e 10
ZHAW_Prometheus 0:168eb7f8ec2e 11
ZHAW_Prometheus 0:168eb7f8ec2e 12 // This function is called when a character goes into the RX buffer.
ZHAW_Prometheus 0:168eb7f8ec2e 13 void Pixy::rxCallback()
ZHAW_Prometheus 0:168eb7f8ec2e 14 {
ZHAW_Prometheus 0:168eb7f8ec2e 15 static const int buffersize = 256;
ZHAW_Prometheus 1:7cc7d76ae814 16 detects = 0;
ZHAW_Prometheus 0:168eb7f8ec2e 17 static int startPoint = 0;
ZHAW_Prometheus 0:168eb7f8ec2e 18 static uint8_t buffer[buffersize] = {};
ZHAW_Prometheus 0:168eb7f8ec2e 19 static bool startFound = false;
ZHAW_Prometheus 0:168eb7f8ec2e 20 static int ii = 1;
ZHAW_Prometheus 0:168eb7f8ec2e 21
ZHAW_Prometheus 0:168eb7f8ec2e 22 while( cam.readable() ) {
ZHAW_Prometheus 0:168eb7f8ec2e 23 buffer[ii] = cam.getc();
ZHAW_Prometheus 0:168eb7f8ec2e 24 if( buffer[ii-1] == 85 && (buffer[ii] == 170 ) ) {
ZHAW_Prometheus 0:168eb7f8ec2e 25 startPoint = ii-1;
ZHAW_Prometheus 0:168eb7f8ec2e 26
ZHAW_Prometheus 0:168eb7f8ec2e 27 //check if detection was on the edge of buffer. Skip package if so.
ZHAW_Prometheus 0:168eb7f8ec2e 28 if( ii<(buffersize-14))
ZHAW_Prometheus 0:168eb7f8ec2e 29 startFound = true;
ZHAW_Prometheus 0:168eb7f8ec2e 30 else
ZHAW_Prometheus 0:168eb7f8ec2e 31 ii = 1;
ZHAW_Prometheus 0:168eb7f8ec2e 32
ZHAW_Prometheus 0:168eb7f8ec2e 33 detects++;
ZHAW_Prometheus 0:168eb7f8ec2e 34 }
ZHAW_Prometheus 0:168eb7f8ec2e 35 ++ii;
ZHAW_Prometheus 0:168eb7f8ec2e 36
ZHAW_Prometheus 0:168eb7f8ec2e 37 //reset ii
ZHAW_Prometheus 0:168eb7f8ec2e 38 if( ii>=(buffersize-1))
ZHAW_Prometheus 0:168eb7f8ec2e 39 ii = 1;
ZHAW_Prometheus 0:168eb7f8ec2e 40 }
ZHAW_Prometheus 0:168eb7f8ec2e 41
ZHAW_Prometheus 0:168eb7f8ec2e 42 //start not found, reset ii to 1
ZHAW_Prometheus 0:168eb7f8ec2e 43 if( !startFound && ii >= 3 || ii >= (buffersize-1)) {
ZHAW_Prometheus 0:168eb7f8ec2e 44 ii = 1;
ZHAW_Prometheus 0:168eb7f8ec2e 45 return;
ZHAW_Prometheus 0:168eb7f8ec2e 46 }
ZHAW_Prometheus 0:168eb7f8ec2e 47
ZHAW_Prometheus 0:168eb7f8ec2e 48 //start is found but not enough bytes received - return
ZHAW_Prometheus 0:168eb7f8ec2e 49 if( (ii-startPoint) <= 13 )
ZHAW_Prometheus 0:168eb7f8ec2e 50 return;
ZHAW_Prometheus 0:168eb7f8ec2e 51
ZHAW_Prometheus 0:168eb7f8ec2e 52 //copy memory to pixy struct
ZHAW_Prometheus 0:168eb7f8ec2e 53 memcpy( &pixy, buffer + startPoint+2, 12);
ZHAW_Prometheus 0:168eb7f8ec2e 54
ZHAW_Prometheus 0:168eb7f8ec2e 55 //reset variables
ZHAW_Prometheus 0:168eb7f8ec2e 56 startFound = false;
ZHAW_Prometheus 0:168eb7f8ec2e 57 ii = 1;
ZHAW_Prometheus 0:168eb7f8ec2e 58 }
ZHAW_Prometheus 0:168eb7f8ec2e 59
ZHAW_Prometheus 0:168eb7f8ec2e 60 int Pixy::getX()
ZHAW_Prometheus 0:168eb7f8ec2e 61 {
ZHAW_Prometheus 0:168eb7f8ec2e 62 return pixy.x;
ZHAW_Prometheus 0:168eb7f8ec2e 63 }
ZHAW_Prometheus 0:168eb7f8ec2e 64
ZHAW_Prometheus 0:168eb7f8ec2e 65 int Pixy::getY()
ZHAW_Prometheus 0:168eb7f8ec2e 66 {
ZHAW_Prometheus 0:168eb7f8ec2e 67 return pixy.y;
ZHAW_Prometheus 0:168eb7f8ec2e 68 }
ZHAW_Prometheus 0:168eb7f8ec2e 69
ZHAW_Prometheus 0:168eb7f8ec2e 70 int Pixy::getSignature()
ZHAW_Prometheus 0:168eb7f8ec2e 71 {
ZHAW_Prometheus 0:168eb7f8ec2e 72 return pixy.signature;
ZHAW_Prometheus 0:168eb7f8ec2e 73 }
ZHAW_Prometheus 0:168eb7f8ec2e 74
ZHAW_Prometheus 1:7cc7d76ae814 75 int Pixy::getDetects()
ZHAW_Prometheus 1:7cc7d76ae814 76 {
ZHAW_Prometheus 1:7cc7d76ae814 77 return detects;
ZHAW_Prometheus 1:7cc7d76ae814 78 }