Jonathan Goret / PixyLibrary

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   Nucleo_motors

Fork of PixyLibrary by Mathieu Malone

TPixy2.h@1:4b14159ab9bb, 2015-04-13 (annotated)

Committer:
johnylafleur
Date:
Mon Apr 13 12:32:52 2015 +0000
Revision:
1:4b14159ab9bb
Parent:
0:56a3009221d3 
Same library

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_H2
MBM 0:56a3009221d3 35 #define _TPIXY_H2
MBM 0:56a3009221d3 36
MBM 0:56a3009221d3 37 #define PIXY_INITIAL_ARRAYSIZE2 30
MBM 0:56a3009221d3 38 #define PIXY_MAXIMUM_ARRAYSIZE2 130
MBM 0:56a3009221d3 39 #define PIXY_START_WORD2 0xaa55
MBM 0:56a3009221d3 40 #define PIXY_START_WORDX2 0x55aa
MBM 0:56a3009221d3 41 #define PIXY_DEFAULT_ADDR2 0x54 // I2C
MBM 0:56a3009221d3 42
MBM 0:56a3009221d3 43 Serial pc2(USBTX, USBRX);
MBM 0:56a3009221d3 44
MBM 0:56a3009221d3 45 struct Block2
MBM 0:56a3009221d3 46 {
MBM 0:56a3009221d3 47 void print2()
MBM 0:56a3009221d3 48 {
MBM 0:56a3009221d3 49 char buf2[64];
MBM 0:56a3009221d3 50 sprintf(buf2, "sig: %d x2: %d y2: %d width2: %d height2: %d\n", signature2, x2, y2, width2, height2);
MBM 0:56a3009221d3 51 printf(buf2);
MBM 0:56a3009221d3 52 Xp2 = x2;
MBM 0:56a3009221d3 53 Yp2= y2;
MBM 0:56a3009221d3 54 sig2 = signature2;
MBM 0:56a3009221d3 55 }
MBM 0:56a3009221d3 56 uint16_t signature2;
MBM 0:56a3009221d3 57 uint16_t x2;
MBM 0:56a3009221d3 58 uint16_t y2;
MBM 0:56a3009221d3 59 uint16_t width2;
MBM 0:56a3009221d3 60 uint16_t height2;
MBM 0:56a3009221d3 61 };
MBM 0:56a3009221d3 62
MBM 0:56a3009221d3 63 template <class LinkType2> class TPixy2
MBM 0:56a3009221d3 64 {
MBM 0:56a3009221d3 65 public:
MBM 0:56a3009221d3 66 TPixy2(uint8_t addr2=PIXY_DEFAULT_ADDR2);
MBM 0:56a3009221d3 67 ~TPixy2();
MBM 0:56a3009221d3 68
MBM 0:56a3009221d3 69 uint16_t getBlocks2(uint16_t maxBlocks2=1000);
MBM 0:56a3009221d3 70 int8_t setServos2(uint16_t s02, uint16_t s12);
MBM 0:56a3009221d3 71 void init2();
MBM 0:56a3009221d3 72
MBM 0:56a3009221d3 73 Block2 *blocks2;
MBM 0:56a3009221d3 74
MBM 0:56a3009221d3 75 private:
MBM 0:56a3009221d3 76 bool getStart2();
MBM 0:56a3009221d3 77 void resize2();
MBM 0:56a3009221d3 78
MBM 0:56a3009221d3 79 LinkType2 link2;
MBM 0:56a3009221d3 80 bool skipStart2;
MBM 0:56a3009221d3 81 uint16_t blockCount2;
MBM 0:56a3009221d3 82 uint16_t blockArraySize2;
MBM 0:56a3009221d3 83 };
MBM 0:56a3009221d3 84
MBM 0:56a3009221d3 85
MBM 0:56a3009221d3 86 template <class LinkType2> TPixy2<LinkType2>::TPixy2(uint8_t addr2)
MBM 0:56a3009221d3 87 {
MBM 0:56a3009221d3 88 skipStart2 = false;
MBM 0:56a3009221d3 89 blockCount2 = 0;
MBM 0:56a3009221d3 90 blockArraySize2 = PIXY_INITIAL_ARRAYSIZE2;
MBM 0:56a3009221d3 91 blocks2 = (Block2 *)malloc(sizeof(Block2)*blockArraySize2);
MBM 0:56a3009221d3 92 link2.setAddress2(addr2);
MBM 0:56a3009221d3 93 }
MBM 0:56a3009221d3 94
MBM 0:56a3009221d3 95 template <class LinkType2> void TPixy2<LinkType2>::init2()
MBM 0:56a3009221d3 96 {
MBM 0:56a3009221d3 97 link2.init2();
MBM 0:56a3009221d3 98 }
MBM 0:56a3009221d3 99
MBM 0:56a3009221d3 100 template <class LinkType2> TPixy2<LinkType2>::~TPixy2()
MBM 0:56a3009221d3 101 {
MBM 0:56a3009221d3 102 free(blocks2);
MBM 0:56a3009221d3 103 }
MBM 0:56a3009221d3 104
MBM 0:56a3009221d3 105 template <class LinkType2> bool TPixy2<LinkType2>::getStart2()
MBM 0:56a3009221d3 106 {
MBM 0:56a3009221d3 107 uint16_t w2, lastw2;
MBM 0:56a3009221d3 108
MBM 0:56a3009221d3 109 lastw2 = 0xffff;
MBM 0:56a3009221d3 110
MBM 0:56a3009221d3 111 while(true)
MBM 0:56a3009221d3 112 {
MBM 0:56a3009221d3 113 w2 = link2.getWord2();
MBM 0:56a3009221d3 114 if (w2==0 && lastw2==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 (w2==PIXY_START_WORD2 && lastw2==PIXY_START_WORD2)
MBM 0:56a3009221d3 120 return true;
MBM 0:56a3009221d3 121 else if (w2==PIXY_START_WORDX2)
MBM 0:56a3009221d3 122 {
MBM 0:56a3009221d3 123 pc2.printf("reorder2");
MBM 0:56a3009221d3 124 link2.getByte2(); // resync
MBM 0:56a3009221d3 125 }
MBM 0:56a3009221d3 126 lastw2 = w2;
MBM 0:56a3009221d3 127 }
MBM 0:56a3009221d3 128 }
MBM 0:56a3009221d3 129
MBM 0:56a3009221d3 130 template <class LinkType2> void TPixy2<LinkType2>::resize2()
MBM 0:56a3009221d3 131 {
MBM 0:56a3009221d3 132 Block2 *newBlocks2;
MBM 0:56a3009221d3 133 blockArraySize2 += PIXY_INITIAL_ARRAYSIZE2;
MBM 0:56a3009221d3 134 newBlocks2 = (Block2 *)malloc(sizeof(Block2)*blockArraySize2);
MBM 0:56a3009221d3 135 memcpy(newBlocks2, blocks2, sizeof(Block2)*blockCount2);
MBM 0:56a3009221d3 136 free(blocks2);
MBM 0:56a3009221d3 137 blocks2 = newBlocks2;
MBM 0:56a3009221d3 138 }
MBM 0:56a3009221d3 139
MBM 0:56a3009221d3 140 template <class LinkType2> uint16_t TPixy2<LinkType2>::getBlocks2(uint16_t maxBlocks2)
MBM 0:56a3009221d3 141 {
MBM 0:56a3009221d3 142 uint8_t i2;
MBM 0:56a3009221d3 143 uint16_t w2, checksum2, sum2;
MBM 0:56a3009221d3 144 Block2 *block2;
MBM 0:56a3009221d3 145
MBM 0:56a3009221d3 146 if (!skipStart2)
MBM 0:56a3009221d3 147 {
MBM 0:56a3009221d3 148 if (getStart2()==false)
MBM 0:56a3009221d3 149 return 0;
MBM 0:56a3009221d3 150 }
MBM 0:56a3009221d3 151 else
MBM 0:56a3009221d3 152 skipStart2 = false;
MBM 0:56a3009221d3 153
MBM 0:56a3009221d3 154 for(blockCount2=0; blockCount2<maxBlocks2 && blockCount2<PIXY_MAXIMUM_ARRAYSIZE2;)
MBM 0:56a3009221d3 155 {
MBM 0:56a3009221d3 156 checksum2 = link2.getWord2();
MBM 0:56a3009221d3 157 if (checksum2==PIXY_START_WORD2) // we've reached the beginning of the next frame
MBM 0:56a3009221d3 158 {
MBM 0:56a3009221d3 159 skipStart2 = true;
MBM 0:56a3009221d3 160 //Serial.println("skip");
MBM 0:56a3009221d3 161 return blockCount2;
MBM 0:56a3009221d3 162 }
MBM 0:56a3009221d3 163 else if (checksum2==0)
MBM 0:56a3009221d3 164 return blockCount2;
MBM 0:56a3009221d3 165
MBM 0:56a3009221d3 166 if (blockCount2>blockArraySize2)
MBM 0:56a3009221d3 167 resize2();
MBM 0:56a3009221d3 168
MBM 0:56a3009221d3 169 block2 = blocks2 + blockCount2;
MBM 0:56a3009221d3 170
MBM 0:56a3009221d3 171 for (i2=0, sum2=0; i2<sizeof(Block2)/sizeof(uint16_t); i2++)
MBM 0:56a3009221d3 172 {
MBM 0:56a3009221d3 173 w2 = link2.getWord2();
MBM 0:56a3009221d3 174 sum2 += w2;
MBM 0:56a3009221d3 175 *((uint16_t *)block2 + i2) = w2;
MBM 0:56a3009221d3 176 }
MBM 0:56a3009221d3 177
MBM 0:56a3009221d3 178 if (checksum2==sum2)
MBM 0:56a3009221d3 179 blockCount2++;
MBM 0:56a3009221d3 180 else
MBM 0:56a3009221d3 181 pc2.printf("cs error 2");
MBM 0:56a3009221d3 182
MBM 0:56a3009221d3 183 w2 = link2.getWord2();
MBM 0:56a3009221d3 184 if (w2!=PIXY_START_WORD2)
MBM 0:56a3009221d3 185 return blockCount2;
MBM 0:56a3009221d3 186 }
MBM 0:56a3009221d3 187 return maxBlocks2;
MBM 0:56a3009221d3 188 }
MBM 0:56a3009221d3 189
MBM 0:56a3009221d3 190 template <class LinkType2> int8_t TPixy2<LinkType2>::setServos2(uint16_t s02, uint16_t s12)
MBM 0:56a3009221d3 191 {
MBM 0:56a3009221d3 192 uint8_t outBuf2[6];
MBM 0:56a3009221d3 193
MBM 0:56a3009221d3 194 outBuf2[0] = 0x00;
MBM 0:56a3009221d3 195 outBuf2[1] = 0xff;
MBM 0:56a3009221d3 196 *(uint16_t *)(outBuf2 + 2) = s02;
MBM 0:56a3009221d3 197 *(uint16_t *)(outBuf2 + 4) = s12;
MBM 0:56a3009221d3 198
MBM 0:56a3009221d3 199 return link2.send2(outBuf2, 6);
MBM 0:56a3009221d3 200 }
MBM 0:56a3009221d3 201
MBM 0:56a3009221d3 202 #endif