Aleksandar Kodzhabashev / Mbed 2 deprecated TrackballQuery

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.

Dependencies:   Servo mbed

main.cpp@10:37e7c46837dc, 2014-03-10 (annotated)

Committer:
d3alek
Date:
Mon Mar 10 20:43:52 2014 +0000
Revision:
10:37e7c46837dc
Parent:
9:2c85d7f99a14
Child:
11:f5dcf8811a4e
Servo movement completed notification working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
d3alek 0:94cffad90b69 1 #include "PS2MS.h"
d3alek 0:94cffad90b69 2 #include "PS2MS_INIT.h"
d3alek 0:94cffad90b69 3 #include "mbed.h"
d3alek 5:43d5529fbe1e 4 #include "Servo.h"
d3alek 0:94cffad90b69 5
d3alek 6:83c4801a027d 6 #define SENSORS_NUM 3
d3alek 8:41b35bda9d48 7 #define BYTES_NUM 3
d3alek 6:83c4801a027d 8
d3alek 9:2c85d7f99a14 9 #define DEBUG false
d3alek 9:2c85d7f99a14 10
d3alek 9:2c85d7f99a14 11 #define MAX_OVERFLOWS 3
d3alek 9:2c85d7f99a14 12
d3alek 9:2c85d7f99a14 13 #define MAX_REPLY_ERRORS 3
d3alek 9:2c85d7f99a14 14
d3alek 10:37e7c46837dc 15 Servo servoYaw(p21);
d3alek 10:37e7c46837dc 16 Servo servoPitch(p24);
d3alek 5:43d5529fbe1e 17
d3alek 0:94cffad90b69 18 DigitalOut myled(LED1);
d3alek 0:94cffad90b69 19 Serial pc(USBTX, USBRX); // tx, rx
d3alek 2:e35627187804 20 /*
d3alek 2:e35627187804 21 * 0xFF: Reset command.
d3alek 2:e35627187804 22 * 0xF3: Set sample rate.
d3alek 2:e35627187804 23 * 0xF2: Read device type.
d3alek 2:e35627187804 24 * 0xE8: Set resolution.
d3alek 2:e35627187804 25 * 0xE6: Set scaling.
d3alek 2:e35627187804 26 * 0xF4: Enable device.
d3alek 2:e35627187804 27 */
d3alek 0:94cffad90b69 28
d3alek 6:83c4801a027d 29 //TODO should Iuse sensor1_init? maybe no 255s?
d3alek 10:37e7c46837dc 30 PS2MS* sensor[3];
d3alek 10:37e7c46837dc 31 PS2MS_INIT* sensor_init[3];
d3alek 0:94cffad90b69 32
d3alek 2:e35627187804 33
d3alek 8:41b35bda9d48 34 int process_sensor_input(int c, int bytenum, char* bytes, int ind);
d3alek 10:37e7c46837dc 35 bool processACKReply(int ind);
d3alek 9:2c85d7f99a14 36 void sendError(int ind);
d3alek 10:37e7c46837dc 37 bool getPacket(int ind);
d3alek 10:37e7c46837dc 38 bool getMovementPacket(int ind);
d3alek 10:37e7c46837dc 39 void sendResend(int ind);
d3alek 10:37e7c46837dc 40 void sendError(int ind);
d3alek 10:37e7c46837dc 41 int sendCommand(int ind, char command);
d3alek 10:37e7c46837dc 42 void processSerial();
d3alek 2:e35627187804 43
d3alek 6:83c4801a027d 44 int sensorXs[SENSORS_NUM];
d3alek 6:83c4801a027d 45 int sensorYs[SENSORS_NUM];
d3alek 6:83c4801a027d 46 bool sensorToPrint[SENSORS_NUM];
d3alek 0:94cffad90b69 47
d3alek 10:37e7c46837dc 48 char last_command[3];
d3alek 10:37e7c46837dc 49
d3alek 9:2c85d7f99a14 50 bool expectingAck1 = false, expectingAck2 = false, expectingAck3 = false;
d3alek 9:2c85d7f99a14 51
d3alek 9:2c85d7f99a14 52 int replyErrors[3];
d3alek 9:2c85d7f99a14 53
d3alek 10:37e7c46837dc 54 static const int SERVO_YAW = 0;
d3alek 10:37e7c46837dc 55 static const int SERVO_PITCH = 1;
d3alek 10:37e7c46837dc 56 float servoPos[2];
d3alek 10:37e7c46837dc 57 float servoAdj[2] = {0.03, 0.03};
d3alek 10:37e7c46837dc 58 bool mCommandCompleted[2] = {true, true};
d3alek 10:37e7c46837dc 59
d3alek 2:e35627187804 60 int main()
d3alek 2:e35627187804 61 {
d3alek 10:37e7c46837dc 62 sensor_init[0] = new PS2MS_INIT(p18, p17);
d3alek 10:37e7c46837dc 63 sensor_init[1] = new PS2MS_INIT(p23, p22);
d3alek 10:37e7c46837dc 64 sensor_init[2] = new PS2MS_INIT(p26, p25);
d3alek 10:37e7c46837dc 65 sensor[0] = new PS2MS(p18, p17);
d3alek 10:37e7c46837dc 66 sensor[1] = new PS2MS(p23, p22);
d3alek 10:37e7c46837dc 67 sensor[2] = new PS2MS(p26, p25);
d3alek 10:37e7c46837dc 68
d3alek 10:37e7c46837dc 69 //TODO: receive all pending packets here
d3alek 5:43d5529fbe1e 70
d3alek 5:43d5529fbe1e 71 float range = 0.00085;
d3alek 6:83c4801a027d 72 float position1 = 0.5;
d3alek 6:83c4801a027d 73 float position2 = 0.5;
d3alek 10:37e7c46837dc 74
d3alek 10:37e7c46837dc 75 servoPos[SERVO_YAW] = position1 + servoAdj[SERVO_YAW];
d3alek 10:37e7c46837dc 76 servoPos[SERVO_PITCH] = position2 + servoAdj[SERVO_PITCH];
d3alek 10:37e7c46837dc 77
d3alek 9:2c85d7f99a14 78 replyErrors[0] = replyErrors[1] = replyErrors[2] = 0;
d3alek 10:37e7c46837dc 79
d3alek 10:37e7c46837dc 80 servoYaw.calibrate(range, 45.0);
d3alek 10:37e7c46837dc 81 servoPitch.calibrate(range, 45.0);
d3alek 10:37e7c46837dc 82 servoYaw = servoPos[SERVO_YAW];
d3alek 10:37e7c46837dc 83 servoPitch = servoPos[SERVO_PITCH];
d3alek 10:37e7c46837dc 84 printf("position = %.3f, range = +/-%0.5f\n", position1, range);
d3alek 5:43d5529fbe1e 85
d3alek 4:73f9fd797965 86 printf("IMHERE START\n");
d3alek 2:e35627187804 87
d3alek 6:83c4801a027d 88 sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
d3alek 7:04ddad10a741 89 int dir;
d3alek 9:2c85d7f99a14 90 float servoSpeed = 0.002;
d3alek 10:37e7c46837dc 91
d3alek 9:2c85d7f99a14 92 bool awaitingPackets = false;
d3alek 10:37e7c46837dc 93
d3alek 0:94cffad90b69 94 while(1) {
d3alek 5:43d5529fbe1e 95 if (pc.readable()) {
d3alek 10:37e7c46837dc 96 processSerial();
d3alek 10:37e7c46837dc 97 }
d3alek 10:37e7c46837dc 98
d3alek 10:37e7c46837dc 99 if (abs(position1 - servoPos[SERVO_YAW]) > servoSpeed) {
d3alek 10:37e7c46837dc 100 dir = position1 < servoPos[SERVO_YAW] ? 1 : -1;
d3alek 10:37e7c46837dc 101 position1 += servoSpeed * dir;
d3alek 10:37e7c46837dc 102 servoYaw = position1;
d3alek 10:37e7c46837dc 103 } else {
d3alek 10:37e7c46837dc 104 if (mCommandCompleted[SERVO_YAW] == false) {
d3alek 10:37e7c46837dc 105 printf("Command completed\n\r");
d3alek 10:37e7c46837dc 106 mCommandCompleted[SERVO_YAW] = true;
d3alek 5:43d5529fbe1e 107 }
d3alek 10:37e7c46837dc 108 position1 = servoPos[SERVO_YAW];
d3alek 10:37e7c46837dc 109 }
d3alek 10:37e7c46837dc 110
d3alek 10:37e7c46837dc 111 if (abs(position2 - servoPos[SERVO_PITCH]) > servoSpeed) {
d3alek 10:37e7c46837dc 112 dir = position2 < servoPos[SERVO_PITCH] ? 1 : -1;
d3alek 10:37e7c46837dc 113 position2 += servoSpeed * dir;
d3alek 10:37e7c46837dc 114 servoPitch = position2;
d3alek 10:37e7c46837dc 115 } else {
d3alek 10:37e7c46837dc 116 if (mCommandCompleted[SERVO_PITCH] == false) {
d3alek 10:37e7c46837dc 117 printf("Command completed\n\r");
d3alek 10:37e7c46837dc 118 mCommandCompleted[SERVO_PITCH] = true;
d3alek 7:04ddad10a741 119 }
d3alek 10:37e7c46837dc 120 position2 = servoPos[SERVO_PITCH];
d3alek 7:04ddad10a741 121 }
d3alek 10:37e7c46837dc 122 int res;
d3alek 9:2c85d7f99a14 123 if (!awaitingPackets) {
d3alek 10:37e7c46837dc 124 //TODO: check for errors on send
d3alek 10:37e7c46837dc 125 res = sendCommand(0, '\xEB');
d3alek 10:37e7c46837dc 126
d3alek 10:37e7c46837dc 127 if (res) {
d3alek 10:37e7c46837dc 128 if (DEBUG) {
d3alek 10:37e7c46837dc 129 printf("%d: send error %d\n\r", 0, res);
d3alek 10:37e7c46837dc 130 }
d3alek 10:37e7c46837dc 131 res = sendCommand(0, '\xEB');
d3alek 10:37e7c46837dc 132 if (DEBUG) {
d3alek 10:37e7c46837dc 133 printf("%d: two failed sends %d\n\r", 0, res);
d3alek 10:37e7c46837dc 134 }
d3alek 10:37e7c46837dc 135 }
d3alek 9:2c85d7f99a14 136 expectingAck1 = true;
d3alek 10:37e7c46837dc 137 res = sendCommand(1, '\xEB');
d3alek 10:37e7c46837dc 138 if (res) {
d3alek 10:37e7c46837dc 139 if (DEBUG) {
d3alek 10:37e7c46837dc 140 printf("%d: send error %d\n\r", 1, res);
d3alek 10:37e7c46837dc 141 }
d3alek 10:37e7c46837dc 142 res = sendCommand(1, '\xEB');
d3alek 10:37e7c46837dc 143 if (DEBUG) {
d3alek 10:37e7c46837dc 144 printf("%d: two failed sends %d\n\r", 1, res);
d3alek 10:37e7c46837dc 145 }
d3alek 10:37e7c46837dc 146 }
d3alek 9:2c85d7f99a14 147 expectingAck2 = true;
d3alek 10:37e7c46837dc 148 res = sendCommand(2, '\xEB');
d3alek 10:37e7c46837dc 149 if (res) {
d3alek 10:37e7c46837dc 150 if (DEBUG) {
d3alek 10:37e7c46837dc 151 printf("%d: send error %d\n\r", 2, res);
d3alek 10:37e7c46837dc 152 }
d3alek 10:37e7c46837dc 153 res = sendCommand(2, '\xEB');
d3alek 10:37e7c46837dc 154 if (res) {
d3alek 10:37e7c46837dc 155 if (DEBUG) {
d3alek 10:37e7c46837dc 156 printf("%d: two failed sends %d\n\r", 2, res);
d3alek 10:37e7c46837dc 157 }
d3alek 10:37e7c46837dc 158 }
d3alek 10:37e7c46837dc 159 }
d3alek 9:2c85d7f99a14 160 expectingAck3 = true;
d3alek 9:2c85d7f99a14 161 awaitingPackets = true;
d3alek 9:2c85d7f99a14 162 }
d3alek 10:37e7c46837dc 163
d3alek 10:37e7c46837dc 164 if (expectingAck1) {
d3alek 10:37e7c46837dc 165 expectingAck1 = !getPacket(0);
d3alek 10:37e7c46837dc 166 } else if (expectingAck2) {
d3alek 10:37e7c46837dc 167 expectingAck2 = !getPacket(1);
d3alek 10:37e7c46837dc 168 } else if (expectingAck3) {
d3alek 10:37e7c46837dc 169 expectingAck3 = !getPacket(2);
d3alek 6:83c4801a027d 170 }
d3alek 2:e35627187804 171 // TODO only prints when both are enabled now
d3alek 6:83c4801a027d 172 if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
d3alek 10:37e7c46837dc 173 if ((sensorXs[0] | sensorYs[0] | sensorXs[1] | sensorYs[1] | sensorXs[2] | sensorYs[2]) ) {
d3alek 10:37e7c46837dc 174 // some of the velocities are not 0
d3alek 10:37e7c46837dc 175 printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
d3alek 10:37e7c46837dc 176 sensorXs[2], sensorYs[2]);
d3alek 10:37e7c46837dc 177 }
d3alek 6:83c4801a027d 178 sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
d3alek 6:83c4801a027d 179 sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
d3alek 9:2c85d7f99a14 180 awaitingPackets = false;
d3alek 9:2c85d7f99a14 181 }
d3alek 10:37e7c46837dc 182 }\
d3alek 10:37e7c46837dc 183 /*
d3alek 10:37e7c46837dc 184 TODO: Deallocate those somewhere
d3alek 10:37e7c46837dc 185
d3alek 10:37e7c46837dc 186 delete[] sensor_init[0];
d3alek 10:37e7c46837dc 187 delete[] sensor_init[1];
d3alek 10:37e7c46837dc 188 delete[] sensor_init[2];
d3alek 10:37e7c46837dc 189 delete[] sensor[0];
d3alek 10:37e7c46837dc 190 delete[] sensor[1];
d3alek 10:37e7c46837dc 191 delete[] sensor[2];
d3alek 10:37e7c46837dc 192 */
d3alek 10:37e7c46837dc 193 }
d3alek 10:37e7c46837dc 194
d3alek 10:37e7c46837dc 195 #define MAX_ANGLE_STR_SIZE 100
d3alek 10:37e7c46837dc 196 char rotateAngleStr[MAX_ANGLE_STR_SIZE];
d3alek 10:37e7c46837dc 197
d3alek 10:37e7c46837dc 198 void processSerial()
d3alek 10:37e7c46837dc 199 {
d3alek 10:37e7c46837dc 200 char c = pc.getc();
d3alek 10:37e7c46837dc 201 if (c <= '9' && c >= '0') {
d3alek 10:37e7c46837dc 202 int servoNum = int(c - '0');
d3alek 10:37e7c46837dc 203 if (servoNum == SERVO_YAW || servoNum == SERVO_PITCH) {
d3alek 10:37e7c46837dc 204 pc.gets(rotateAngleStr, MAX_ANGLE_STR_SIZE);
d3alek 10:37e7c46837dc 205
d3alek 10:37e7c46837dc 206 double rotateAngleNum = atoi(rotateAngleStr);
d3alek 10:37e7c46837dc 207
d3alek 10:37e7c46837dc 208 if (rotateAngleNum > 90 || rotateAngleNum < -90) {
d3alek 10:37e7c46837dc 209 return;
d3alek 10:37e7c46837dc 210 }
d3alek 10:37e7c46837dc 211 rotateAngleNum = 0.5 + rotateAngleNum / 90.;
d3alek 10:37e7c46837dc 212 servoPos[servoNum] = rotateAngleNum + servoAdj[servoNum];
d3alek 10:37e7c46837dc 213 mCommandCompleted[servoNum] = false;
d3alek 10:37e7c46837dc 214 }
d3alek 9:2c85d7f99a14 215 }
d3alek 9:2c85d7f99a14 216 }
d3alek 9:2c85d7f99a14 217
d3alek 10:37e7c46837dc 218 int sendCommand(int ind, char command)
d3alek 10:37e7c46837dc 219 {
d3alek 10:37e7c46837dc 220 int res;
d3alek 10:37e7c46837dc 221 last_command[ind] = command;
d3alek 10:37e7c46837dc 222 //res = sensor_init[ind]->send(command);
d3alek 10:37e7c46837dc 223 res = sensor[ind]->sendCommand(command);
d3alek 10:37e7c46837dc 224 //__enable_irq();
d3alek 10:37e7c46837dc 225 if (res) {
d3alek 10:37e7c46837dc 226 if (DEBUG) printf("error sending command %#x to %d\n\r", command, ind);
d3alek 10:37e7c46837dc 227 }
d3alek 10:37e7c46837dc 228 return res;
d3alek 10:37e7c46837dc 229 }
d3alek 10:37e7c46837dc 230
d3alek 10:37e7c46837dc 231 bool getPacket(int ind)
d3alek 10:37e7c46837dc 232 {
d3alek 10:37e7c46837dc 233 bool successful = processACKReply(ind);
d3alek 10:37e7c46837dc 234 if (!successful) {
d3alek 10:37e7c46837dc 235 sendResend(ind);
d3alek 10:37e7c46837dc 236 successful = processACKReply(ind);
d3alek 10:37e7c46837dc 237 if (!successful) {
d3alek 10:37e7c46837dc 238 printf("DUNNO WHAT TO DO ACK Reply %d\n\r", ind);
d3alek 10:37e7c46837dc 239 //wait(1);
d3alek 10:37e7c46837dc 240 sendCommand(ind, '\xEB');
d3alek 10:37e7c46837dc 241 return getPacket(ind);
d3alek 0:94cffad90b69 242 }
d3alek 0:94cffad90b69 243 }
d3alek 10:37e7c46837dc 244 successful = getMovementPacket(ind);
d3alek 10:37e7c46837dc 245
d3alek 10:37e7c46837dc 246 if (!successful) {
d3alek 10:37e7c46837dc 247 printf("DISCARDING PACKET %d\n\r", ind);
d3alek 10:37e7c46837dc 248 if (DEBUG) {
d3alek 10:37e7c46837dc 249 sendCommand(ind, '\xEB');
d3alek 10:37e7c46837dc 250 //wait(1);
d3alek 10:37e7c46837dc 251 }
d3alek 9:2c85d7f99a14 252 return false;
d3alek 9:2c85d7f99a14 253 }
d3alek 9:2c85d7f99a14 254 return true;
d3alek 0:94cffad90b69 255 }
d3alek 0:94cffad90b69 256
d3alek 10:37e7c46837dc 257 void sendError(int ind)
d3alek 10:37e7c46837dc 258 {
d3alek 10:37e7c46837dc 259 sendCommand(ind, '\xFC');
d3alek 10:37e7c46837dc 260 }
d3alek 10:37e7c46837dc 261
d3alek 10:37e7c46837dc 262 void sendResend(int ind)
d3alek 10:37e7c46837dc 263 {
d3alek 10:37e7c46837dc 264 sendCommand(ind, '\xFE');
d3alek 10:37e7c46837dc 265 }
d3alek 10:37e7c46837dc 266
d3alek 10:37e7c46837dc 267 bool getMovementPacket(int ind)
d3alek 10:37e7c46837dc 268 {
d3alek 10:37e7c46837dc 269 char bytes[3];
d3alek 10:37e7c46837dc 270 int c;
d3alek 10:37e7c46837dc 271 for (int i = 0; i < 3; ++i) {
d3alek 10:37e7c46837dc 272 c = sensor[ind]->getc();
d3alek 10:37e7c46837dc 273 if (c < 0) {
d3alek 10:37e7c46837dc 274 //printf("%d: 255\n\r", ind);
d3alek 10:37e7c46837dc 275 return false;
d3alek 10:37e7c46837dc 276 } else if (i == 0) {
d3alek 10:37e7c46837dc 277 bytes[0] = c;
d3alek 10:37e7c46837dc 278 if (!((c << 5) & 0x100)) {
d3alek 10:37e7c46837dc 279 // not byte[0] wrong offset, skip e
d3alek 10:37e7c46837dc 280 if (DEBUG) printf("%d: w %d ", ind, c);
d3alek 10:37e7c46837dc 281 c = sensor[ind]->getc();
d3alek 10:37e7c46837dc 282 while (c >= 0) {
d3alek 10:37e7c46837dc 283 printf("%d ", c);
d3alek 10:37e7c46837dc 284 c = sensor[ind]->getc();
d3alek 10:37e7c46837dc 285 }
d3alek 10:37e7c46837dc 286 printf("\n\r");
d3alek 10:37e7c46837dc 287 return false;
d3alek 10:37e7c46837dc 288 }
d3alek 10:37e7c46837dc 289 } else if (i == 1) {
d3alek 10:37e7c46837dc 290 bytes[1] = c;
d3alek 10:37e7c46837dc 291 } else if (i == 2) {
d3alek 10:37e7c46837dc 292 bytes[2] = c;
d3alek 10:37e7c46837dc 293 //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 10:37e7c46837dc 294
d3alek 10:37e7c46837dc 295 //TODO: check for overflow
d3alek 10:37e7c46837dc 296 if ((1 << 6) & bytes[0]) {
d3alek 10:37e7c46837dc 297 //printf("%d: Overflow x %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 10:37e7c46837dc 298 return false;
d3alek 10:37e7c46837dc 299 } else if ((1 << 7) & bytes[0]) {
d3alek 10:37e7c46837dc 300 printf("%d: Overflow y %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 10:37e7c46837dc 301 return false;
d3alek 10:37e7c46837dc 302 }
d3alek 10:37e7c46837dc 303 // check x and y signs
d3alek 10:37e7c46837dc 304 else {
d3alek 10:37e7c46837dc 305 int x = bytes[1] - ((bytes[0] << 4) & 0x100);
d3alek 10:37e7c46837dc 306 int y = bytes[2] - ((bytes[0] << 3) & 0x100);
d3alek 10:37e7c46837dc 307 //printf("%s: x = %d y = %d\n\r", id, x, y);
d3alek 10:37e7c46837dc 308 sensorXs[ind] = x;
d3alek 10:37e7c46837dc 309 sensorYs[ind] = y;
d3alek 10:37e7c46837dc 310 sensorToPrint[ind] = true;
d3alek 10:37e7c46837dc 311 //printf("%d ", ind);
d3alek 10:37e7c46837dc 312 }
d3alek 10:37e7c46837dc 313 }
d3alek 10:37e7c46837dc 314 }
d3alek 10:37e7c46837dc 315 return true;
d3alek 10:37e7c46837dc 316 }
d3alek 10:37e7c46837dc 317
d3alek 10:37e7c46837dc 318 bool processACKReply(int ind)
d3alek 10:37e7c46837dc 319 {
d3alek 10:37e7c46837dc 320 int reply = sensor[ind]->getc();
d3alek 10:37e7c46837dc 321 if (reply < 0) {
d3alek 10:37e7c46837dc 322 if (DEBUG) printf("%d: Error %d", ind, reply);
d3alek 10:37e7c46837dc 323 return false;
d3alek 10:37e7c46837dc 324 } else if (reply == '\xFA') {
d3alek 10:37e7c46837dc 325 //if (DEBUG) printf("%d: ACK ", ind);
d3alek 10:37e7c46837dc 326 return true;
d3alek 10:37e7c46837dc 327 } else if (reply == '\xEB') {
d3alek 10:37e7c46837dc 328 if (DEBUG) printf("%d: READ_DATA ", ind);
d3alek 10:37e7c46837dc 329 //TODO: inf loop possible here cause recursion
d3alek 10:37e7c46837dc 330 return processACKReply(ind);
d3alek 10:37e7c46837dc 331 } else if (reply == '\xFE') {
d3alek 10:37e7c46837dc 332 if (last_command[ind] == '\xFE') {
d3alek 10:37e7c46837dc 333 if (DEBUG) printf("%d: REPEATING_COMMAND BUT REPEAT_COMMAND!", ind, last_command[ind]);
d3alek 10:37e7c46837dc 334 //wait(1);
d3alek 10:37e7c46837dc 335 sendCommand(ind, '\xEB');
d3alek 10:37e7c46837dc 336 return processACKReply(ind);
d3alek 10:37e7c46837dc 337 }
d3alek 10:37e7c46837dc 338 // repeat command
d3alek 10:37e7c46837dc 339 if (DEBUG) printf("%d: REPEATING_COMMAND %#x", ind, last_command[ind]);
d3alek 10:37e7c46837dc 340 //wait(1);
d3alek 10:37e7c46837dc 341 sendCommand(ind, last_command[ind]);
d3alek 10:37e7c46837dc 342 return processACKReply(ind);
d3alek 10:37e7c46837dc 343 } else if (reply == '\xF2') {
d3alek 10:37e7c46837dc 344 // get device ID??
d3alek 10:37e7c46837dc 345 if (DEBUG) printf("%d: GET_DEVICE_ID %#x", ind, reply);
d3alek 10:37e7c46837dc 346 //wait(1);
d3alek 10:37e7c46837dc 347 return processACKReply(ind);
d3alek 10:37e7c46837dc 348 } else {
d3alek 10:37e7c46837dc 349 if (DEBUG) printf("%d: Unexpected ACK Reply %d\n\r", ind, reply);
d3alek 10:37e7c46837dc 350 //wait(1);
d3alek 10:37e7c46837dc 351 return processACKReply(ind);
d3alek 10:37e7c46837dc 352 }
d3alek 10:37e7c46837dc 353 }
d3alek 10:37e7c46837dc 354 /*
d3alek 8:41b35bda9d48 355 int process_sensor_input(int c, int bytenum, char* bytes, int ind)
d3alek 2:e35627187804 356 {
d3alek 8:41b35bda9d48 357 if (c < 0) {
d3alek 8:41b35bda9d48 358 //printf("%d: 255\n\r", ind);
d3alek 2:e35627187804 359 bytenum = -1;
d3alek 8:41b35bda9d48 360 } else if (bytenum % BYTES_NUM == 0) {
d3alek 2:e35627187804 361 bytes[0] = c;
d3alek 8:41b35bda9d48 362 if (!((c << 5) & 0x100)) {
d3alek 2:e35627187804 363 // not byte[0] wrong offset, skip c
d3alek 9:2c85d7f99a14 364 if (DEBUG) printf("%d: w %d\n\r", ind, c);
d3alek 2:e35627187804 365 bytenum = -1;
d3alek 9:2c85d7f99a14 366 sendError(ind);
d3alek 0:94cffad90b69 367 }
d3alek 8:41b35bda9d48 368 } else if (bytenum % BYTES_NUM == 1) {
d3alek 2:e35627187804 369 bytes[1] = c;
d3alek 8:41b35bda9d48 370 } else if (bytenum % BYTES_NUM == 2) {
d3alek 2:e35627187804 371 bytes[2] = c;
d3alek 8:41b35bda9d48 372 //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 0:94cffad90b69 373
d3alek 2:e35627187804 374 //TODO: check for overflow
d3alek 2:e35627187804 375 if ((1 << 6) & bytes[0]) {
d3alek 9:2c85d7f99a14 376 printf("%d: Overflow x %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
d3alek 9:2c85d7f99a14 377 if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
d3alek 9:2c85d7f99a14 378 sendError(ind);
d3alek 10:37e7c46837dc 379 } else {
d3alek 9:2c85d7f99a14 380 consecutiveOverflows[ind] = 0;
d3alek 9:2c85d7f99a14 381 }
d3alek 3:eb3c3c9587d7 382 bytenum = -1;
d3alek 10:37e7c46837dc 383 } else if ((1 << 7) & bytes[0]) {
d3alek 9:2c85d7f99a14 384 printf("%d: Overflow y %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
d3alek 9:2c85d7f99a14 385 if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
d3alek 9:2c85d7f99a14 386 sendError(ind);
d3alek 10:37e7c46837dc 387 } else {
d3alek 9:2c85d7f99a14 388 consecutiveOverflows[ind] = 0;
d3alek 9:2c85d7f99a14 389 }
d3alek 9:2c85d7f99a14 390 bytenum = -1;
d3alek 0:94cffad90b69 391 }
d3alek 2:e35627187804 392 // check x and y signs
d3alek 3:eb3c3c9587d7 393 else {
d3alek 3:eb3c3c9587d7 394 int x = bytes[1] - ((bytes[0] << 4) & 0x100);
d3alek 3:eb3c3c9587d7 395 int y = bytes[2] - ((bytes[0] << 3) & 0x100);
d3alek 3:eb3c3c9587d7 396 //printf("%s: x = %d y = %d\n\r", id, x, y);
d3alek 3:eb3c3c9587d7 397 sensorXs[ind] = x;
d3alek 3:eb3c3c9587d7 398 sensorYs[ind] = y;
d3alek 3:eb3c3c9587d7 399 sensorToPrint[ind] = true;
d3alek 8:41b35bda9d48 400 //printf("%d ", ind);
d3alek 3:eb3c3c9587d7 401 bytenum = -1;
d3alek 3:eb3c3c9587d7 402 }
d3alek 0:94cffad90b69 403 }
d3alek 8:41b35bda9d48 404 return (bytenum + 1) % BYTES_NUM;
d3alek 0:94cffad90b69 405 }
d3alek 10:37e7c46837dc 406 */
d3alek 10:37e7c46837dc 407 /*
d3alek 9:2c85d7f99a14 408 void sendError(int ind)
d3alek 9:2c85d7f99a14 409 {
d3alek 9:2c85d7f99a14 410 switch (ind) {
d3alek 9:2c85d7f99a14 411 case 0:
d3alek 9:2c85d7f99a14 412 sensor1_init.send('\xFE');
d3alek 9:2c85d7f99a14 413 expectingAck1 = true;
d3alek 9:2c85d7f99a14 414 break;
d3alek 9:2c85d7f99a14 415 case 1:
d3alek 9:2c85d7f99a14 416 sensor2_init.send('\xFE');
d3alek 9:2c85d7f99a14 417 expectingAck2 = true;
d3alek 9:2c85d7f99a14 418 break;
d3alek 9:2c85d7f99a14 419 case 2:
d3alek 9:2c85d7f99a14 420 sensor3_init.send('\xFE');
d3alek 9:2c85d7f99a14 421 expectingAck3 = true;
d3alek 9:2c85d7f99a14 422 break;
d3alek 9:2c85d7f99a14 423 }
d3alek 10:37e7c46837dc 424 }*/