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/Pixy2Line.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 // This file is for defining the Block struct and the Pixy template class version 2.
maspadaru 10:b1bdc51e1c50 16 // (TPixy2). TPixy takes a communication link as a template parameter so that
maspadaru 10:b1bdc51e1c50 17 // all communication modes (SPI, I2C and UART) can share the same code.
maspadaru 10:b1bdc51e1c50 18 //
maspadaru 10:b1bdc51e1c50 19
maspadaru 10:b1bdc51e1c50 20 #ifndef _PIXY2LINE_H
maspadaru 10:b1bdc51e1c50 21 #define _PIXY2LINE_H
maspadaru 10:b1bdc51e1c50 22
maspadaru 10:b1bdc51e1c50 23 #define LINE_REQUEST_GET_FEATURES 0x30
maspadaru 10:b1bdc51e1c50 24 #define LINE_RESPONSE_GET_FEATURES 0x31
maspadaru 10:b1bdc51e1c50 25 #define LINE_REQUEST_SET_MODE 0x36
maspadaru 10:b1bdc51e1c50 26 #define LINE_REQUEST_SET_VECTOR 0x38
maspadaru 10:b1bdc51e1c50 27 #define LINE_REQUEST_SET_NEXT_TURN_ANGLE 0x3a
maspadaru 10:b1bdc51e1c50 28 #define LINE_REQUEST_SET_DEFAULT_TURN_ANGLE 0x3c
maspadaru 10:b1bdc51e1c50 29 #define LINE_REQUEST_REVERSE_VECTOR 0x3e
maspadaru 10:b1bdc51e1c50 30
maspadaru 10:b1bdc51e1c50 31 #define LINE_GET_MAIN_FEATURES 0x00
maspadaru 10:b1bdc51e1c50 32 #define LINE_GET_ALL_FEATURES 0x01
maspadaru 10:b1bdc51e1c50 33
maspadaru 10:b1bdc51e1c50 34 #define LINE_MODE_TURN_DELAYED 0x01
maspadaru 10:b1bdc51e1c50 35 #define LINE_MODE_MANUAL_SELECT_VECTOR 0x02
maspadaru 10:b1bdc51e1c50 36 #define LINE_MODE_WHITE_LINE 0x80
maspadaru 10:b1bdc51e1c50 37
maspadaru 10:b1bdc51e1c50 38 // features
maspadaru 10:b1bdc51e1c50 39 #define LINE_VECTOR 0x01
maspadaru 10:b1bdc51e1c50 40 #define LINE_INTERSECTION 0x02
maspadaru 10:b1bdc51e1c50 41 #define LINE_BARCODE 0x04
maspadaru 10:b1bdc51e1c50 42 #define LINE_ALL_FEATURES (LINE_VECTOR | LINE_INTERSECTION | LINE_BARCODE)
maspadaru 10:b1bdc51e1c50 43
maspadaru 10:b1bdc51e1c50 44 #define LINE_FLAG_INVALID 0x02
maspadaru 10:b1bdc51e1c50 45 #define LINE_FLAG_INTERSECTION_PRESENT 0x04
maspadaru 10:b1bdc51e1c50 46
maspadaru 10:b1bdc51e1c50 47 #define LINE_MAX_INTERSECTION_LINES 6
maspadaru 10:b1bdc51e1c50 48
maspadaru 10:b1bdc51e1c50 49 #include <stdint.h>
maspadaru 10:b1bdc51e1c50 50
maspadaru 10:b1bdc51e1c50 51 struct Vector {
maspadaru 10:b1bdc51e1c50 52 void print(Serial* serial)
maspadaru 10:b1bdc51e1c50 53 {
maspadaru 10:b1bdc51e1c50 54 if (serial) {
maspadaru 10:b1bdc51e1c50 55 serial->printf("vector: (%d %d) (%d %d) index: %d flags %d\n", m_x0, m_y0, m_x1, m_y1, m_index, m_flags);
maspadaru 10:b1bdc51e1c50 56 }
maspadaru 10:b1bdc51e1c50 57 }
maspadaru 10:b1bdc51e1c50 58
maspadaru 10:b1bdc51e1c50 59 uint8_t m_x0;
maspadaru 10:b1bdc51e1c50 60 uint8_t m_y0;
maspadaru 10:b1bdc51e1c50 61 uint8_t m_x1;
maspadaru 10:b1bdc51e1c50 62 uint8_t m_y1;
maspadaru 10:b1bdc51e1c50 63 uint8_t m_index;
maspadaru 10:b1bdc51e1c50 64 uint8_t m_flags;
maspadaru 10:b1bdc51e1c50 65 };
maspadaru 10:b1bdc51e1c50 66
maspadaru 10:b1bdc51e1c50 67 struct IntersectionLine {
maspadaru 10:b1bdc51e1c50 68 uint8_t m_index;
maspadaru 10:b1bdc51e1c50 69 uint8_t m_reserved;
maspadaru 10:b1bdc51e1c50 70 int16_t m_angle;
maspadaru 10:b1bdc51e1c50 71 };
maspadaru 10:b1bdc51e1c50 72
maspadaru 10:b1bdc51e1c50 73 struct Intersection {
maspadaru 10:b1bdc51e1c50 74 void print(Serial* serial)
maspadaru 10:b1bdc51e1c50 75 {
maspadaru 10:b1bdc51e1c50 76 if (serial) {
maspadaru 10:b1bdc51e1c50 77 uint8_t i;
maspadaru 10:b1bdc51e1c50 78 serial->printf("intersection: (%d %d)\n", m_x, m_y);
maspadaru 10:b1bdc51e1c50 79 for (i = 0; i < m_n; i++) {
maspadaru 10:b1bdc51e1c50 80 serial->printf(" %d: index: %d angle: %d\n", i, m_intLines[i].m_index, m_intLines[i].m_angle);
maspadaru 10:b1bdc51e1c50 81 }
maspadaru 10:b1bdc51e1c50 82 }
maspadaru 10:b1bdc51e1c50 83 }
maspadaru 10:b1bdc51e1c50 84
maspadaru 10:b1bdc51e1c50 85 uint8_t m_x;
maspadaru 10:b1bdc51e1c50 86 uint8_t m_y;
maspadaru 10:b1bdc51e1c50 87
maspadaru 10:b1bdc51e1c50 88 uint8_t m_n;
maspadaru 10:b1bdc51e1c50 89 uint8_t m_reserved;
maspadaru 10:b1bdc51e1c50 90 IntersectionLine m_intLines[LINE_MAX_INTERSECTION_LINES];
maspadaru 10:b1bdc51e1c50 91 };
maspadaru 10:b1bdc51e1c50 92
maspadaru 10:b1bdc51e1c50 93 struct Barcode {
maspadaru 10:b1bdc51e1c50 94 void print(Serial* serial)
maspadaru 10:b1bdc51e1c50 95 {
maspadaru 10:b1bdc51e1c50 96 if (serial) {
maspadaru 10:b1bdc51e1c50 97 serial->printf("Barcode: (%d %d), val: %d flags: %d\n", m_x, m_y, m_code, m_flags);
maspadaru 10:b1bdc51e1c50 98 }
maspadaru 10:b1bdc51e1c50 99 }
maspadaru 10:b1bdc51e1c50 100
maspadaru 10:b1bdc51e1c50 101 uint8_t m_x;
maspadaru 10:b1bdc51e1c50 102 uint8_t m_y;
maspadaru 10:b1bdc51e1c50 103 uint8_t m_flags;
maspadaru 10:b1bdc51e1c50 104 uint8_t m_code;
maspadaru 10:b1bdc51e1c50 105 };
maspadaru 10:b1bdc51e1c50 106
maspadaru 10:b1bdc51e1c50 107 template <class LinkType>
maspadaru 10:b1bdc51e1c50 108 class TPixy2;
maspadaru 10:b1bdc51e1c50 109
maspadaru 10:b1bdc51e1c50 110 template <class LinkType>
maspadaru 10:b1bdc51e1c50 111 class Pixy2Line {
maspadaru 10:b1bdc51e1c50 112 public:
maspadaru 10:b1bdc51e1c50 113 Pixy2Line(TPixy2<LinkType>* pixy)
maspadaru 10:b1bdc51e1c50 114 {
maspadaru 10:b1bdc51e1c50 115 m_pixy = pixy;
maspadaru 10:b1bdc51e1c50 116 }
maspadaru 10:b1bdc51e1c50 117
maspadaru 10:b1bdc51e1c50 118 /**
maspadaru 10:b1bdc51e1c50 119 * This function gets the latest features including the Vector, any intersection that connects to the Vector, and barcodes.
maspadaru 10:b1bdc51e1c50 120 *
maspadaru 10:b1bdc51e1c50 121 * The results are returned in the variables {@link vectors}, {@link intersections}, and {@link barcodes}, respectively.
maspadaru 10:b1bdc51e1c50 122 *
maspadaru 10:b1bdc51e1c50 123 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 124 */
maspadaru 10:b1bdc51e1c50 125 int8_t getMainFeatures(uint8_t features = LINE_ALL_FEATURES, bool wait = true)
maspadaru 10:b1bdc51e1c50 126 {
maspadaru 10:b1bdc51e1c50 127 return getFeatures(LINE_GET_MAIN_FEATURES, features, wait);
maspadaru 10:b1bdc51e1c50 128 }
maspadaru 10:b1bdc51e1c50 129
maspadaru 10:b1bdc51e1c50 130 /**
maspadaru 10:b1bdc51e1c50 131 * This function returns all lines, intersections and barcodes that the line tracking algorithm detects.
maspadaru 10:b1bdc51e1c50 132 *
maspadaru 10:b1bdc51e1c50 133 * The results are returned in the variables {@link vectors}, {@link intersections}, and {@link barcodes}, respectively.
maspadaru 10:b1bdc51e1c50 134 *
maspadaru 10:b1bdc51e1c50 135 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 136 */
maspadaru 10:b1bdc51e1c50 137 int8_t getAllFeatures(uint8_t features = LINE_ALL_FEATURES, bool wait = true)
maspadaru 10:b1bdc51e1c50 138 {
maspadaru 10:b1bdc51e1c50 139 return getFeatures(LINE_GET_ALL_FEATURES, features, wait);
maspadaru 10:b1bdc51e1c50 140 }
maspadaru 10:b1bdc51e1c50 141
maspadaru 10:b1bdc51e1c50 142 /**
maspadaru 10:b1bdc51e1c50 143 * This function sets various modes in the line tracking algorithm.
maspadaru 10:b1bdc51e1c50 144 *
maspadaru 10:b1bdc51e1c50 145 * @param mode argument consists of a bitwise-ORing of the following Values:
maspadaru 10:b1bdc51e1c50 146 * LINE_MODE_TURN_DELAYED :
maspadaru 10:b1bdc51e1c50 147 * will prevent the line tracking algorithm from choosing the path automatically;
maspadaru 10:b1bdc51e1c50 148 * LINE_MODE_MANUAL_SELECT_VECTOR :
maspadaru 10:b1bdc51e1c50 149 * will prevent the line tracking algorithm from choosing the Vector automatically;
maspadaru 10:b1bdc51e1c50 150 * LINE_MODE_WHITE_LINE :
maspadaru 10:b1bdc51e1c50 151 * will instruct the line tracking algorithm to look for light lines on a dark background.
maspadaru 10:b1bdc51e1c50 152 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 153 */
maspadaru 10:b1bdc51e1c50 154 int8_t setMode(uint8_t mode);
maspadaru 10:b1bdc51e1c50 155
maspadaru 10:b1bdc51e1c50 156 /**
maspadaru 10:b1bdc51e1c50 157 * This function tells the line tracking algorithm which path it should take at the next intersection.
maspadaru 10:b1bdc51e1c50 158 *
maspadaru 10:b1bdc51e1c50 159 * setNextTurn() will remember the turn angle you give it, and execute it at the next intersection.
maspadaru 10:b1bdc51e1c50 160 * The line tracking algorithm will then go back to the default turn angle for subsequent intersections.
maspadaru 10:b1bdc51e1c50 161 *
maspadaru 10:b1bdc51e1c50 162 * @param angle turn angle in degrees, with 0 being straight ahead. Valide angles are between -180 and 180
maspadaru 10:b1bdc51e1c50 163 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 164 */
maspadaru 10:b1bdc51e1c50 165 int8_t setNextTurn(int16_t angle);
maspadaru 10:b1bdc51e1c50 166
maspadaru 10:b1bdc51e1c50 167 /**
maspadaru 10:b1bdc51e1c50 168 * This function tells the line tracking algorithm which path to choose by default upon encountering an intersection.
maspadaru 10:b1bdc51e1c50 169 *
maspadaru 10:b1bdc51e1c50 170 * @param angle turn angle in degrees, with 0 being straight ahead. Valide angles are between -180 and 180
maspadaru 10:b1bdc51e1c50 171 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 172 */
maspadaru 10:b1bdc51e1c50 173 int8_t setDefaultTurn(int16_t angle);
maspadaru 10:b1bdc51e1c50 174
maspadaru 10:b1bdc51e1c50 175 /**
maspadaru 10:b1bdc51e1c50 176 * Set the vector the line tracking algorithm will follow if the LINE_MODE_MANUAL_SELECT_VECTOR mode bit is set.
maspadaru 10:b1bdc51e1c50 177 *
maspadaru 10:b1bdc51e1c50 178 * @param index vector provided by index of the line
maspadaru 10:b1bdc51e1c50 179 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 180 */
maspadaru 10:b1bdc51e1c50 181 int8_t setVector(uint8_t index);
maspadaru 10:b1bdc51e1c50 182
maspadaru 10:b1bdc51e1c50 183 /**
maspadaru 10:b1bdc51e1c50 184 * Reverse direction of the vector the line tracking algorithm is currently following.
maspadaru 10:b1bdc51e1c50 185 *
maspadaru 10:b1bdc51e1c50 186 * The Vector has a direction. It normally points up, from the bottom of the camera frame to the top of the
maspadaru 10:b1bdc51e1c50 187 * camera frame for a forward-moving robot. Calling reverseVector() will invert the vector.
maspadaru 10:b1bdc51e1c50 188 * This will typically cause your robot to back-up and change directions.
maspadaru 10:b1bdc51e1c50 189 *
maspadaru 10:b1bdc51e1c50 190 * @returns an error value (<0) if it fails and PIXY_RESULT_OK if it succeeds
maspadaru 10:b1bdc51e1c50 191 */
maspadaru 10:b1bdc51e1c50 192 int8_t reverseVector();
maspadaru 10:b1bdc51e1c50 193
maspadaru 10:b1bdc51e1c50 194 uint8_t numVectors;
maspadaru 10:b1bdc51e1c50 195 Vector* vectors;
maspadaru 10:b1bdc51e1c50 196
maspadaru 10:b1bdc51e1c50 197 uint8_t numIntersections;
maspadaru 10:b1bdc51e1c50 198 Intersection* intersections;
maspadaru 10:b1bdc51e1c50 199
maspadaru 10:b1bdc51e1c50 200 uint8_t numBarcodes;
maspadaru 10:b1bdc51e1c50 201 Barcode* barcodes;
maspadaru 10:b1bdc51e1c50 202
maspadaru 10:b1bdc51e1c50 203 private:
maspadaru 10:b1bdc51e1c50 204 int8_t getFeatures(uint8_t type, uint8_t features, bool wait);
maspadaru 10:b1bdc51e1c50 205 TPixy2<LinkType>* m_pixy;
maspadaru 10:b1bdc51e1c50 206 };
maspadaru 10:b1bdc51e1c50 207
maspadaru 10:b1bdc51e1c50 208 template <class LinkType>
maspadaru 10:b1bdc51e1c50 209 int8_t Pixy2Line<LinkType>::getFeatures(uint8_t type, uint8_t features, bool wait)
maspadaru 10:b1bdc51e1c50 210 {
maspadaru 10:b1bdc51e1c50 211 int8_t res;
maspadaru 10:b1bdc51e1c50 212 uint8_t offset, fsize, ftype, *fdata;
maspadaru 10:b1bdc51e1c50 213
maspadaru 10:b1bdc51e1c50 214 vectors = NULL;
maspadaru 10:b1bdc51e1c50 215 numVectors = 0;
maspadaru 10:b1bdc51e1c50 216 intersections = NULL;
maspadaru 10:b1bdc51e1c50 217 numIntersections = 0;
maspadaru 10:b1bdc51e1c50 218 barcodes = NULL;
maspadaru 10:b1bdc51e1c50 219 numBarcodes = 0;
maspadaru 10:b1bdc51e1c50 220
maspadaru 10:b1bdc51e1c50 221 while (1) {
maspadaru 10:b1bdc51e1c50 222 // fill in request data
maspadaru 10:b1bdc51e1c50 223 m_pixy->m_length = 2;
maspadaru 10:b1bdc51e1c50 224 m_pixy->m_type = LINE_REQUEST_GET_FEATURES;
maspadaru 10:b1bdc51e1c50 225 m_pixy->m_bufPayload[0] = type;
maspadaru 10:b1bdc51e1c50 226 m_pixy->m_bufPayload[1] = features;
maspadaru 10:b1bdc51e1c50 227
maspadaru 10:b1bdc51e1c50 228 // send request
maspadaru 10:b1bdc51e1c50 229 m_pixy->sendPacket();
maspadaru 10:b1bdc51e1c50 230 if (m_pixy->recvPacket() == 0) {
maspadaru 10:b1bdc51e1c50 231 if (m_pixy->m_type == LINE_RESPONSE_GET_FEATURES) {
maspadaru 10:b1bdc51e1c50 232 // parse line response
maspadaru 10:b1bdc51e1c50 233 for (offset = 0, res = 0; m_pixy->m_length > offset; offset += fsize + 2) {
maspadaru 10:b1bdc51e1c50 234 ftype = m_pixy->m_buf[offset];
maspadaru 10:b1bdc51e1c50 235 fsize = m_pixy->m_buf[offset + 1];
maspadaru 10:b1bdc51e1c50 236 fdata = &m_pixy->m_buf[offset + 2];
maspadaru 10:b1bdc51e1c50 237 if (ftype == LINE_VECTOR) {
maspadaru 10:b1bdc51e1c50 238 vectors = (Vector*)fdata;
maspadaru 10:b1bdc51e1c50 239 numVectors = fsize / sizeof(Vector);
maspadaru 10:b1bdc51e1c50 240 res |= LINE_VECTOR;
maspadaru 10:b1bdc51e1c50 241 } else if (ftype == LINE_INTERSECTION) {
maspadaru 10:b1bdc51e1c50 242 intersections = (Intersection*)fdata;
maspadaru 10:b1bdc51e1c50 243 numIntersections = fsize / sizeof(Intersection);
maspadaru 10:b1bdc51e1c50 244 res |= LINE_INTERSECTION;
maspadaru 10:b1bdc51e1c50 245 } else if (ftype == LINE_BARCODE) {
maspadaru 10:b1bdc51e1c50 246 barcodes = (Barcode*)fdata;
maspadaru 10:b1bdc51e1c50 247 numBarcodes = fsize / sizeof(Barcode);
maspadaru 10:b1bdc51e1c50 248 ;
maspadaru 10:b1bdc51e1c50 249 res |= LINE_BARCODE;
maspadaru 10:b1bdc51e1c50 250 } else
maspadaru 10:b1bdc51e1c50 251 break; // parse error
maspadaru 10:b1bdc51e1c50 252 }
maspadaru 10:b1bdc51e1c50 253 return res;
maspadaru 10:b1bdc51e1c50 254 } else if (m_pixy->m_type == PIXY_TYPE_RESPONSE_ERROR) {
maspadaru 10:b1bdc51e1c50 255 // if it's not a busy response, return the error
maspadaru 10:b1bdc51e1c50 256 if ((int8_t)m_pixy->m_buf[0] != PIXY_RESULT_BUSY)
maspadaru 10:b1bdc51e1c50 257 return m_pixy->m_buf[0];
maspadaru 10:b1bdc51e1c50 258 else if (!wait) // we're busy
maspadaru 10:b1bdc51e1c50 259 return PIXY_RESULT_BUSY; // new data not available yet
maspadaru 10:b1bdc51e1c50 260 }
maspadaru 10:b1bdc51e1c50 261 } else
maspadaru 10:b1bdc51e1c50 262 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 263
maspadaru 10:b1bdc51e1c50 264 // If we're waiting for frame data, don't thrash Pixy with requests.
maspadaru 10:b1bdc51e1c50 265 // We can give up half a millisecond of latency (worst case)
maspadaru 10:b1bdc51e1c50 266 wait_ms(500);
maspadaru 10:b1bdc51e1c50 267 }
maspadaru 10:b1bdc51e1c50 268 }
maspadaru 10:b1bdc51e1c50 269
maspadaru 10:b1bdc51e1c50 270 template <class LinkType>
maspadaru 10:b1bdc51e1c50 271 int8_t Pixy2Line<LinkType>::setMode(uint8_t mode)
maspadaru 10:b1bdc51e1c50 272 {
maspadaru 10:b1bdc51e1c50 273 uint32_t res;
maspadaru 10:b1bdc51e1c50 274
maspadaru 10:b1bdc51e1c50 275 *(int8_t*)m_pixy->m_bufPayload = mode;
maspadaru 10:b1bdc51e1c50 276 m_pixy->m_length = 1;
maspadaru 10:b1bdc51e1c50 277 m_pixy->m_type = LINE_REQUEST_SET_MODE;
maspadaru 10:b1bdc51e1c50 278 m_pixy->sendPacket();
maspadaru 10:b1bdc51e1c50 279 if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
maspadaru 10:b1bdc51e1c50 280 res = *(uint32_t*)m_pixy->m_buf;
maspadaru 10:b1bdc51e1c50 281 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 282 } else
maspadaru 10:b1bdc51e1c50 283 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 284 }
maspadaru 10:b1bdc51e1c50 285
maspadaru 10:b1bdc51e1c50 286 template <class LinkType>
maspadaru 10:b1bdc51e1c50 287 int8_t Pixy2Line<LinkType>::setNextTurn(int16_t angle)
maspadaru 10:b1bdc51e1c50 288 {
maspadaru 10:b1bdc51e1c50 289 uint32_t res;
maspadaru 10:b1bdc51e1c50 290
maspadaru 10:b1bdc51e1c50 291 *(int16_t*)m_pixy->m_bufPayload = angle;
maspadaru 10:b1bdc51e1c50 292 m_pixy->m_length = 2;
maspadaru 10:b1bdc51e1c50 293 m_pixy->m_type = LINE_REQUEST_SET_NEXT_TURN_ANGLE;
maspadaru 10:b1bdc51e1c50 294 m_pixy->sendPacket();
maspadaru 10:b1bdc51e1c50 295 if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
maspadaru 10:b1bdc51e1c50 296 res = *(uint32_t*)m_pixy->m_buf;
maspadaru 10:b1bdc51e1c50 297 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 298 } else
maspadaru 10:b1bdc51e1c50 299 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 300 }
maspadaru 10:b1bdc51e1c50 301
maspadaru 10:b1bdc51e1c50 302 template <class LinkType>
maspadaru 10:b1bdc51e1c50 303 int8_t Pixy2Line<LinkType>::setDefaultTurn(int16_t angle)
maspadaru 10:b1bdc51e1c50 304 {
maspadaru 10:b1bdc51e1c50 305 uint32_t res;
maspadaru 10:b1bdc51e1c50 306
maspadaru 10:b1bdc51e1c50 307 *(int16_t*)m_pixy->m_bufPayload = angle;
maspadaru 10:b1bdc51e1c50 308 m_pixy->m_length = 2;
maspadaru 10:b1bdc51e1c50 309 m_pixy->m_type = LINE_REQUEST_SET_DEFAULT_TURN_ANGLE;
maspadaru 10:b1bdc51e1c50 310 m_pixy->sendPacket();
maspadaru 10:b1bdc51e1c50 311 if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
maspadaru 10:b1bdc51e1c50 312 res = *(uint32_t*)m_pixy->m_buf;
maspadaru 10:b1bdc51e1c50 313 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 314 } else
maspadaru 10:b1bdc51e1c50 315 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 316 }
maspadaru 10:b1bdc51e1c50 317
maspadaru 10:b1bdc51e1c50 318 template <class LinkType>
maspadaru 10:b1bdc51e1c50 319 int8_t Pixy2Line<LinkType>::setVector(uint8_t index)
maspadaru 10:b1bdc51e1c50 320 {
maspadaru 10:b1bdc51e1c50 321 uint32_t res;
maspadaru 10:b1bdc51e1c50 322
maspadaru 10:b1bdc51e1c50 323 *(int8_t*)m_pixy->m_bufPayload = index;
maspadaru 10:b1bdc51e1c50 324 m_pixy->m_length = 1;
maspadaru 10:b1bdc51e1c50 325 m_pixy->m_type = LINE_REQUEST_SET_VECTOR;
maspadaru 10:b1bdc51e1c50 326 m_pixy->sendPacket();
maspadaru 10:b1bdc51e1c50 327 if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
maspadaru 10:b1bdc51e1c50 328 res = *(uint32_t*)m_pixy->m_buf;
maspadaru 10:b1bdc51e1c50 329 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 330 } else
maspadaru 10:b1bdc51e1c50 331 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 332 }
maspadaru 10:b1bdc51e1c50 333
maspadaru 10:b1bdc51e1c50 334 template <class LinkType>
maspadaru 10:b1bdc51e1c50 335 int8_t Pixy2Line<LinkType>::reverseVector()
maspadaru 10:b1bdc51e1c50 336 {
maspadaru 10:b1bdc51e1c50 337 uint32_t res;
maspadaru 10:b1bdc51e1c50 338
maspadaru 10:b1bdc51e1c50 339 m_pixy->m_length = 0;
maspadaru 10:b1bdc51e1c50 340 m_pixy->m_type = LINE_REQUEST_REVERSE_VECTOR;
maspadaru 10:b1bdc51e1c50 341 m_pixy->sendPacket();
maspadaru 10:b1bdc51e1c50 342 if (m_pixy->recvPacket() == 0 && m_pixy->m_type == PIXY_TYPE_RESPONSE_RESULT && m_pixy->m_length == 4) {
maspadaru 10:b1bdc51e1c50 343 res = *(uint32_t*)m_pixy->m_buf;
maspadaru 10:b1bdc51e1c50 344 return (int8_t)res;
maspadaru 10:b1bdc51e1c50 345 } else
maspadaru 10:b1bdc51e1c50 346 return PIXY_RESULT_ERROR; // some kind of bitstream error
maspadaru 10:b1bdc51e1c50 347 }
maspadaru 10:b1bdc51e1c50 348
maspadaru 10:b1bdc51e1c50 349 #endif
maspadaru 10:b1bdc51e1c50 350