Mike Spadaru / physcom

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.

pixy2/TPixy2.h@10:b1bdc51e1c50, 2020-11-24 (annotated)

Committer:
maspadaru
Date:
Tue Nov 24 14:26:05 2020 +0000
Revision:
10:b1bdc51e1c50
added Pixy2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maspadaru 10:b1bdc51e1c50 1 //
maspadaru 10:b1bdc51e1c50 2 // begin license header
maspadaru 10:b1bdc51e1c50 3 //
maspadaru 10:b1bdc51e1c50 4 // This file is part of Pixy CMUcam5 or "Pixy" for short
maspadaru 10:b1bdc51e1c50 5 //
maspadaru 10:b1bdc51e1c50 6 // All Pixy source code is provided under the terms of the
maspadaru 10:b1bdc51e1c50 7 // GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
maspadaru 10:b1bdc51e1c50 8 // Those wishing to use Pixy source code, software and/or
maspadaru 10:b1bdc51e1c50 9 // technologies under different licensing terms should contact us at
maspadaru 10:b1bdc51e1c50 10 // cmucam@cs.cmu.edu. Such licensing terms are available for
maspadaru 10:b1bdc51e1c50 11 // all portions of the Pixy codebase presented here.
maspadaru 10:b1bdc51e1c50 12 //
maspadaru 10:b1bdc51e1c50 13 // end license header
maspadaru 10:b1bdc51e1c50 14 //
maspadaru 10:b1bdc51e1c50 15 // Main Pixy template class. This class takes a link class and uses
maspadaru 10:b1bdc51e1c50 16 // it to communicate with Pixy over I2C, SPI, UART or USB using the
maspadaru 10:b1bdc51e1c50 17 // Pixy packet protocol.
maspadaru 10:b1bdc51e1c50 18
maspadaru 10:b1bdc51e1c50 19 #ifndef _TPIXY2_H
maspadaru 10:b1bdc51e1c50 20 #define _TPIXY2_H
maspadaru 10:b1bdc51e1c50 21
maspadaru 10:b1bdc51e1c50 22 // uncomment to turn on debug prints to console
maspadaru 10:b1bdc51e1c50 23 #define PIXY_DEBUG
maspadaru 10:b1bdc51e1c50 24
maspadaru 10:b1bdc51e1c50 25 #define PIXY_DEFAULT_ARGVAL 0x80000000
maspadaru 10:b1bdc51e1c50 26 #define PIXY_BUFFERSIZE 0x104
maspadaru 10:b1bdc51e1c50 27 #define PIXY_CHECKSUM_SYNC 0xc1af
maspadaru 10:b1bdc51e1c50 28 #define PIXY_NO_CHECKSUM_SYNC 0xc1ae
maspadaru 10:b1bdc51e1c50 29 #define PIXY_SEND_HEADER_SIZE 4
maspadaru 10:b1bdc51e1c50 30 #define PIXY_MAX_PROGNAME 33
maspadaru 10:b1bdc51e1c50 31
maspadaru 10:b1bdc51e1c50 32 #define PIXY_TYPE_REQUEST_CHANGE_PROG 0x02
maspadaru 10:b1bdc51e1c50 33 #define PIXY_TYPE_REQUEST_RESOLUTION 0x0c
maspadaru 10:b1bdc51e1c50 34 #define PIXY_TYPE_RESPONSE_RESOLUTION 0x0d
maspadaru 10:b1bdc51e1c50 35 #define PIXY_TYPE_REQUEST_VERSION 0x0e
maspadaru 10:b1bdc51e1c50 36 #define PIXY_TYPE_RESPONSE_VERSION 0x0f
maspadaru 10:b1bdc51e1c50 37 #define PIXY_TYPE_RESPONSE_RESULT 0x01
maspadaru 10:b1bdc51e1c50 38 #define PIXY_TYPE_RESPONSE_ERROR 0x03
maspadaru 10:b1bdc51e1c50 39 #define PIXY_TYPE_REQUEST_BRIGHTNESS 0x10
maspadaru 10:b1bdc51e1c50 40 #define PIXY_TYPE_REQUEST_SERVO 0x12
maspadaru 10:b1bdc51e1c50 41 #define PIXY_TYPE_REQUEST_LED 0x14
maspadaru 10:b1bdc51e1c50 42 #define PIXY_TYPE_REQUEST_LAMP 0x16
maspadaru 10:b1bdc51e1c50 43 #define PIXY_TYPE_REQUEST_FPS 0x18
maspadaru 10:b1bdc51e1c50 44
maspadaru 10:b1bdc51e1c50 45 #define PIXY_RESULT_OK 0
maspadaru 10:b1bdc51e1c50 46 #define PIXY_RESULT_ERROR -1
maspadaru 10:b1bdc51e1c50 47 #define PIXY_RESULT_BUSY -2
maspadaru 10:b1bdc51e1c50 48 #define PIXY_RESULT_CHECKSUM_ERROR -3
maspadaru 10:b1bdc51e1c50 49 #define PIXY_RESULT_TIMEOUT -4
maspadaru 10:b1bdc51e1c50 50 #define PIXY_RESULT_BUTTON_OVERRIDE -5
maspadaru 10:b1bdc51e1c50 51 #define PIXY_RESULT_PROG_CHANGING -6
maspadaru 10:b1bdc51e1c50 52
maspadaru 10:b1bdc51e1c50 53 // RC-servo values
maspadaru 10:b1bdc51e1c50 54 #define PIXY_RCS_MIN_POS 0
maspadaru 10:b1bdc51e1c50 55 #define PIXY_RCS_MAX_POS 1000L
maspadaru 10:b1bdc51e1c50 56 #define PIXY_RCS_CENTER_POS ((PIXY_RCS_MAX_POS - PIXY_RCS_MIN_POS) / 2)
maspadaru 10:b1bdc51e1c50 57
maspadaru 10:b1bdc51e1c50 58 #include "Pixy2CCC.h"
maspadaru 10:b1bdc51e1c50 59 #include "Pixy2Line.h"
maspadaru 10:b1bdc51e1c50 60 #include "Pixy2Video.h"
maspadaru 10:b1bdc51e1c50 61 #include "mbed.h"
maspadaru 10:b1bdc51e1c50 62
maspadaru 10:b1bdc51e1c50 63 struct Version {
maspadaru 10:b1bdc51e1c50 64 void print(Serial* serial)
maspadaru 10:b1bdc51e1c50 65 {
maspadaru 10:b1bdc51e1c50 66 if (serial) {
maspadaru 10:b1bdc51e1c50 67 serial->printf("hardware ver: 0x%x firmware ver: %d.%d.%d %s\n", hardware, firmwareMajor, firmwareMinor, firmwareBuild, firmwareType);
maspadaru 10:b1bdc51e1c50 68 }
maspadaru 10:b1bdc51e1c50 69 }
maspadaru 10:b1bdc51e1c50 70
maspadaru 10:b1bdc51e1c50 71 // This variable contains the hardware version number.
maspadaru 10:b1bdc51e1c50 72 uint16_t hardware;
maspadaru 10:b1bdc51e1c50 73 // This variable contains the firmware major version number.
maspadaru 10:b1bdc51e1c50 74 uint8_t firmwareMajor;
maspadaru 10:b1bdc51e1c50 75 // This variable contains the firmware minor version number.
maspadaru 10:b1bdc51e1c50 76 uint8_t firmwareMinor;
maspadaru 10:b1bdc51e1c50 77 // This variable contains the firmware build version number.
maspadaru 10:b1bdc51e1c50 78 uint16_t firmwareBuild;
maspadaru 10:b1bdc51e1c50 79 // This variable contains the a string description of the firmware type.
maspadaru 10:b1bdc51e1c50 80 char firmwareType[10];
maspadaru 10:b1bdc51e1c50 81 };
maspadaru 10:b1bdc51e1c50 82
maspadaru 10:b1bdc51e1c50 83 template <class LinkType>
maspadaru 10:b1bdc51e1c50 84 class TPixy2 {
maspadaru 10:b1bdc51e1c50 85 public:
maspadaru 10:b1bdc51e1c50 86 TPixy2();
maspadaru 10:b1bdc51e1c50 87 TPixy2(SPI* spi);
maspadaru 10:b1bdc51e1c50 88 ~TPixy2();
maspadaru 10:b1bdc51e1c50 89
maspadaru 10:b1bdc51e1c50 90 int8_t init(uint32_t arg = PIXY_DEFAULT_ARGVAL);
maspadaru 10:b1bdc51e1c50 91
maspadaru 10:b1bdc51e1c50 92 /**
maspadaru 10:b1bdc51e1c50 93 * Queries and receives the firmware and hardware version of Pixy2.
maspadaru 10:b1bdc51e1c50 94 * It is called automatically as part of init().
maspadaru 10:b1bdc51e1c50 95 *
maspadaru 10:b1bdc51e1c50 96 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 97 */
maspadaru 10:b1bdc51e1c50 98 int8_t getVersion();
maspadaru 10:b1bdc51e1c50 99
maspadaru 10:b1bdc51e1c50 100 /**
maspadaru 10:b1bdc51e1c50 101 * Instructs Pixy2 to switch programs.
maspadaru 10:b1bdc51e1c50 102 *
maspadaru 10:b1bdc51e1c50 103 * @param prog string with the program name, Values: @TODO
maspadaru 10:b1bdc51e1c50 104 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 105 */
maspadaru 10:b1bdc51e1c50 106 int8_t changeProg(const char* prog);
maspadaru 10:b1bdc51e1c50 107
maspadaru 10:b1bdc51e1c50 108 /**
maspadaru 10:b1bdc51e1c50 109 * Sets the servo positions of servos plugged into Pixy2's two RC servo connectors.
maspadaru 10:b1bdc51e1c50 110 *
maspadaru 10:b1bdc51e1c50 111 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 112 */
maspadaru 10:b1bdc51e1c50 113 int8_t setServos(uint16_t s0, uint16_t s1);
maspadaru 10:b1bdc51e1c50 114
maspadaru 10:b1bdc51e1c50 115 /**
maspadaru 10:b1bdc51e1c50 116 * Sets the relative exposure level of Pixy2's image sensor.
maspadaru 10:b1bdc51e1c50 117 *
maspadaru 10:b1bdc51e1c50 118 * @param brightness value between 0(lowest brightness) and 255(max brightness)
maspadaru 10:b1bdc51e1c50 119 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 120 */
maspadaru 10:b1bdc51e1c50 121 int8_t setCameraBrightness(uint8_t brightness);
maspadaru 10:b1bdc51e1c50 122
maspadaru 10:b1bdc51e1c50 123 /**
maspadaru 10:b1bdc51e1c50 124 * Sets Pixy2's RGB LED value.
maspadaru 10:b1bdc51e1c50 125 *
maspadaru 10:b1bdc51e1c50 126 * @param r red led brightness value between 0 and 255
maspadaru 10:b1bdc51e1c50 127 * @param g green led brightness value between 0 and 255
maspadaru 10:b1bdc51e1c50 128 * @param b blue led brighenss value between 0 and 255
maspadaru 10:b1bdc51e1c50 129 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 130 */
maspadaru 10:b1bdc51e1c50 131 int8_t setLED(uint8_t r, uint8_t g, uint8_t b);
maspadaru 10:b1bdc51e1c50 132
maspadaru 10:b1bdc51e1c50 133 /**
maspadaru 10:b1bdc51e1c50 134 * Turns on/off Pixy2's integrated light source.
maspadaru 10:b1bdc51e1c50 135 *
maspadaru 10:b1bdc51e1c50 136 * @param upper controls the two white LEDs along the top edge of Pixy2's PCB. Values: 0: off; 1: on
maspadaru 10:b1bdc51e1c50 137 * @param lower controls the RGB LED, turning on/off all three color channel, Values: 0: off; 1: on
maspadaru 10:b1bdc51e1c50 138 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 139 */
maspadaru 10:b1bdc51e1c50 140 int8_t setLamp(uint8_t upper, uint8_t lower);
maspadaru 10:b1bdc51e1c50 141
maspadaru 10:b1bdc51e1c50 142 /**
maspadaru 10:b1bdc51e1c50 143 * Gets the width and height of the frames used by the current program.
maspadaru 10:b1bdc51e1c50 144 * After calling this function, the width and height can be found in
maspadaru 10:b1bdc51e1c50 145 * the frameWidth and frameHeight member variables
maspadaru 10:b1bdc51e1c50 146 *
maspadaru 10:b1bdc51e1c50 147 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 148 */
maspadaru 10:b1bdc51e1c50 149 int8_t getResolution();
maspadaru 10:b1bdc51e1c50 150
maspadaru 10:b1bdc51e1c50 151 /**
maspadaru 10:b1bdc51e1c50 152 * Gets Pixy2's framerate.
maspadaru 10:b1bdc51e1c50 153 *
maspadaru 10:b1bdc51e1c50 154 * @returns a value in the range between 2 and 62 frames per second.
maspadaru 10:b1bdc51e1c50 155 */
maspadaru 10:b1bdc51e1c50 156 int8_t getFPS();
maspadaru 10:b1bdc51e1c50 157
maspadaru 10:b1bdc51e1c50 158 Version* version;
maspadaru 10:b1bdc51e1c50 159 uint16_t frameWidth;
maspadaru 10:b1bdc51e1c50 160 uint16_t frameHeight;
maspadaru 10:b1bdc51e1c50 161
maspadaru 10:b1bdc51e1c50 162 // Color connected components, color codes
maspadaru 10:b1bdc51e1c50 163 Pixy2CCC<LinkType> ccc;
maspadaru 10:b1bdc51e1c50 164 friend class Pixy2CCC<LinkType>;
maspadaru 10:b1bdc51e1c50 165
maspadaru 10:b1bdc51e1c50 166 // Line following
maspadaru 10:b1bdc51e1c50 167 Pixy2Line<LinkType> line;
maspadaru 10:b1bdc51e1c50 168 friend class Pixy2Line<LinkType>;
maspadaru 10:b1bdc51e1c50 169
maspadaru 10:b1bdc51e1c50 170 // Video
maspadaru 10:b1bdc51e1c50 171 Pixy2Video<LinkType> video;
maspadaru 10:b1bdc51e1c50 172 friend class Pixy2Video<LinkType>;
maspadaru 10:b1bdc51e1c50 173
maspadaru 10:b1bdc51e1c50 174 LinkType m_link;
maspadaru 10:b1bdc51e1c50 175
maspadaru 10:b1bdc51e1c50 176 private:
maspadaru 10:b1bdc51e1c50 177 void prepareBuffer();
maspadaru 10:b1bdc51e1c50 178 int16_t getSync();
maspadaru 10:b1bdc51e1c50 179 int16_t recvPacket();
maspadaru 10:b1bdc51e1c50 180 int16_t sendPacket();
maspadaru 10:b1bdc51e1c50 181
maspadaru 10:b1bdc51e1c50 182 uint8_t* m_buf;
maspadaru 10:b1bdc51e1c50 183 uint8_t* m_bufPayload;
maspadaru 10:b1bdc51e1c50 184 uint8_t m_type;
maspadaru 10:b1bdc51e1c50 185 uint8_t m_length;
maspadaru 10:b1bdc51e1c50 186 bool m_cs;
maspadaru 10:b1bdc51e1c50 187 };
maspadaru 10:b1bdc51e1c50 188
maspadaru 10:b1bdc51e1c50 189 template <class LinkType>
maspadaru 10:b1bdc51e1c50 190 TPixy2<LinkType>::TPixy2()
maspadaru 10:b1bdc51e1c50 191 : ccc(this)
maspadaru 10:b1bdc51e1c50 192 , line(this)
maspadaru 10:b1bdc51e1c50 193 , video(this)
maspadaru 10:b1bdc51e1c50 194 {
maspadaru 10:b1bdc51e1c50 195 prepareBuffer();
maspadaru 10:b1bdc51e1c50 196 }
maspadaru 10:b1bdc51e1c50 197
maspadaru 10:b1bdc51e1c50 198 template <class LinkType>
maspadaru 10:b1bdc51e1c50 199 TPixy2<LinkType>::TPixy2(SPI* spi)
maspadaru 10:b1bdc51e1c50 200 : ccc(this)
maspadaru 10:b1bdc51e1c50 201 , line(this)
maspadaru 10:b1bdc51e1c50 202 , video(this)
maspadaru 10:b1bdc51e1c50 203 {
maspadaru 10:b1bdc51e1c50 204 prepareBuffer();
maspadaru 10:b1bdc51e1c50 205 }
maspadaru 10:b1bdc51e1c50 206
maspadaru 10:b1bdc51e1c50 207 template <class LinkType>
maspadaru 10:b1bdc51e1c50 208 TPixy2<LinkType>::~TPixy2()
maspadaru 10:b1bdc51e1c50 209 {
maspadaru 10:b1bdc51e1c50 210 m_link.close();
maspadaru 10:b1bdc51e1c50 211 free(m_buf);
maspadaru 10:b1bdc51e1c50 212 }
maspadaru 10:b1bdc51e1c50 213
maspadaru 10:b1bdc51e1c50 214 template <class LinkType>
maspadaru 10:b1bdc51e1c50 215 void TPixy2<LinkType>::prepareBuffer()
maspadaru 10:b1bdc51e1c50 216 {
maspadaru 10:b1bdc51e1c50 217 // allocate buffer space for send/receive
maspadaru 10:b1bdc51e1c50 218 m_buf = (uint8_t*)malloc(PIXY_BUFFERSIZE);
maspadaru 10:b1bdc51e1c50 219 // shifted buffer is used for sending, so we have space to write header information
maspadaru 10:b1bdc51e1c50 220 m_bufPayload = m_buf + PIXY_SEND_HEADER_SIZE;
maspadaru 10:b1bdc51e1c50 221 frameWidth = frameHeight = 0;
maspadaru 10:b1bdc51e1c50 222 version = NULL;
maspadaru 10:b1bdc51e1c50 223 }
maspadaru 10:b1bdc51e1c50 224
maspadaru 10:b1bdc51e1c50 225 template <class LinkType>
maspadaru 10:b1bdc51e1c50 226 int8_t TPixy2<LinkType>::init(uint32_t arg)
maspadaru 10:b1bdc51e1c50 227 {
maspadaru 10:b1bdc51e1c50 228 uint32_t t0;
maspadaru 10:b1bdc51e1c50 229 int8_t res;
maspadaru 10:b1bdc51e1c50 230 Timer timer;
maspadaru 10:b1bdc51e1c50 231
maspadaru 10:b1bdc51e1c50 232 res = m_link.open(arg);
maspadaru 10:b1bdc51e1c50 233 if (res < 0)
maspadaru 10:b1bdc51e1c50 234 return res;
maspadaru 10:b1bdc51e1c50 235
maspadaru 10:b1bdc51e1c50 236 timer.start();
maspadaru 10:b1bdc51e1c50 237 // wait for pixy to be ready -- that is, Pixy takes a second or 2 boot up
maspadaru 10:b1bdc51e1c50 238 // getVersion is an effective "ping". We timeout after 5s.
maspadaru 10:b1bdc51e1c50 239 for (t0 = timer.read_ms(); timer.read_ms() - t0 < 5000;) {
maspadaru 10:b1bdc51e1c50 240 if (getVersion() >= 0) // successful version get -> pixy is ready
maspadaru 10:b1bdc51e1c50 241 {
maspadaru 10:b1bdc51e1c50 242 getResolution(); // get resolution so we have it
maspadaru 10:b1bdc51e1c50 243 return PIXY_RESULT_OK;
maspadaru 10:b1bdc51e1c50 244 }
maspadaru 10:b1bdc51e1c50 245 wait_ms(5000); // delay for sync
maspadaru 10:b1bdc51e1c50 246 }
maspadaru 10:b1bdc51e1c50 247 // timeout
maspadaru 10:b1bdc51e1c50 248 return PIXY_RESULT_TIMEOUT;
maspadaru 10:b1bdc51e1c50 249 }
maspadaru 10:b1bdc51e1c50 250
maspadaru 10:b1bdc51e1c50 251 template <class LinkType>
maspadaru 10:b1bdc51e1c50 252 int16_t TPixy2<LinkType>::getSync()
maspadaru 10:b1bdc51e1c50 253 {
maspadaru 10:b1bdc51e1c50 254 uint8_t i, j, c, cprev;
maspadaru 10:b1bdc51e1c50 255 int16_t res;
maspadaru 10:b1bdc51e1c50 256 uint16_t start;
maspadaru 10:b1bdc51e1c50 257
maspadaru 10:b1bdc51e1c50 258 // parse bytes until we find sync
maspadaru 10:b1bdc51e1c50 259 for (i = j = 0, cprev = 0; true; i++) {
maspadaru 10:b1bdc51e1c50 260 res = m_link.recv(&c, 1);
maspadaru 10:b1bdc51e1c50 261 if (res >= PIXY_RESULT_OK) {
maspadaru 10:b1bdc51e1c50 262 // since we're using little endian, previous byte is least significant byte
maspadaru 10:b1bdc51e1c50 263 start = cprev;
maspadaru 10:b1bdc51e1c50 264 // current byte is most significant byte
maspadaru 10:b1bdc51e1c50 265 start |= c << 8;
maspadaru 10:b1bdc51e1c50 266 cprev = c;
maspadaru 10:b1bdc51e1c50 267 if (start == PIXY_CHECKSUM_SYNC) {
maspadaru 10:b1bdc51e1c50 268 m_cs = true;
maspadaru 10:b1bdc51e1c50 269 return PIXY_RESULT_OK;
maspadaru 10:b1bdc51e1c50 270 }
maspadaru 10:b1bdc51e1c50 271 if (start == PIXY_NO_CHECKSUM_SYNC) {
maspadaru 10:b1bdc51e1c50 272 m_cs = false;
maspadaru 10:b1bdc51e1c50 273 return PIXY_RESULT_OK;
maspadaru 10:b1bdc51e1c50 274 }
maspadaru 10:b1bdc51e1c50 275 }
maspadaru 10:b1bdc51e1c50 276 // If we've read some bytes and no sync, then wait and try again.
maspadaru 10:b1bdc51e1c50 277 // And do that several more times before we give up.
maspadaru 10:b1bdc51e1c50 278 // Pixy guarantees to respond within 100us.
maspadaru 10:b1bdc51e1c50 279 if (i >= 4) {
maspadaru 10:b1bdc51e1c50 280 if (j >= 4) {
maspadaru 10:b1bdc51e1c50 281 #ifdef PIXY_DEBUG
maspadaru 10:b1bdc51e1c50 282 // Serial.println("error: no response");
maspadaru 10:b1bdc51e1c50 283 #endif
maspadaru 10:b1bdc51e1c50 284 return PIXY_RESULT_ERROR;
maspadaru 10:b1bdc51e1c50 285 }
maspadaru 10:b1bdc51e1c50 286 wait_ms(25);
maspadaru 10:b1bdc51e1c50 287 j++;
maspadaru 10:b1bdc51e1c50 288 i = 0;
maspadaru 10:b1bdc51e1c50 289 }
maspadaru 10:b1bdc51e1c50 290 }
maspadaru 10:b1bdc51e1c50 291 }
maspadaru 10:b1bdc51e1c50 292
maspadaru 10:b1bdc51e1c50 293 template <class LinkType>
maspadaru 10:b1bdc51e1c50 294 int16_t TPixy2<LinkType>::recvPacket()
maspadaru 10:b1bdc51e1c50 295 {
maspadaru 10:b1bdc51e1c50 296 uint16_t csCalc, csSerial;
maspadaru 10:b1bdc51e1c50 297 int16_t res;
maspadaru 10:b1bdc51e1c50 298
maspadaru 10:b1bdc51e1c50 299 res = getSync();
maspadaru 10:b1bdc51e1c50 300 if (res < 0)
maspadaru 10:b1bdc51e1c50 301 return res;
maspadaru 10:b1bdc51e1c50 302
maspadaru 10:b1bdc51e1c50 303 if (m_cs) {
maspadaru 10:b1bdc51e1c50 304 res = m_link.recv(m_buf, 4);
maspadaru 10:b1bdc51e1c50 305 if (res < 0)
maspadaru 10:b1bdc51e1c50 306 return res;
maspadaru 10:b1bdc51e1c50 307
maspadaru 10:b1bdc51e1c50 308 m_type = m_buf[0];
maspadaru 10:b1bdc51e1c50 309 m_length = m_buf[1];
maspadaru 10:b1bdc51e1c50 310
maspadaru 10:b1bdc51e1c50 311 csSerial = *(uint16_t*)&m_buf[2];
maspadaru 10:b1bdc51e1c50 312
maspadaru 10:b1bdc51e1c50 313 res = m_link.recv(m_buf, m_length, &csCalc);
maspadaru 10:b1bdc51e1c50 314 if (res < 0)
maspadaru 10:b1bdc51e1c50 315 return res;
maspadaru 10:b1bdc51e1c50 316
maspadaru 10:b1bdc51e1c50 317 if (csSerial != csCalc) {
maspadaru 10:b1bdc51e1c50 318 #ifdef PIXY_DEBUG
maspadaru 10:b1bdc51e1c50 319 // Serial.println("error: checksum");
maspadaru 10:b1bdc51e1c50 320 #endif
maspadaru 10:b1bdc51e1c50 321 return PIXY_RESULT_CHECKSUM_ERROR;
maspadaru 10:b1bdc51e1c50 322 }
maspadaru 10:b1bdc51e1c50 323 } else {
maspadaru 10:b1bdc51e1c50 324 res = m_link.recv(m_buf, 2);
maspadaru 10:b1bdc51e1c50 325 if (res < 0)
maspadaru 10:b1bdc51e1c50 326 return res;
maspadaru 10:b1bdc51e1c50 327
maspadaru 10:b1bdc51e1c50 328 m_type = m_buf[0];
maspadaru 10:b1bdc51e1c50 329 m_length = m_buf[1];
maspadaru 10:b1bdc51e1c50 330
maspadaru 10:b1bdc51e1c50 331 res = m_link.recv(m_buf, m_length);
maspadaru 10:b1bdc51e1c50 332 if (res < 0)
maspadaru 10:b1bdc51e1c50 333 return res;
maspadaru 10:b1bdc51e1c50 334 }
maspadaru 10:b1bdc51e1c50 335 return PIXY_RESULT_OK;
maspadaru 10:b1bdc51e1c50 336 }
maspadaru 10:b1bdc51e1c50 337
maspadaru 10:b1bdc51e1c50 338 template <class LinkType>
maspadaru 10:b1bdc51e1c50 339 int16_t TPixy2<LinkType>::sendPacket()
maspadaru 10:b1bdc51e1c50 340 {
maspadaru 10:b1bdc51e1c50 341 // write header info at beginnig of buffer
maspadaru 10:b1bdc51e1c50 342 m_buf[0] = PIXY_NO_CHECKSUM_SYNC & 0xff;
maspadaru 10:b1bdc51e1c50 343 m_buf[1] = PIXY_NO_CHECKSUM_SYNC >> 8;
maspadaru 10:b1bdc51e1c50 344 m_buf[2] = m_type;
maspadaru 10:b1bdc51e1c50 345 m_buf[3] = m_length;
maspadaru 10:b1bdc51e1c50 346 // send whole thing -- header and data in one call
maspadaru 10:b1bdc51e1c50 347 return m_link.send(m_buf, m_length + PIXY_SEND_HEADER_SIZE);
maspadaru 10:b1bdc51e1c50 348 }
maspadaru 10:b1bdc51e1c50 349
maspadaru 10:b1bdc51e1c50 350 template <class LinkType>
maspadaru 10:b1bdc51e1c50 351 int8_t TPixy2<LinkType>::changeProg(const char* prog)
maspadaru 10:b1bdc51e1c50 352 {
maspadaru 10:b1bdc51e1c50 353 int32_t res;
maspadaru 10:b1bdc51e1c50 354
maspadaru 10:b1bdc51e1c50 355 // poll for program to change
maspadaru 10:b1bdc51e1c50 356 while (1) {
maspadaru 10:b1bdc51e1c50 357 strncpy((char*)m_bufPayload, prog, PIXY_MAX_PROGNAME);
maspadaru 10:b1bdc51e1c50 358 m_length = PIXY_MAX_PROGNAME;
maspadaru 10:b1bdc51e1c50 359 m_type = PIXY_TYPE_REQUEST_CHANGE_PROG;
maspadaru 10:b1bdc51e1c50 360 sendPacket();
maspadaru 10:b1bdc51e1c50 361 if (recvPacket() == 0) {
maspadaru 10:b1bdc51e1c50 362 res = *(uint32_t*)m_buf;
maspadaru 10:b1bdc51e1c50 363 if (res > 0) {
maspadaru 10:b1bdc51e1c50 364 getResolution(); // get resolution so we have it
maspadaru 10:b1bdc51e1c50 365 return PIXY_RESULT_OK; // success
maspadaru 10:b1bdc51e1c50 366 }
maspadaru 10:b1bdc51e1c50 367 } else
maspadaru 10:b1bdc51e1c50 368 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 369 wait_ms(1000);
maspadaru 10:b1bdc51e1c50 370 }
maspadaru 10:b1bdc51e1c50 371 }
maspadaru 10:b1bdc51e1c50 372
maspadaru 10:b1bdc51e1c50 373 template <class LinkType>
maspadaru 10:b1bdc51e1c50 374 int8_t TPixy2<LinkType>::getVersion()
maspadaru 10:b1bdc51e1c50 375 {
maspadaru 10:b1bdc51e1c50 376 m_length = 0;
maspadaru 10:b1bdc51e1c50 377 m_type = PIXY_TYPE_REQUEST_VERSION;
maspadaru 10:b1bdc51e1c50 378 sendPacket();
maspadaru 10:b1bdc51e1c50 379 if (recvPacket() == 0) {
maspadaru 10:b1bdc51e1c50 380 if (m_type == PIXY_TYPE_RESPONSE_VERSION) {
maspadaru 10:b1bdc51e1c50 381 version = (Version*)m_buf;
maspadaru 10:b1bdc51e1c50 382 return m_length;
maspadaru 10:b1bdc51e1c50 383 } else if (m_type == PIXY_TYPE_RESPONSE_ERROR)
maspadaru 10:b1bdc51e1c50 384 return PIXY_RESULT_BUSY;
maspadaru 10:b1bdc51e1c50 385 }
maspadaru 10:b1bdc51e1c50 386 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 387 }
maspadaru 10:b1bdc51e1c50 388
maspadaru 10:b1bdc51e1c50 389 template <class LinkType>
maspadaru 10:b1bdc51e1c50 390 int8_t TPixy2<LinkType>::getResolution()
maspadaru 10:b1bdc51e1c50 391 {
maspadaru 10:b1bdc51e1c50 392 m_length = 1;
maspadaru 10:b1bdc51e1c50 393 m_bufPayload[0] = 0; // for future types of queries
maspadaru 10:b1bdc51e1c50 394 m_type = PIXY_TYPE_REQUEST_RESOLUTION;
maspadaru 10:b1bdc51e1c50 395 sendPacket();
maspadaru 10:b1bdc51e1c50 396 if (recvPacket() == 0) {
maspadaru 10:b1bdc51e1c50 397 if (m_type == PIXY_TYPE_RESPONSE_RESOLUTION) {
maspadaru 10:b1bdc51e1c50 398 frameWidth = *(uint16_t*)m_buf;
maspadaru 10:b1bdc51e1c50 399 frameHeight = *(uint16_t*)(m_buf + sizeof(uint16_t));
maspadaru 10:b1bdc51e1c50 400 return PIXY_RESULT_OK; // success
maspadaru 10:b1bdc51e1c50 401 } else
maspadaru 10:b1bdc51e1c50 402 return PIXY_RESULT_ERROR;
maspadaru 10:b1bdc51e1c50 403 } else
maspadaru 10:b1bdc51e1c50 404 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 405 }
maspadaru 10:b1bdc51e1c50 406
maspadaru 10:b1bdc51e1c50 407 template <class LinkType>
maspadaru 10:b1bdc51e1c50 408 int8_t TPixy2<LinkType>::setCameraBrightness(uint8_t brightness)
maspadaru 10:b1bdc51e1c50 409 {
maspadaru 10:b1bdc51e1c50 410 uint32_t res;
maspadaru 10:b1bdc51e1c50 411
maspadaru 10:b1bdc51e1c50 412 m_bufPayload[0] = brightness;
maspadaru 10:b1bdc51e1c50 413 m_length = 1;
maspadaru 10:b1bdc51e1c50 414 m_type = PIXY_TYPE_REQUEST_BRIGHTNESS;
maspadaru 10:b1bdc51e1c50 415 sendPacket();
maspadaru 10:b1bdc51e1c50 416 if (recvPacket() == 0) // && m_type==PIXY_TYPE_RESPONSE_RESULT && m_length==4)
maspadaru 10:b1bdc51e1c50 417 {
maspadaru 10:b1bdc51e1c50 418 res = *(uint32_t*)m_buf;
maspadaru 10:b1bdc51e1c50 419 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 420 } else
maspadaru 10:b1bdc51e1c50 421 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 422 }
maspadaru 10:b1bdc51e1c50 423
maspadaru 10:b1bdc51e1c50 424 template <class LinkType>
maspadaru 10:b1bdc51e1c50 425 int8_t TPixy2<LinkType>::setServos(uint16_t s0, uint16_t s1)
maspadaru 10:b1bdc51e1c50 426 {
maspadaru 10:b1bdc51e1c50 427 uint32_t res;
maspadaru 10:b1bdc51e1c50 428
maspadaru 10:b1bdc51e1c50 429 *(int16_t*)(m_bufPayload + 0) = s0;
maspadaru 10:b1bdc51e1c50 430 *(int16_t*)(m_bufPayload + 2) = s1;
maspadaru 10:b1bdc51e1c50 431 m_length = 4;
maspadaru 10:b1bdc51e1c50 432 m_type = PIXY_TYPE_REQUEST_SERVO;
maspadaru 10:b1bdc51e1c50 433 sendPacket();
maspadaru 10:b1bdc51e1c50 434 if (recvPacket() == 0 && m_type == PIXY_TYPE_RESPONSE_RESULT && m_length == 4) {
maspadaru 10:b1bdc51e1c50 435 res = *(uint32_t*)m_buf;
maspadaru 10:b1bdc51e1c50 436 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 437 } else
maspadaru 10:b1bdc51e1c50 438 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 439 }
maspadaru 10:b1bdc51e1c50 440
maspadaru 10:b1bdc51e1c50 441 template <class LinkType>
maspadaru 10:b1bdc51e1c50 442 int8_t TPixy2<LinkType>::setLED(uint8_t r, uint8_t g, uint8_t b)
maspadaru 10:b1bdc51e1c50 443 {
maspadaru 10:b1bdc51e1c50 444 uint32_t res;
maspadaru 10:b1bdc51e1c50 445
maspadaru 10:b1bdc51e1c50 446 m_bufPayload[0] = r;
maspadaru 10:b1bdc51e1c50 447 m_bufPayload[1] = g;
maspadaru 10:b1bdc51e1c50 448 m_bufPayload[2] = b;
maspadaru 10:b1bdc51e1c50 449 m_length = 3;
maspadaru 10:b1bdc51e1c50 450 m_type = PIXY_TYPE_REQUEST_LED;
maspadaru 10:b1bdc51e1c50 451 sendPacket();
maspadaru 10:b1bdc51e1c50 452 if (recvPacket() == 0 && m_type == PIXY_TYPE_RESPONSE_RESULT && m_length == 4) {
maspadaru 10:b1bdc51e1c50 453 res = *(uint32_t*)m_buf;
maspadaru 10:b1bdc51e1c50 454 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 455 } else
maspadaru 10:b1bdc51e1c50 456 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 457 }
maspadaru 10:b1bdc51e1c50 458
maspadaru 10:b1bdc51e1c50 459 template <class LinkType>
maspadaru 10:b1bdc51e1c50 460 int8_t TPixy2<LinkType>::setLamp(uint8_t upper, uint8_t lower)
maspadaru 10:b1bdc51e1c50 461 {
maspadaru 10:b1bdc51e1c50 462 uint32_t res;
maspadaru 10:b1bdc51e1c50 463
maspadaru 10:b1bdc51e1c50 464 m_bufPayload[0] = upper;
maspadaru 10:b1bdc51e1c50 465 m_bufPayload[1] = lower;
maspadaru 10:b1bdc51e1c50 466 m_length = 2;
maspadaru 10:b1bdc51e1c50 467 m_type = PIXY_TYPE_REQUEST_LAMP;
maspadaru 10:b1bdc51e1c50 468 sendPacket();
maspadaru 10:b1bdc51e1c50 469 if (recvPacket() == 0 && m_type == PIXY_TYPE_RESPONSE_RESULT && m_length == 4) {
maspadaru 10:b1bdc51e1c50 470 res = *(uint32_t*)m_buf;
maspadaru 10:b1bdc51e1c50 471 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 472 } else
maspadaru 10:b1bdc51e1c50 473 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 474 }
maspadaru 10:b1bdc51e1c50 475
maspadaru 10:b1bdc51e1c50 476 template <class LinkType>
maspadaru 10:b1bdc51e1c50 477 int8_t TPixy2<LinkType>::getFPS()
maspadaru 10:b1bdc51e1c50 478 {
maspadaru 10:b1bdc51e1c50 479 uint32_t res;
maspadaru 10:b1bdc51e1c50 480
maspadaru 10:b1bdc51e1c50 481 m_length = 0; // no args
maspadaru 10:b1bdc51e1c50 482 m_type = PIXY_TYPE_REQUEST_FPS;
maspadaru 10:b1bdc51e1c50 483 sendPacket();
maspadaru 10:b1bdc51e1c50 484 if (recvPacket() == 0 && m_type == PIXY_TYPE_RESPONSE_RESULT && m_length == 4) {
maspadaru 10:b1bdc51e1c50 485 res = *(uint32_t*)m_buf;
maspadaru 10:b1bdc51e1c50 486 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 487 } else
maspadaru 10:b1bdc51e1c50 488 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 489 }
maspadaru 10:b1bdc51e1c50 490
maspadaru 10:b1bdc51e1c50 491 #endif
maspadaru 10:b1bdc51e1c50 492