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

Committer:
d3alek
Date:
Thu Mar 13 12:07:48 2014 +0000
Revision:
12:c9d0b1ff36f2
Parent:
11:f5dcf8811a4e
Child:
13:582064cfb9b2
timed execution

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 Serial pc(USBTX, USBRX); // tx, rx
d3alek 2:e35627187804 19 /*
d3alek 2:e35627187804 20 * 0xFF: Reset command.
d3alek 2:e35627187804 21 * 0xF3: Set sample rate.
d3alek 2:e35627187804 22 * 0xF2: Read device type.
d3alek 2:e35627187804 23 * 0xE8: Set resolution.
d3alek 2:e35627187804 24 * 0xE6: Set scaling.
d3alek 2:e35627187804 25 * 0xF4: Enable device.
d3alek 2:e35627187804 26 */
d3alek 0:94cffad90b69 27
d3alek 6:83c4801a027d 28 //TODO should Iuse sensor1_init? maybe no 255s?
d3alek 10:37e7c46837dc 29 PS2MS* sensor[3];
d3alek 2:e35627187804 30
d3alek 8:41b35bda9d48 31 int process_sensor_input(int c, int bytenum, char* bytes, int ind);
d3alek 10:37e7c46837dc 32 bool processACKReply(int ind);
d3alek 9:2c85d7f99a14 33 void sendError(int ind);
d3alek 10:37e7c46837dc 34 bool getPacket(int ind);
d3alek 10:37e7c46837dc 35 bool getMovementPacket(int ind);
d3alek 10:37e7c46837dc 36 void sendResend(int ind);
d3alek 10:37e7c46837dc 37 void sendError(int ind);
d3alek 10:37e7c46837dc 38 int sendCommand(int ind, char command);
d3alek 10:37e7c46837dc 39 void processSerial();
d3alek 2:e35627187804 40
d3alek 6:83c4801a027d 41 int sensorXs[SENSORS_NUM];
d3alek 6:83c4801a027d 42 int sensorYs[SENSORS_NUM];
d3alek 6:83c4801a027d 43 bool sensorToPrint[SENSORS_NUM];
d3alek 0:94cffad90b69 44
d3alek 10:37e7c46837dc 45 char last_command[3];
d3alek 10:37e7c46837dc 46
d3alek 9:2c85d7f99a14 47 bool expectingAck1 = false, expectingAck2 = false, expectingAck3 = false;
d3alek 9:2c85d7f99a14 48
d3alek 9:2c85d7f99a14 49 int replyErrors[3];
d3alek 9:2c85d7f99a14 50
d3alek 10:37e7c46837dc 51 static const int SERVO_YAW = 0;
d3alek 10:37e7c46837dc 52 static const int SERVO_PITCH = 1;
d3alek 10:37e7c46837dc 53 float servoPos[2];
d3alek 10:37e7c46837dc 54 float servoAdj[2] = {0.03, 0.03};
d3alek 10:37e7c46837dc 55 bool mCommandCompleted[2] = {true, true};
d3alek 10:37e7c46837dc 56
d3alek 11:f5dcf8811a4e 57 const float SERVO_SPEED_ADJ = 0.001;
d3alek 11:f5dcf8811a4e 58
d3alek 11:f5dcf8811a4e 59 float servoSpeed = 1 * SERVO_SPEED_ADJ;
d3alek 11:f5dcf8811a4e 60
d3alek 12:c9d0b1ff36f2 61 Timer timer;
d3alek 12:c9d0b1ff36f2 62
d3alek 12:c9d0b1ff36f2 63 int mDiscardedCount[3] = {0, 0, 0};
d3alek 12:c9d0b1ff36f2 64 const int MAX_DISCARDED = 3;
d3alek 12:c9d0b1ff36f2 65
d3alek 12:c9d0b1ff36f2 66 PinName sensorPorts[3][2] = {
d3alek 12:c9d0b1ff36f2 67 {p18, p17}, // sensor 0
d3alek 12:c9d0b1ff36f2 68 {p15, p14}, // sensor 1
d3alek 12:c9d0b1ff36f2 69 {p26, p25} // sensor 2
d3alek 12:c9d0b1ff36f2 70 };
d3alek 12:c9d0b1ff36f2 71
d3alek 12:c9d0b1ff36f2 72 void initializeSensor(int ind) {
d3alek 12:c9d0b1ff36f2 73 PS2MS_INIT s1(sensorPorts[ind][0], sensorPorts[ind][1]);
d3alek 12:c9d0b1ff36f2 74 printf("SENSOR_INIT % DONE on ports %d and %d\n\r", ind, sensorPorts[ind][0], sensorPorts[ind][1]);
d3alek 12:c9d0b1ff36f2 75 sensor[ind] = new PS2MS(sensorPorts[ind][0], sensorPorts[ind][1]);
d3alek 12:c9d0b1ff36f2 76 }
d3alek 12:c9d0b1ff36f2 77
d3alek 12:c9d0b1ff36f2 78 bool notTooFarOff() {
d3alek 12:c9d0b1ff36f2 79 int maxAbs1 = abs(sensorXs[0]) > abs(sensorYs[0]) ? abs(sensorXs[0]) : abs(sensorYs[0]);
d3alek 12:c9d0b1ff36f2 80 int maxAbs2 = abs(sensorXs[1]) > abs(sensorYs[1]) ? abs(sensorXs[1]) : abs(sensorYs[1]);
d3alek 12:c9d0b1ff36f2 81 int maxAbs3 = abs(sensorXs[2]) > abs(sensorYs[2]) ? abs(sensorXs[2]) : abs(sensorYs[1]);
d3alek 12:c9d0b1ff36f2 82
d3alek 12:c9d0b1ff36f2 83 if (maxAbs1 == 0) maxAbs1 = 1;
d3alek 12:c9d0b1ff36f2 84 if (maxAbs2 == 0) maxAbs2 = 1;
d3alek 12:c9d0b1ff36f2 85 if (maxAbs3 == 0) maxAbs3 = 1;
d3alek 12:c9d0b1ff36f2 86
d3alek 12:c9d0b1ff36f2 87 if (maxAbs1 > maxAbs2 * 20) {
d3alek 12:c9d0b1ff36f2 88 return false;
d3alek 12:c9d0b1ff36f2 89 }
d3alek 12:c9d0b1ff36f2 90
d3alek 12:c9d0b1ff36f2 91 if (maxAbs2 > maxAbs3 * 20) {
d3alek 12:c9d0b1ff36f2 92 return false;
d3alek 12:c9d0b1ff36f2 93 }
d3alek 12:c9d0b1ff36f2 94
d3alek 12:c9d0b1ff36f2 95 if (maxAbs3 > maxAbs1 * 20) {
d3alek 12:c9d0b1ff36f2 96 return false;
d3alek 12:c9d0b1ff36f2 97 }
d3alek 12:c9d0b1ff36f2 98
d3alek 12:c9d0b1ff36f2 99 return true;
d3alek 12:c9d0b1ff36f2 100 }
d3alek 12:c9d0b1ff36f2 101
d3alek 12:c9d0b1ff36f2 102 //TODO switch to Tickers instead of checking these in a while loop, much cleaner
d3alek 12:c9d0b1ff36f2 103 long lastTickMs;
d3alek 12:c9d0b1ff36f2 104 long lastSendMs;
d3alek 12:c9d0b1ff36f2 105 const int TICK_EVERY_MS = 10;
d3alek 12:c9d0b1ff36f2 106 const int SEND_EVERY_MS = 30;
d3alek 12:c9d0b1ff36f2 107
d3alek 2:e35627187804 108 int main()
d3alek 2:e35627187804 109 {
d3alek 11:f5dcf8811a4e 110 printf("MAIN START\n\r");
d3alek 12:c9d0b1ff36f2 111 initializeSensor(0);
d3alek 12:c9d0b1ff36f2 112 initializeSensor(1);
d3alek 12:c9d0b1ff36f2 113 initializeSensor(2);
d3alek 10:37e7c46837dc 114 //TODO: receive all pending packets here
d3alek 5:43d5529fbe1e 115
d3alek 12:c9d0b1ff36f2 116 timer.start();
d3alek 12:c9d0b1ff36f2 117
d3alek 5:43d5529fbe1e 118 float range = 0.00085;
d3alek 6:83c4801a027d 119 float position1 = 0.5;
d3alek 6:83c4801a027d 120 float position2 = 0.5;
d3alek 10:37e7c46837dc 121
d3alek 10:37e7c46837dc 122 servoPos[SERVO_YAW] = position1 + servoAdj[SERVO_YAW];
d3alek 10:37e7c46837dc 123 servoPos[SERVO_PITCH] = position2 + servoAdj[SERVO_PITCH];
d3alek 10:37e7c46837dc 124
d3alek 9:2c85d7f99a14 125 replyErrors[0] = replyErrors[1] = replyErrors[2] = 0;
d3alek 10:37e7c46837dc 126
d3alek 10:37e7c46837dc 127 servoYaw.calibrate(range, 45.0);
d3alek 10:37e7c46837dc 128 servoPitch.calibrate(range, 45.0);
d3alek 10:37e7c46837dc 129 servoYaw = servoPos[SERVO_YAW];
d3alek 10:37e7c46837dc 130 servoPitch = servoPos[SERVO_PITCH];
d3alek 2:e35627187804 131
d3alek 6:83c4801a027d 132 sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
d3alek 7:04ddad10a741 133 int dir;
d3alek 10:37e7c46837dc 134
d3alek 9:2c85d7f99a14 135 bool awaitingPackets = false;
d3alek 12:c9d0b1ff36f2 136
d3alek 12:c9d0b1ff36f2 137 lastTickMs = 0;
d3alek 12:c9d0b1ff36f2 138 lastSendMs = 0;
d3alek 12:c9d0b1ff36f2 139
d3alek 12:c9d0b1ff36f2 140 int mTooFarOffFor = 0;
d3alek 12:c9d0b1ff36f2 141 const int MAX_TOO_FAR_OFF_COUNT = 2;
d3alek 12:c9d0b1ff36f2 142
d3alek 0:94cffad90b69 143 while(1) {
d3alek 5:43d5529fbe1e 144 if (pc.readable()) {
d3alek 10:37e7c46837dc 145 processSerial();
d3alek 10:37e7c46837dc 146 }
d3alek 10:37e7c46837dc 147
d3alek 10:37e7c46837dc 148 if (abs(position1 - servoPos[SERVO_YAW]) > servoSpeed) {
d3alek 10:37e7c46837dc 149 dir = position1 < servoPos[SERVO_YAW] ? 1 : -1;
d3alek 10:37e7c46837dc 150 position1 += servoSpeed * dir;
d3alek 10:37e7c46837dc 151 servoYaw = position1;
d3alek 10:37e7c46837dc 152 } else {
d3alek 10:37e7c46837dc 153 if (mCommandCompleted[SERVO_YAW] == false) {
d3alek 12:c9d0b1ff36f2 154 printf("Command completed %d\n\r", timer.read_ms());
d3alek 10:37e7c46837dc 155 mCommandCompleted[SERVO_YAW] = true;
d3alek 5:43d5529fbe1e 156 }
d3alek 10:37e7c46837dc 157 position1 = servoPos[SERVO_YAW];
d3alek 10:37e7c46837dc 158 }
d3alek 10:37e7c46837dc 159
d3alek 10:37e7c46837dc 160 if (abs(position2 - servoPos[SERVO_PITCH]) > servoSpeed) {
d3alek 10:37e7c46837dc 161 dir = position2 < servoPos[SERVO_PITCH] ? 1 : -1;
d3alek 10:37e7c46837dc 162 position2 += servoSpeed * dir;
d3alek 10:37e7c46837dc 163 servoPitch = position2;
d3alek 10:37e7c46837dc 164 } else {
d3alek 10:37e7c46837dc 165 if (mCommandCompleted[SERVO_PITCH] == false) {
d3alek 12:c9d0b1ff36f2 166 printf("Command completed %d\n\r", timer.read_ms());
d3alek 10:37e7c46837dc 167 mCommandCompleted[SERVO_PITCH] = true;
d3alek 7:04ddad10a741 168 }
d3alek 10:37e7c46837dc 169 position2 = servoPos[SERVO_PITCH];
d3alek 7:04ddad10a741 170 }
d3alek 12:c9d0b1ff36f2 171
d3alek 12:c9d0b1ff36f2 172 if (lastSendMs + SEND_EVERY_MS <= timer.read_ms()) {
d3alek 12:c9d0b1ff36f2 173 //printf("SEND %d\n\r", timer.read_ms());
d3alek 12:c9d0b1ff36f2 174 lastSendMs = timer.read_ms();
d3alek 12:c9d0b1ff36f2 175 int res;
d3alek 12:c9d0b1ff36f2 176 if (!awaitingPackets) {
d3alek 12:c9d0b1ff36f2 177 //TODO: check for errors on send
d3alek 10:37e7c46837dc 178 res = sendCommand(0, '\xEB');
d3alek 12:c9d0b1ff36f2 179
d3alek 12:c9d0b1ff36f2 180 if (res) {
d3alek 12:c9d0b1ff36f2 181 if (DEBUG) {
d3alek 12:c9d0b1ff36f2 182 printf("%d: send error %d\n\r", 0, res);
d3alek 12:c9d0b1ff36f2 183 }
d3alek 12:c9d0b1ff36f2 184 res = sendCommand(0, '\xEB');
d3alek 12:c9d0b1ff36f2 185 if (DEBUG) {
d3alek 12:c9d0b1ff36f2 186 printf("%d: two failed sends %d\n\r", 0, res);
d3alek 12:c9d0b1ff36f2 187 }
d3alek 10:37e7c46837dc 188 }
d3alek 12:c9d0b1ff36f2 189 expectingAck1 = true;
d3alek 10:37e7c46837dc 190 res = sendCommand(1, '\xEB');
d3alek 12:c9d0b1ff36f2 191 if (res) {
d3alek 12:c9d0b1ff36f2 192 if (DEBUG) {
d3alek 12:c9d0b1ff36f2 193 printf("%d: send error %d\n\r", 1, res);
d3alek 12:c9d0b1ff36f2 194 }
d3alek 12:c9d0b1ff36f2 195 res = sendCommand(1, '\xEB');
d3alek 12:c9d0b1ff36f2 196 if (DEBUG) {
d3alek 12:c9d0b1ff36f2 197 printf("%d: two failed sends %d\n\r", 1, res);
d3alek 12:c9d0b1ff36f2 198 }
d3alek 10:37e7c46837dc 199 }
d3alek 12:c9d0b1ff36f2 200 expectingAck2 = true;
d3alek 10:37e7c46837dc 201 res = sendCommand(2, '\xEB');
d3alek 10:37e7c46837dc 202 if (res) {
d3alek 10:37e7c46837dc 203 if (DEBUG) {
d3alek 12:c9d0b1ff36f2 204 printf("%d: send error %d\n\r", 2, res);
d3alek 12:c9d0b1ff36f2 205 }
d3alek 12:c9d0b1ff36f2 206 res = sendCommand(2, '\xEB');
d3alek 12:c9d0b1ff36f2 207 if (res) {
d3alek 12:c9d0b1ff36f2 208 if (DEBUG) {
d3alek 12:c9d0b1ff36f2 209 printf("%d: two failed sends %d\n\r", 2, res);
d3alek 12:c9d0b1ff36f2 210 }
d3alek 10:37e7c46837dc 211 }
d3alek 10:37e7c46837dc 212 }
d3alek 12:c9d0b1ff36f2 213 expectingAck3 = true;
d3alek 12:c9d0b1ff36f2 214 awaitingPackets = true;
d3alek 10:37e7c46837dc 215 }
d3alek 9:2c85d7f99a14 216 }
d3alek 10:37e7c46837dc 217
d3alek 10:37e7c46837dc 218 if (expectingAck1) {
d3alek 10:37e7c46837dc 219 expectingAck1 = !getPacket(0);
d3alek 10:37e7c46837dc 220 } else if (expectingAck2) {
d3alek 10:37e7c46837dc 221 expectingAck2 = !getPacket(1);
d3alek 10:37e7c46837dc 222 } else if (expectingAck3) {
d3alek 10:37e7c46837dc 223 expectingAck3 = !getPacket(2);
d3alek 6:83c4801a027d 224 }
d3alek 2:e35627187804 225 // TODO only prints when both are enabled now
d3alek 6:83c4801a027d 226 if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
d3alek 10:37e7c46837dc 227 if ((sensorXs[0] | sensorYs[0] | sensorXs[1] | sensorYs[1] | sensorXs[2] | sensorYs[2]) ) {
d3alek 10:37e7c46837dc 228 // some of the velocities are not 0
d3alek 12:c9d0b1ff36f2 229 if (notTooFarOff()) {
d3alek 12:c9d0b1ff36f2 230 printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
d3alek 10:37e7c46837dc 231 sensorXs[2], sensorYs[2]);
d3alek 12:c9d0b1ff36f2 232 }
d3alek 12:c9d0b1ff36f2 233 else {
d3alek 12:c9d0b1ff36f2 234 mTooFarOffFor++;
d3alek 12:c9d0b1ff36f2 235 if (mTooFarOffFor < MAX_TOO_FAR_OFF_COUNT) {
d3alek 12:c9d0b1ff36f2 236 printf("Too far off %d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
d3alek 12:c9d0b1ff36f2 237 sensorXs[2], sensorYs[2]);
d3alek 12:c9d0b1ff36f2 238 sendResend(0); expectingAck1 = true;
d3alek 12:c9d0b1ff36f2 239 sendResend(1); expectingAck2 = true;
d3alek 12:c9d0b1ff36f2 240 sendResend(2); expectingAck3 = true;
d3alek 12:c9d0b1ff36f2 241 continue;
d3alek 12:c9d0b1ff36f2 242 }
d3alek 12:c9d0b1ff36f2 243 else {
d3alek 12:c9d0b1ff36f2 244 printf("Ignoring a TooFarOff packet %d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
d3alek 12:c9d0b1ff36f2 245 sensorXs[2], sensorYs[2]);
d3alek 12:c9d0b1ff36f2 246 }
d3alek 12:c9d0b1ff36f2 247 }
d3alek 10:37e7c46837dc 248 }
d3alek 12:c9d0b1ff36f2 249 mTooFarOffFor = 0;
d3alek 6:83c4801a027d 250 sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
d3alek 6:83c4801a027d 251 sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
d3alek 9:2c85d7f99a14 252 awaitingPackets = false;
d3alek 9:2c85d7f99a14 253 }
d3alek 12:c9d0b1ff36f2 254 while (lastTickMs + TICK_EVERY_MS > timer.read_ms()) {
d3alek 12:c9d0b1ff36f2 255 wait_ms(1);
d3alek 12:c9d0b1ff36f2 256 }
d3alek 12:c9d0b1ff36f2 257 lastTickMs = timer.read_ms();
d3alek 12:c9d0b1ff36f2 258 //printf("TICK %d\n\r", timer.read_ms());
d3alek 12:c9d0b1ff36f2 259 }
d3alek 10:37e7c46837dc 260 /*
d3alek 10:37e7c46837dc 261 TODO: Deallocate those somewhere
d3alek 10:37e7c46837dc 262
d3alek 10:37e7c46837dc 263 delete[] sensor_init[0];
d3alek 10:37e7c46837dc 264 delete[] sensor_init[1];
d3alek 10:37e7c46837dc 265 delete[] sensor_init[2];
d3alek 10:37e7c46837dc 266 delete[] sensor[0];
d3alek 10:37e7c46837dc 267 delete[] sensor[1];
d3alek 10:37e7c46837dc 268 delete[] sensor[2];
d3alek 10:37e7c46837dc 269 */
d3alek 10:37e7c46837dc 270 }
d3alek 10:37e7c46837dc 271
d3alek 10:37e7c46837dc 272 #define MAX_ANGLE_STR_SIZE 100
d3alek 10:37e7c46837dc 273
d3alek 10:37e7c46837dc 274 void processSerial()
d3alek 10:37e7c46837dc 275 {
d3alek 10:37e7c46837dc 276 char c = pc.getc();
d3alek 10:37e7c46837dc 277 if (c <= '9' && c >= '0') {
d3alek 10:37e7c46837dc 278 int servoNum = int(c - '0');
d3alek 10:37e7c46837dc 279 if (servoNum == SERVO_YAW || servoNum == SERVO_PITCH) {
d3alek 12:c9d0b1ff36f2 280 char rotateAngleStr[MAX_ANGLE_STR_SIZE];
d3alek 10:37e7c46837dc 281 pc.gets(rotateAngleStr, MAX_ANGLE_STR_SIZE);
d3alek 10:37e7c46837dc 282
d3alek 10:37e7c46837dc 283 double rotateAngleNum = atoi(rotateAngleStr);
d3alek 10:37e7c46837dc 284 if (rotateAngleNum > 90 || rotateAngleNum < -90) {
d3alek 10:37e7c46837dc 285 return;
d3alek 10:37e7c46837dc 286 }
d3alek 11:f5dcf8811a4e 287 rotateAngleNum = 0.5 + rotateAngleNum / 180.;
d3alek 10:37e7c46837dc 288 servoPos[servoNum] = rotateAngleNum + servoAdj[servoNum];
d3alek 10:37e7c46837dc 289 mCommandCompleted[servoNum] = false;
d3alek 12:c9d0b1ff36f2 290 printf("Command started %d\n\r", timer.read_ms());
d3alek 10:37e7c46837dc 291 }
d3alek 9:2c85d7f99a14 292 }
d3alek 11:f5dcf8811a4e 293 else if (c == 's') {
d3alek 11:f5dcf8811a4e 294 char buffer[100];
d3alek 11:f5dcf8811a4e 295 pc.gets(buffer, 100);
d3alek 11:f5dcf8811a4e 296 int speedStep = atoi(buffer);
d3alek 11:f5dcf8811a4e 297 servoSpeed = speedStep * SERVO_SPEED_ADJ;
d3alek 11:f5dcf8811a4e 298 printf("Servo speed set to %d\n\r", speedStep);
d3alek 11:f5dcf8811a4e 299 }
d3alek 9:2c85d7f99a14 300 }
d3alek 9:2c85d7f99a14 301
d3alek 10:37e7c46837dc 302 int sendCommand(int ind, char command)
d3alek 10:37e7c46837dc 303 {
d3alek 10:37e7c46837dc 304 int res;
d3alek 10:37e7c46837dc 305 last_command[ind] = command;
d3alek 10:37e7c46837dc 306 //res = sensor_init[ind]->send(command);
d3alek 10:37e7c46837dc 307 res = sensor[ind]->sendCommand(command);
d3alek 10:37e7c46837dc 308 //__enable_irq();
d3alek 10:37e7c46837dc 309 if (res) {
d3alek 10:37e7c46837dc 310 if (DEBUG) printf("error sending command %#x to %d\n\r", command, ind);
d3alek 10:37e7c46837dc 311 }
d3alek 10:37e7c46837dc 312 return res;
d3alek 10:37e7c46837dc 313 }
d3alek 10:37e7c46837dc 314
d3alek 10:37e7c46837dc 315 bool getPacket(int ind)
d3alek 10:37e7c46837dc 316 {
d3alek 10:37e7c46837dc 317 bool successful = processACKReply(ind);
d3alek 10:37e7c46837dc 318 if (!successful) {
d3alek 10:37e7c46837dc 319 sendResend(ind);
d3alek 10:37e7c46837dc 320 successful = processACKReply(ind);
d3alek 10:37e7c46837dc 321 if (!successful) {
d3alek 11:f5dcf8811a4e 322 if (DEBUG) printf("DUNNO WHAT TO DO ACK Reply %d\n\r", ind);
d3alek 10:37e7c46837dc 323 //wait(1);
d3alek 10:37e7c46837dc 324 sendCommand(ind, '\xEB');
d3alek 10:37e7c46837dc 325 return getPacket(ind);
d3alek 0:94cffad90b69 326 }
d3alek 0:94cffad90b69 327 }
d3alek 10:37e7c46837dc 328 successful = getMovementPacket(ind);
d3alek 10:37e7c46837dc 329
d3alek 10:37e7c46837dc 330 if (!successful) {
d3alek 10:37e7c46837dc 331 printf("DISCARDING PACKET %d\n\r", ind);
d3alek 12:c9d0b1ff36f2 332 mDiscardedCount[ind]++;
d3alek 12:c9d0b1ff36f2 333 if (mDiscardedCount[ind] > MAX_DISCARDED) {
d3alek 12:c9d0b1ff36f2 334 initializeSensor(ind);
d3alek 10:37e7c46837dc 335 }
d3alek 12:c9d0b1ff36f2 336 //if (DEBUG) {
d3alek 12:c9d0b1ff36f2 337 sendCommand(ind, '\xEB');
d3alek 12:c9d0b1ff36f2 338 //}
d3alek 9:2c85d7f99a14 339 return false;
d3alek 9:2c85d7f99a14 340 }
d3alek 12:c9d0b1ff36f2 341 mDiscardedCount[ind] = 0;
d3alek 9:2c85d7f99a14 342 return true;
d3alek 0:94cffad90b69 343 }
d3alek 0:94cffad90b69 344
d3alek 10:37e7c46837dc 345 void sendError(int ind)
d3alek 10:37e7c46837dc 346 {
d3alek 10:37e7c46837dc 347 sendCommand(ind, '\xFC');
d3alek 10:37e7c46837dc 348 }
d3alek 10:37e7c46837dc 349
d3alek 10:37e7c46837dc 350 void sendResend(int ind)
d3alek 10:37e7c46837dc 351 {
d3alek 10:37e7c46837dc 352 sendCommand(ind, '\xFE');
d3alek 10:37e7c46837dc 353 }
d3alek 10:37e7c46837dc 354
d3alek 10:37e7c46837dc 355 bool getMovementPacket(int ind)
d3alek 10:37e7c46837dc 356 {
d3alek 10:37e7c46837dc 357 char bytes[3];
d3alek 10:37e7c46837dc 358 int c;
d3alek 10:37e7c46837dc 359 for (int i = 0; i < 3; ++i) {
d3alek 10:37e7c46837dc 360 c = sensor[ind]->getc();
d3alek 10:37e7c46837dc 361 if (c < 0) {
d3alek 10:37e7c46837dc 362 //printf("%d: 255\n\r", ind);
d3alek 10:37e7c46837dc 363 return false;
d3alek 10:37e7c46837dc 364 } else if (i == 0) {
d3alek 10:37e7c46837dc 365 bytes[0] = c;
d3alek 10:37e7c46837dc 366 if (!((c << 5) & 0x100)) {
d3alek 10:37e7c46837dc 367 // not byte[0] wrong offset, skip e
d3alek 12:c9d0b1ff36f2 368 printf("%d: w %d ", ind, c);
d3alek 10:37e7c46837dc 369 c = sensor[ind]->getc();
d3alek 10:37e7c46837dc 370 while (c >= 0) {
d3alek 10:37e7c46837dc 371 printf("%d ", c);
d3alek 10:37e7c46837dc 372 c = sensor[ind]->getc();
d3alek 10:37e7c46837dc 373 }
d3alek 10:37e7c46837dc 374 printf("\n\r");
d3alek 10:37e7c46837dc 375 return false;
d3alek 10:37e7c46837dc 376 }
d3alek 10:37e7c46837dc 377 } else if (i == 1) {
d3alek 10:37e7c46837dc 378 bytes[1] = c;
d3alek 10:37e7c46837dc 379 } else if (i == 2) {
d3alek 10:37e7c46837dc 380 bytes[2] = c;
d3alek 10:37e7c46837dc 381 //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 10:37e7c46837dc 382
d3alek 10:37e7c46837dc 383 //TODO: check for overflow
d3alek 10:37e7c46837dc 384 if ((1 << 6) & bytes[0]) {
d3alek 10:37e7c46837dc 385 //printf("%d: Overflow x %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 10:37e7c46837dc 386 return false;
d3alek 10:37e7c46837dc 387 } else if ((1 << 7) & bytes[0]) {
d3alek 10:37e7c46837dc 388 printf("%d: Overflow y %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 10:37e7c46837dc 389 return false;
d3alek 10:37e7c46837dc 390 }
d3alek 10:37e7c46837dc 391 // check x and y signs
d3alek 10:37e7c46837dc 392 else {
d3alek 10:37e7c46837dc 393 int x = bytes[1] - ((bytes[0] << 4) & 0x100);
d3alek 10:37e7c46837dc 394 int y = bytes[2] - ((bytes[0] << 3) & 0x100);
d3alek 10:37e7c46837dc 395 //printf("%s: x = %d y = %d\n\r", id, x, y);
d3alek 10:37e7c46837dc 396 sensorXs[ind] = x;
d3alek 10:37e7c46837dc 397 sensorYs[ind] = y;
d3alek 10:37e7c46837dc 398 sensorToPrint[ind] = true;
d3alek 10:37e7c46837dc 399 //printf("%d ", ind);
d3alek 10:37e7c46837dc 400 }
d3alek 10:37e7c46837dc 401 }
d3alek 10:37e7c46837dc 402 }
d3alek 10:37e7c46837dc 403 return true;
d3alek 10:37e7c46837dc 404 }
d3alek 10:37e7c46837dc 405
d3alek 10:37e7c46837dc 406 bool processACKReply(int ind)
d3alek 10:37e7c46837dc 407 {
d3alek 10:37e7c46837dc 408 int reply = sensor[ind]->getc();
d3alek 10:37e7c46837dc 409 if (reply < 0) {
d3alek 10:37e7c46837dc 410 if (DEBUG) printf("%d: Error %d", ind, reply);
d3alek 10:37e7c46837dc 411 return false;
d3alek 10:37e7c46837dc 412 } else if (reply == '\xFA') {
d3alek 10:37e7c46837dc 413 //if (DEBUG) printf("%d: ACK ", ind);
d3alek 10:37e7c46837dc 414 return true;
d3alek 10:37e7c46837dc 415 } else if (reply == '\xEB') {
d3alek 10:37e7c46837dc 416 if (DEBUG) printf("%d: READ_DATA ", ind);
d3alek 10:37e7c46837dc 417 //TODO: inf loop possible here cause recursion
d3alek 10:37e7c46837dc 418 return processACKReply(ind);
d3alek 10:37e7c46837dc 419 } else if (reply == '\xFE') {
d3alek 10:37e7c46837dc 420 if (last_command[ind] == '\xFE') {
d3alek 10:37e7c46837dc 421 if (DEBUG) printf("%d: REPEATING_COMMAND BUT REPEAT_COMMAND!", ind, last_command[ind]);
d3alek 10:37e7c46837dc 422 //wait(1);
d3alek 10:37e7c46837dc 423 sendCommand(ind, '\xEB');
d3alek 10:37e7c46837dc 424 return processACKReply(ind);
d3alek 10:37e7c46837dc 425 }
d3alek 10:37e7c46837dc 426 // repeat command
d3alek 10:37e7c46837dc 427 if (DEBUG) printf("%d: REPEATING_COMMAND %#x", ind, last_command[ind]);
d3alek 10:37e7c46837dc 428 //wait(1);
d3alek 10:37e7c46837dc 429 sendCommand(ind, last_command[ind]);
d3alek 10:37e7c46837dc 430 return processACKReply(ind);
d3alek 10:37e7c46837dc 431 } else if (reply == '\xF2') {
d3alek 10:37e7c46837dc 432 // get device ID??
d3alek 10:37e7c46837dc 433 if (DEBUG) printf("%d: GET_DEVICE_ID %#x", ind, reply);
d3alek 10:37e7c46837dc 434 //wait(1);
d3alek 10:37e7c46837dc 435 return processACKReply(ind);
d3alek 10:37e7c46837dc 436 } else {
d3alek 10:37e7c46837dc 437 if (DEBUG) printf("%d: Unexpected ACK Reply %d\n\r", ind, reply);
d3alek 10:37e7c46837dc 438 //wait(1);
d3alek 10:37e7c46837dc 439 return processACKReply(ind);
d3alek 10:37e7c46837dc 440 }
d3alek 10:37e7c46837dc 441 }
d3alek 10:37e7c46837dc 442 /*
d3alek 8:41b35bda9d48 443 int process_sensor_input(int c, int bytenum, char* bytes, int ind)
d3alek 2:e35627187804 444 {
d3alek 8:41b35bda9d48 445 if (c < 0) {
d3alek 8:41b35bda9d48 446 //printf("%d: 255\n\r", ind);
d3alek 2:e35627187804 447 bytenum = -1;
d3alek 8:41b35bda9d48 448 } else if (bytenum % BYTES_NUM == 0) {
d3alek 2:e35627187804 449 bytes[0] = c;
d3alek 8:41b35bda9d48 450 if (!((c << 5) & 0x100)) {
d3alek 2:e35627187804 451 // not byte[0] wrong offset, skip c
d3alek 9:2c85d7f99a14 452 if (DEBUG) printf("%d: w %d\n\r", ind, c);
d3alek 2:e35627187804 453 bytenum = -1;
d3alek 9:2c85d7f99a14 454 sendError(ind);
d3alek 0:94cffad90b69 455 }
d3alek 8:41b35bda9d48 456 } else if (bytenum % BYTES_NUM == 1) {
d3alek 2:e35627187804 457 bytes[1] = c;
d3alek 8:41b35bda9d48 458 } else if (bytenum % BYTES_NUM == 2) {
d3alek 2:e35627187804 459 bytes[2] = c;
d3alek 8:41b35bda9d48 460 //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 0:94cffad90b69 461
d3alek 2:e35627187804 462 //TODO: check for overflow
d3alek 2:e35627187804 463 if ((1 << 6) & bytes[0]) {
d3alek 9:2c85d7f99a14 464 printf("%d: Overflow x %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
d3alek 9:2c85d7f99a14 465 if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
d3alek 9:2c85d7f99a14 466 sendError(ind);
d3alek 10:37e7c46837dc 467 } else {
d3alek 9:2c85d7f99a14 468 consecutiveOverflows[ind] = 0;
d3alek 9:2c85d7f99a14 469 }
d3alek 3:eb3c3c9587d7 470 bytenum = -1;
d3alek 10:37e7c46837dc 471 } else if ((1 << 7) & bytes[0]) {
d3alek 9:2c85d7f99a14 472 printf("%d: Overflow y %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
d3alek 9:2c85d7f99a14 473 if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
d3alek 9:2c85d7f99a14 474 sendError(ind);
d3alek 10:37e7c46837dc 475 } else {
d3alek 9:2c85d7f99a14 476 consecutiveOverflows[ind] = 0;
d3alek 9:2c85d7f99a14 477 }
d3alek 9:2c85d7f99a14 478 bytenum = -1;
d3alek 0:94cffad90b69 479 }
d3alek 2:e35627187804 480 // check x and y signs
d3alek 3:eb3c3c9587d7 481 else {
d3alek 3:eb3c3c9587d7 482 int x = bytes[1] - ((bytes[0] << 4) & 0x100);
d3alek 3:eb3c3c9587d7 483 int y = bytes[2] - ((bytes[0] << 3) & 0x100);
d3alek 3:eb3c3c9587d7 484 //printf("%s: x = %d y = %d\n\r", id, x, y);
d3alek 3:eb3c3c9587d7 485 sensorXs[ind] = x;
d3alek 3:eb3c3c9587d7 486 sensorYs[ind] = y;
d3alek 3:eb3c3c9587d7 487 sensorToPrint[ind] = true;
d3alek 8:41b35bda9d48 488 //printf("%d ", ind);
d3alek 3:eb3c3c9587d7 489 bytenum = -1;
d3alek 3:eb3c3c9587d7 490 }
d3alek 0:94cffad90b69 491 }
d3alek 8:41b35bda9d48 492 return (bytenum + 1) % BYTES_NUM;
d3alek 0:94cffad90b69 493 }
d3alek 10:37e7c46837dc 494 */
d3alek 10:37e7c46837dc 495 /*
d3alek 9:2c85d7f99a14 496 void sendError(int ind)
d3alek 9:2c85d7f99a14 497 {
d3alek 9:2c85d7f99a14 498 switch (ind) {
d3alek 9:2c85d7f99a14 499 case 0:
d3alek 9:2c85d7f99a14 500 sensor1_init.send('\xFE');
d3alek 9:2c85d7f99a14 501 expectingAck1 = true;
d3alek 9:2c85d7f99a14 502 break;
d3alek 9:2c85d7f99a14 503 case 1:
d3alek 9:2c85d7f99a14 504 sensor2_init.send('\xFE');
d3alek 9:2c85d7f99a14 505 expectingAck2 = true;
d3alek 9:2c85d7f99a14 506 break;
d3alek 9:2c85d7f99a14 507 case 2:
d3alek 9:2c85d7f99a14 508 sensor3_init.send('\xFE');
d3alek 9:2c85d7f99a14 509 expectingAck3 = true;
d3alek 9:2c85d7f99a14 510 break;
d3alek 9:2c85d7f99a14 511 }
d3alek 10:37e7c46837dc 512 }*/