zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Official

k

Dependencies:   Servo ServoArm mbed

Sources/Pixy.cpp@0:15a8480061e8, 2017-05-22 (annotated)

Committer:
beacon
Date:
Mon May 22 11:24:46 2017 +0000
Revision:
0:15a8480061e8
o

Who changed what in which revision?

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