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@11:f5dcf8811a4e, 2014-03-12 (annotated)

Committer:
d3alek
Date:
Wed Mar 12 10:35:47 2014 +0000
Revision:
11:f5dcf8811a4e
Parent:
10:37e7c46837dc
Child:
12:c9d0b1ff36f2
two way communication;

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