Mechatronics Robotics
/
BrobotV1
These are the core files for the Robot at Team conception.
Pixy.cpp@0:eba74e7a229b, 2017-05-26 (annotated)
- Committer:
- obrie829
- Date:
- Fri May 26 07:40:48 2017 +0000
- Revision:
- 0:eba74e7a229b
- Child:
- 1:ff0674b96cc5
This is the first version committed to team Brobot and should have all header and class files contained within
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
obrie829 | 0:eba74e7a229b | 1 | |
obrie829 | 0:eba74e7a229b | 2 | #include "Pixy.h" |
obrie829 | 0:eba74e7a229b | 3 | |
obrie829 | 0:eba74e7a229b | 4 | Pixy::Pixy(Serial& _cam) : cam(_cam) |
obrie829 | 0:eba74e7a229b | 5 | { |
obrie829 | 0:eba74e7a229b | 6 | //cam.baud( 230400 ); |
obrie829 | 0:eba74e7a229b | 7 | cam.baud( 460800 ); |
obrie829 | 0:eba74e7a229b | 8 | cam.attach(this, &Pixy::rxCallback, Serial::RxIrq); |
obrie829 | 0:eba74e7a229b | 9 | } |
obrie829 | 0:eba74e7a229b | 10 | |
obrie829 | 0:eba74e7a229b | 11 | |
obrie829 | 0:eba74e7a229b | 12 | // This function is called when a character goes into the RX buffer. |
obrie829 | 0:eba74e7a229b | 13 | void Pixy::rxCallback() |
obrie829 | 0:eba74e7a229b | 14 | { |
obrie829 | 0:eba74e7a229b | 15 | static const int buffersize = 256; |
obrie829 | 0:eba74e7a229b | 16 | static int detects = 0; |
obrie829 | 0:eba74e7a229b | 17 | static int startPoint = 0; |
obrie829 | 0:eba74e7a229b | 18 | static uint8_t buffer[buffersize] = {}; |
obrie829 | 0:eba74e7a229b | 19 | static bool startFound = false; |
obrie829 | 0:eba74e7a229b | 20 | static int ii = 1; |
obrie829 | 0:eba74e7a229b | 21 | |
obrie829 | 0:eba74e7a229b | 22 | while( cam.readable() ) { |
obrie829 | 0:eba74e7a229b | 23 | buffer[ii] = cam.getc(); |
obrie829 | 0:eba74e7a229b | 24 | if( buffer[ii-1] == 85 && (buffer[ii] == 170 ) ) { |
obrie829 | 0:eba74e7a229b | 25 | startPoint = ii-1; |
obrie829 | 0:eba74e7a229b | 26 | |
obrie829 | 0:eba74e7a229b | 27 | //check if detection was on the edge of buffer. Skip package if so. |
obrie829 | 0:eba74e7a229b | 28 | if( ii<(buffersize-14)) |
obrie829 | 0:eba74e7a229b | 29 | startFound = true; |
obrie829 | 0:eba74e7a229b | 30 | else |
obrie829 | 0:eba74e7a229b | 31 | ii = 1; |
obrie829 | 0:eba74e7a229b | 32 | |
obrie829 | 0:eba74e7a229b | 33 | detects++; |
obrie829 | 0:eba74e7a229b | 34 | } |
obrie829 | 0:eba74e7a229b | 35 | ++ii; |
obrie829 | 0:eba74e7a229b | 36 | |
obrie829 | 0:eba74e7a229b | 37 | //reset ii |
obrie829 | 0:eba74e7a229b | 38 | if( ii>=(buffersize-1)) |
obrie829 | 0:eba74e7a229b | 39 | ii = 1; |
obrie829 | 0:eba74e7a229b | 40 | } |
obrie829 | 0:eba74e7a229b | 41 | |
obrie829 | 0:eba74e7a229b | 42 | //start not found, reset ii to 1 |
obrie829 | 0:eba74e7a229b | 43 | if( !startFound && ii >= 3 || ii >= (buffersize-1)) { |
obrie829 | 0:eba74e7a229b | 44 | ii = 1; |
obrie829 | 0:eba74e7a229b | 45 | return; |
obrie829 | 0:eba74e7a229b | 46 | } |
obrie829 | 0:eba74e7a229b | 47 | |
obrie829 | 0:eba74e7a229b | 48 | //start is found but not enough bytes received - return |
obrie829 | 0:eba74e7a229b | 49 | if( (ii-startPoint) <= 13 ) |
obrie829 | 0:eba74e7a229b | 50 | return; |
obrie829 | 0:eba74e7a229b | 51 | |
obrie829 | 0:eba74e7a229b | 52 | //copy memory to pixy struct |
obrie829 | 0:eba74e7a229b | 53 | memcpy( &pixy, buffer + startPoint+2, 12); |
obrie829 | 0:eba74e7a229b | 54 | |
obrie829 | 0:eba74e7a229b | 55 | //reset variables |
obrie829 | 0:eba74e7a229b | 56 | startFound = false; |
obrie829 | 0:eba74e7a229b | 57 | ii = 1; |
obrie829 | 0:eba74e7a229b | 58 | } |
obrie829 | 0:eba74e7a229b | 59 | |
obrie829 | 0:eba74e7a229b | 60 | int Pixy::getX() |
obrie829 | 0:eba74e7a229b | 61 | { |
obrie829 | 0:eba74e7a229b | 62 | return pixy.x; |
obrie829 | 0:eba74e7a229b | 63 | } |
obrie829 | 0:eba74e7a229b | 64 | |
obrie829 | 0:eba74e7a229b | 65 | int Pixy::getY() |
obrie829 | 0:eba74e7a229b | 66 | { |
obrie829 | 0:eba74e7a229b | 67 | return pixy.y; |
obrie829 | 0:eba74e7a229b | 68 | } |
obrie829 | 0:eba74e7a229b | 69 | |
obrie829 | 0:eba74e7a229b | 70 | int Pixy::getSignature() |
obrie829 | 0:eba74e7a229b | 71 | { |
obrie829 | 0:eba74e7a229b | 72 | return pixy.signature; |
obrie829 | 0:eba74e7a229b | 73 | } |
obrie829 | 0:eba74e7a229b | 74 |