Mathieu Malone / Mbed 2 deprecated PixyLibrary

Library used to initialize and to communicate with the CMUcam5 Pixy

Dependencies:   mbed

Dependents:   PixyStereoCam

TPixy1.h@0:56a3009221d3, 2014-08-12 (annotated)

Committer:
MBM
Date:
Tue Aug 12 11:01:31 2014 +0000
Revision:
0:56a3009221d3
The PixyLibrary was created from the Arduino libraries for the CMUcam5 Pixy found on http://cmucam.org/projects/cmucam5/files.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MBM 0:56a3009221d3 1 //--------------------------------------------------------------------------------------------
MBM 0:56a3009221d3 2 //Original Property of: charmedlabs.com/pixystart -> arduino_pixy-x.y.z.zip
MBM 0:56a3009221d3 3 //
MBM 0:56a3009221d3 4 //Modifications made by: Mathieu Malone
MBM 0:56a3009221d3 5 //Modifications: Modified Arduino code to function with mbed development platform
MBM 0:56a3009221d3 6 //Output Method: This program uses "Serial pc(USBTX, USBRX)" in order to allow communication
MBM 0:56a3009221d3 7 // between the mbed platform and putty terminal through USB
MBM 0:56a3009221d3 8 //
MBM 0:56a3009221d3 9 //Latest update by: Mathieu Malone
MBM 0:56a3009221d3 10 //Date of last update: July 24th, 2014
MBM 0:56a3009221d3 11 //--------------------------------------------------------------------------------------------
MBM 0:56a3009221d3 12 //
MBM 0:56a3009221d3 13 // begin license header
MBM 0:56a3009221d3 14 //
MBM 0:56a3009221d3 15 // This file is part of Pixy CMUcam5 or "Pixy" for short
MBM 0:56a3009221d3 16 //
MBM 0:56a3009221d3 17 // All Pixy source code is provided under the terms of the
MBM 0:56a3009221d3 18 // GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
MBM 0:56a3009221d3 19 // Those wishing to use Pixy source code, software and/or
MBM 0:56a3009221d3 20 // technologies under different licensing terms should contact us at
MBM 0:56a3009221d3 21 // cmucam@cs.cmu.edu. Such licensing terms are available for
MBM 0:56a3009221d3 22 // all portions of the Pixy codebase presented here.
MBM 0:56a3009221d3 23 //
MBM 0:56a3009221d3 24 // end license header
MBM 0:56a3009221d3 25 //
MBM 0:56a3009221d3 26
MBM 0:56a3009221d3 27 /*
MBM 0:56a3009221d3 28 06.04.2014 v0.1.3 John Leimon
MBM 0:56a3009221d3 29 + Added init() for initializing Pixy, which should
MBM 0:56a3009221d3 30 be called from the setup() function. See comment
MBM 0:56a3009221d3 31 in Pixy.h for details.
MBM 0:56a3009221d3 32 */
MBM 0:56a3009221d3 33
MBM 0:56a3009221d3 34 #ifndef _TPIXY_H1
MBM 0:56a3009221d3 35 #define _TPIXY_H1
MBM 0:56a3009221d3 36
MBM 0:56a3009221d3 37 #define PIXY_INITIAL_ARRAYSIZE1 30
MBM 0:56a3009221d3 38 #define PIXY_MAXIMUM_ARRAYSIZE1 130
MBM 0:56a3009221d3 39 #define PIXY_START_WORD1 0xaa55
MBM 0:56a3009221d3 40 #define PIXY_START_WORDX1 0x55aa
MBM 0:56a3009221d3 41 #define PIXY_DEFAULT_ADDR1 0x54 // I2C
MBM 0:56a3009221d3 42
MBM 0:56a3009221d3 43 Serial pc1(USBTX, USBRX);
MBM 0:56a3009221d3 44
MBM 0:56a3009221d3 45 struct Block1
MBM 0:56a3009221d3 46 {
MBM 0:56a3009221d3 47 void print1()
MBM 0:56a3009221d3 48 {
MBM 0:56a3009221d3 49 char buf1[64];
MBM 0:56a3009221d3 50 sprintf(buf1, "sig: %d x1: %d y1: %d width1: %d height1: %d\n", signature1, x1, y1, width1, height1);
MBM 0:56a3009221d3 51 printf(buf1);
MBM 0:56a3009221d3 52 Xp1 = x1;
MBM 0:56a3009221d3 53 Yp1 = y1;
MBM 0:56a3009221d3 54 sig1 = signature1;
MBM 0:56a3009221d3 55 }
MBM 0:56a3009221d3 56 uint16_t signature1;
MBM 0:56a3009221d3 57 uint16_t x1;
MBM 0:56a3009221d3 58 uint16_t y1;
MBM 0:56a3009221d3 59 uint16_t width1;
MBM 0:56a3009221d3 60 uint16_t height1;
MBM 0:56a3009221d3 61 };
MBM 0:56a3009221d3 62
MBM 0:56a3009221d3 63 template <class LinkType1> class TPixy1
MBM 0:56a3009221d3 64 {
MBM 0:56a3009221d3 65 public:
MBM 0:56a3009221d3 66 TPixy1(uint8_t addr1=PIXY_DEFAULT_ADDR1);
MBM 0:56a3009221d3 67 ~TPixy1();
MBM 0:56a3009221d3 68
MBM 0:56a3009221d3 69 uint16_t getBlocks1(uint16_t maxBlocks1=1000);
MBM 0:56a3009221d3 70 int8_t setServos1(uint16_t s01, uint16_t s11);
MBM 0:56a3009221d3 71 void init1();
MBM 0:56a3009221d3 72
MBM 0:56a3009221d3 73 Block1 *blocks1;
MBM 0:56a3009221d3 74
MBM 0:56a3009221d3 75 private:
MBM 0:56a3009221d3 76 bool getStart1();
MBM 0:56a3009221d3 77 void resize1();
MBM 0:56a3009221d3 78
MBM 0:56a3009221d3 79 LinkType1 link1;
MBM 0:56a3009221d3 80 bool skipStart1;
MBM 0:56a3009221d3 81 uint16_t blockCount1;
MBM 0:56a3009221d3 82 uint16_t blockArraySize1;
MBM 0:56a3009221d3 83 };
MBM 0:56a3009221d3 84
MBM 0:56a3009221d3 85
MBM 0:56a3009221d3 86 template <class LinkType1> TPixy1<LinkType1>::TPixy1(uint8_t addr1)
MBM 0:56a3009221d3 87 {
MBM 0:56a3009221d3 88 skipStart1 = false;
MBM 0:56a3009221d3 89 blockCount1 = 0;
MBM 0:56a3009221d3 90 blockArraySize1 = PIXY_INITIAL_ARRAYSIZE1;
MBM 0:56a3009221d3 91 blocks1 = (Block1 *)malloc(sizeof(Block1)*blockArraySize1);
MBM 0:56a3009221d3 92 link1.setAddress1(addr1);
MBM 0:56a3009221d3 93 }
MBM 0:56a3009221d3 94
MBM 0:56a3009221d3 95 template <class LinkType1> void TPixy1<LinkType1>::init1()
MBM 0:56a3009221d3 96 {
MBM 0:56a3009221d3 97 link1.init1();
MBM 0:56a3009221d3 98 }
MBM 0:56a3009221d3 99
MBM 0:56a3009221d3 100 template <class LinkType1> TPixy1<LinkType1>::~TPixy1()
MBM 0:56a3009221d3 101 {
MBM 0:56a3009221d3 102 free(blocks1);
MBM 0:56a3009221d3 103 }
MBM 0:56a3009221d3 104
MBM 0:56a3009221d3 105 template <class LinkType1> bool TPixy1<LinkType1>::getStart1()
MBM 0:56a3009221d3 106 {
MBM 0:56a3009221d3 107 uint16_t w1, lastw1;
MBM 0:56a3009221d3 108
MBM 0:56a3009221d3 109 lastw1 = 0xffff;
MBM 0:56a3009221d3 110
MBM 0:56a3009221d3 111 while(true)
MBM 0:56a3009221d3 112 {
MBM 0:56a3009221d3 113 w1 = link1.getWord1();
MBM 0:56a3009221d3 114 if (w1==0 && lastw1==0)
MBM 0:56a3009221d3 115 {
MBM 0:56a3009221d3 116 wait(0.00001);
MBM 0:56a3009221d3 117 return false;
MBM 0:56a3009221d3 118 }
MBM 0:56a3009221d3 119 else if (w1==PIXY_START_WORD1 && lastw1==PIXY_START_WORD1)
MBM 0:56a3009221d3 120 return true;
MBM 0:56a3009221d3 121 else if (w1==PIXY_START_WORDX1)
MBM 0:56a3009221d3 122 {
MBM 0:56a3009221d3 123 pc1.printf("reorder1");
MBM 0:56a3009221d3 124 link1.getByte1(); // resync
MBM 0:56a3009221d3 125 }
MBM 0:56a3009221d3 126 lastw1 = w1;
MBM 0:56a3009221d3 127 }
MBM 0:56a3009221d3 128 }
MBM 0:56a3009221d3 129
MBM 0:56a3009221d3 130 template <class LinkType1> void TPixy1<LinkType1>::resize1()
MBM 0:56a3009221d3 131 {
MBM 0:56a3009221d3 132 Block1 *newBlocks1;
MBM 0:56a3009221d3 133 blockArraySize1 += PIXY_INITIAL_ARRAYSIZE1;
MBM 0:56a3009221d3 134 newBlocks1 = (Block1 *)malloc(sizeof(Block1)*blockArraySize1);
MBM 0:56a3009221d3 135 memcpy(newBlocks1, blocks1, sizeof(Block1)*blockCount1);
MBM 0:56a3009221d3 136 free(blocks1);
MBM 0:56a3009221d3 137 blocks1 = newBlocks1;
MBM 0:56a3009221d3 138 }
MBM 0:56a3009221d3 139
MBM 0:56a3009221d3 140 template <class LinkType1> uint16_t TPixy1<LinkType1>::getBlocks1(uint16_t maxBlocks1)
MBM 0:56a3009221d3 141 {
MBM 0:56a3009221d3 142 uint8_t i1;
MBM 0:56a3009221d3 143 uint16_t w1, checksum1, sum1;
MBM 0:56a3009221d3 144 Block1 *block1;
MBM 0:56a3009221d3 145
MBM 0:56a3009221d3 146 if (!skipStart1)
MBM 0:56a3009221d3 147 {
MBM 0:56a3009221d3 148 if (getStart1()==false)
MBM 0:56a3009221d3 149 return 0;
MBM 0:56a3009221d3 150 }
MBM 0:56a3009221d3 151 else
MBM 0:56a3009221d3 152 skipStart1 = false;
MBM 0:56a3009221d3 153
MBM 0:56a3009221d3 154 for(blockCount1=0; blockCount1<maxBlocks1 && blockCount1<PIXY_MAXIMUM_ARRAYSIZE1;)
MBM 0:56a3009221d3 155 {
MBM 0:56a3009221d3 156 checksum1 = link1.getWord1();
MBM 0:56a3009221d3 157 if (checksum1==PIXY_START_WORD1) // we've reached the beginning of the next frame
MBM 0:56a3009221d3 158 {
MBM 0:56a3009221d3 159 skipStart1 = true;
MBM 0:56a3009221d3 160 //Serial.println("skip");
MBM 0:56a3009221d3 161 return blockCount1;
MBM 0:56a3009221d3 162 }
MBM 0:56a3009221d3 163 else if (checksum1==0)
MBM 0:56a3009221d3 164 return blockCount1;
MBM 0:56a3009221d3 165
MBM 0:56a3009221d3 166 if (blockCount1>blockArraySize1)
MBM 0:56a3009221d3 167 resize1();
MBM 0:56a3009221d3 168
MBM 0:56a3009221d3 169 block1 = blocks1 + blockCount1;
MBM 0:56a3009221d3 170
MBM 0:56a3009221d3 171 for (i1=0, sum1=0; i1<sizeof(Block1)/sizeof(uint16_t); i1++)
MBM 0:56a3009221d3 172 {
MBM 0:56a3009221d3 173 w1 = link1.getWord1();
MBM 0:56a3009221d3 174 sum1 += w1;
MBM 0:56a3009221d3 175 *((uint16_t *)block1 + i1) = w1;
MBM 0:56a3009221d3 176 }
MBM 0:56a3009221d3 177
MBM 0:56a3009221d3 178 if (checksum1==sum1)
MBM 0:56a3009221d3 179 blockCount1++;
MBM 0:56a3009221d3 180 else
MBM 0:56a3009221d3 181 pc1.printf("cs error 1");
MBM 0:56a3009221d3 182
MBM 0:56a3009221d3 183 w1 = link1.getWord1();
MBM 0:56a3009221d3 184 if (w1!=PIXY_START_WORD1)
MBM 0:56a3009221d3 185 return blockCount1;
MBM 0:56a3009221d3 186 }
MBM 0:56a3009221d3 187 return maxBlocks1;
MBM 0:56a3009221d3 188 }
MBM 0:56a3009221d3 189
MBM 0:56a3009221d3 190 template <class LinkType1> int8_t TPixy1<LinkType1>::setServos1(uint16_t s01, uint16_t s11)
MBM 0:56a3009221d3 191 {
MBM 0:56a3009221d3 192 uint8_t outBuf1[6];
MBM 0:56a3009221d3 193
MBM 0:56a3009221d3 194 outBuf1[0] = 0x00;
MBM 0:56a3009221d3 195 outBuf1[1] = 0xff;
MBM 0:56a3009221d3 196 *(uint16_t *)(outBuf1 + 2) = s01;
MBM 0:56a3009221d3 197 *(uint16_t *)(outBuf1 + 4) = s11;
MBM 0:56a3009221d3 198
MBM 0:56a3009221d3 199 return link1.send1(outBuf1, 6);
MBM 0:56a3009221d3 200 }
MBM 0:56a3009221d3 201
MBM 0:56a3009221d3 202 #endif
MBM 0:56a3009221d3 203
MBM 0:56a3009221d3 204
MBM 0:56a3009221d3 205
MBM 0:56a3009221d3 206
MBM 0:56a3009221d3 207