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@9:2c85d7f99a14, 2014-03-06 (annotated)

Committer:
d3alek
Date:
Thu Mar 06 18:09:37 2014 +0000
Revision:
9:2c85d7f99a14
Parent:
8:41b35bda9d48
Child:
10:37e7c46837dc
remote mode works, sometimes gets stuck

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 2:e35627187804 6 #define ENABLE_1 true
d3alek 2:e35627187804 7 #define ENABLE_2 true
d3alek 6:83c4801a027d 8 #define ENABLE_3 true
d3alek 2:e35627187804 9
d3alek 6:83c4801a027d 10 #define SENSORS_NUM 3
d3alek 8:41b35bda9d48 11 #define BYTES_NUM 3
d3alek 6:83c4801a027d 12
d3alek 9:2c85d7f99a14 13 #define DEBUG false
d3alek 9:2c85d7f99a14 14
d3alek 9:2c85d7f99a14 15 #define MAX_OVERFLOWS 3
d3alek 9:2c85d7f99a14 16
d3alek 9:2c85d7f99a14 17 #define MAX_REPLY_ERRORS 3
d3alek 9:2c85d7f99a14 18
d3alek 6:83c4801a027d 19 Servo servo1(p21);
d3alek 6:83c4801a027d 20 Servo servo2(p24);
d3alek 5:43d5529fbe1e 21
d3alek 0:94cffad90b69 22 DigitalOut myled(LED1);
d3alek 0:94cffad90b69 23 Serial pc(USBTX, USBRX); // tx, rx
d3alek 2:e35627187804 24 /*
d3alek 2:e35627187804 25 * 0xFF: Reset command.
d3alek 2:e35627187804 26 * 0xF3: Set sample rate.
d3alek 2:e35627187804 27 * 0xF2: Read device type.
d3alek 2:e35627187804 28 * 0xE8: Set resolution.
d3alek 2:e35627187804 29 * 0xE6: Set scaling.
d3alek 2:e35627187804 30 * 0xF4: Enable device.
d3alek 2:e35627187804 31 */
d3alek 0:94cffad90b69 32
d3alek 6:83c4801a027d 33 //TODO should Iuse sensor1_init? maybe no 255s?
d3alek 6:83c4801a027d 34 PS2MS_INIT sensor1_init(p18, p17);
d3alek 6:83c4801a027d 35 PS2MS sensor1(p18, p17);
d3alek 0:94cffad90b69 36
d3alek 6:83c4801a027d 37 PS2MS_INIT sensor2_init(p23, p22);
d3alek 6:83c4801a027d 38 PS2MS sensor2(p23, p22);
d3alek 2:e35627187804 39
d3alek 6:83c4801a027d 40 PS2MS_INIT sensor3_init(p26, p25);
d3alek 6:83c4801a027d 41 PS2MS sensor3(p26, p25);
d3alek 2:e35627187804 42
d3alek 8:41b35bda9d48 43 int process_sensor_input(int c, int bytenum, char* bytes, int ind);
d3alek 9:2c85d7f99a14 44 bool processACKReply(int reply, int ind);
d3alek 9:2c85d7f99a14 45 void sendError(int ind);
d3alek 2:e35627187804 46
d3alek 6:83c4801a027d 47 int sensorXs[SENSORS_NUM];
d3alek 6:83c4801a027d 48 int sensorYs[SENSORS_NUM];
d3alek 6:83c4801a027d 49 bool sensorToPrint[SENSORS_NUM];
d3alek 0:94cffad90b69 50
d3alek 9:2c85d7f99a14 51 bool expectingAck1 = false, expectingAck2 = false, expectingAck3 = false;
d3alek 9:2c85d7f99a14 52 int consecutiveOverflows[3];
d3alek 9:2c85d7f99a14 53
d3alek 9:2c85d7f99a14 54 int replyErrors[3];
d3alek 9:2c85d7f99a14 55
d3alek 2:e35627187804 56 int main()
d3alek 2:e35627187804 57 {
d3alek 5:43d5529fbe1e 58
d3alek 5:43d5529fbe1e 59 float range = 0.00085;
d3alek 6:83c4801a027d 60 float position1 = 0.5;
d3alek 6:83c4801a027d 61 float position2 = 0.5;
d3alek 5:43d5529fbe1e 62 float position_adj = 0.03;
d3alek 5:43d5529fbe1e 63
d3alek 7:04ddad10a741 64 float newPos1 = position1, newPos2 = position2;
d3alek 7:04ddad10a741 65
d3alek 9:2c85d7f99a14 66 consecutiveOverflows[0] = consecutiveOverflows[1] = consecutiveOverflows[2] = 0;
d3alek 9:2c85d7f99a14 67 replyErrors[0] = replyErrors[1] = replyErrors[2] = 0;
d3alek 9:2c85d7f99a14 68
d3alek 6:83c4801a027d 69 servo1.calibrate(range, 45.0);
d3alek 6:83c4801a027d 70 servo2.calibrate(range, 45.0);
d3alek 9:2c85d7f99a14 71 servo1 = position1 + position_adj;
d3alek 9:2c85d7f99a14 72 servo2 = position2 + position_adj;
d3alek 6:83c4801a027d 73 printf("position = %.3f, range = +/-%0.5f\n", position1, range, position_adj);
d3alek 5:43d5529fbe1e 74
d3alek 4:73f9fd797965 75 printf("IMHERE START\n");
d3alek 2:e35627187804 76 int s1bytenum = 0;
d3alek 8:41b35bda9d48 77 char s1bytes[BYTES_NUM];
d3alek 2:e35627187804 78
d3alek 2:e35627187804 79 int s2bytenum = 0;
d3alek 8:41b35bda9d48 80 char s2bytes[BYTES_NUM];
d3alek 6:83c4801a027d 81
d3alek 6:83c4801a027d 82 int s3bytenum = 0;
d3alek 8:41b35bda9d48 83 char s3bytes[BYTES_NUM];
d3alek 2:e35627187804 84
d3alek 8:41b35bda9d48 85 int s1c, s2c, s3c;
d3alek 4:73f9fd797965 86 printf("IMHERE GET SENSORS\n");
d3alek 2:e35627187804 87 s1c = sensor1.getc();
d3alek 4:73f9fd797965 88 printf("IMHERE GOT S1\n");
d3alek 2:e35627187804 89 s2c = sensor2.getc();
d3alek 4:73f9fd797965 90 printf("IMHERE GOT S2\n");
d3alek 6:83c4801a027d 91 s3c = sensor2.getc();
d3alek 6:83c4801a027d 92 printf("IMHERE GOT S3\n");
d3alek 6:83c4801a027d 93 sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
d3alek 7:04ddad10a741 94 int dir;
d3alek 9:2c85d7f99a14 95 float servoSpeed = 0.002;
d3alek 9:2c85d7f99a14 96
d3alek 9:2c85d7f99a14 97 int recv;
d3alek 9:2c85d7f99a14 98
d3alek 9:2c85d7f99a14 99 bool awaitingPackets = false;
d3alek 2:e35627187804 100
d3alek 0:94cffad90b69 101 while(1) {
d3alek 5:43d5529fbe1e 102 if (pc.readable()) {
d3alek 5:43d5529fbe1e 103 switch(pc.getc()) {
d3alek 7:04ddad10a741 104 case '1': newPos1 = 0.0 + position_adj; break; //position1 = 0.0 + position_adj; break;
d3alek 7:04ddad10a741 105 case '2': newPos1 = 0.5 + position_adj; break; //position1 = 0.5 + position_adj; break;
d3alek 9:2c85d7f99a14 106 case '3': newPos1 = 1.0 + position_adj; break; //position1 = 1.0 + position_adj; break;
d3alek 7:04ddad10a741 107 case '4': newPos2 = 0.3 + position_adj; break;
d3alek 7:04ddad10a741 108 case '5': newPos2 = 0.5 + position_adj; break;
d3alek 7:04ddad10a741 109 case '6': newPos2 = 0.7 + position_adj; break;
d3alek 5:43d5529fbe1e 110 }
d3alek 5:43d5529fbe1e 111 //printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
d3alek 7:04ddad10a741 112 //dir = position1 < newPos1 ? 1 : -1;
d3alek 7:04ddad10a741 113 /*printf("IMHERE %f %f %d\n", position1, newPos1, dir);
d3alek 7:04ddad10a741 114 for (float i = position1; abs(i - newPos1) > servoSpeed; i += servoSpeed * dir) {
d3alek 7:04ddad10a741 115 servo1 = i;
d3alek 7:04ddad10a741 116 printf("%f %f\n", i, newPos1);
d3alek 7:04ddad10a741 117 }
d3alek 7:04ddad10a741 118 position1 = newPos1;*/
d3alek 7:04ddad10a741 119 //servo1 = position1;
d3alek 7:04ddad10a741 120 //servo2 = position2;
d3alek 7:04ddad10a741 121 }
d3alek 7:04ddad10a741 122
d3alek 7:04ddad10a741 123 if (abs(position1 - newPos1) > servoSpeed) {
d3alek 7:04ddad10a741 124 dir = position1 < newPos1 ? 1 : -1;
d3alek 7:04ddad10a741 125 position1 += servoSpeed * dir;
d3alek 6:83c4801a027d 126 servo1 = position1;
d3alek 7:04ddad10a741 127 }
d3alek 7:04ddad10a741 128 else {
d3alek 7:04ddad10a741 129 position1 = newPos1;
d3alek 7:04ddad10a741 130 }
d3alek 7:04ddad10a741 131
d3alek 7:04ddad10a741 132 if (abs(position2 - newPos2) > servoSpeed) {
d3alek 7:04ddad10a741 133 dir = position2 < newPos2 ? 1 : -1;
d3alek 7:04ddad10a741 134 position2 += servoSpeed * dir;
d3alek 6:83c4801a027d 135 servo2 = position2;
d3alek 5:43d5529fbe1e 136 }
d3alek 7:04ddad10a741 137 else {
d3alek 7:04ddad10a741 138 position2 = newPos2;
d3alek 7:04ddad10a741 139 }
d3alek 9:2c85d7f99a14 140
d3alek 9:2c85d7f99a14 141 if (!awaitingPackets) {
d3alek 9:2c85d7f99a14 142 sensor1_init.send('\xEB');
d3alek 9:2c85d7f99a14 143 expectingAck1 = true;
d3alek 9:2c85d7f99a14 144 sensor2_init.send('\xEB');
d3alek 9:2c85d7f99a14 145 expectingAck2 = true;
d3alek 9:2c85d7f99a14 146 sensor3_init.send('\xEB');
d3alek 9:2c85d7f99a14 147 expectingAck3 = true;
d3alek 9:2c85d7f99a14 148 awaitingPackets = true;
d3alek 9:2c85d7f99a14 149 }
d3alek 9:2c85d7f99a14 150
d3alek 2:e35627187804 151 if (ENABLE_1) {
d3alek 9:2c85d7f99a14 152
d3alek 2:e35627187804 153 s1c = sensor1.getc();
d3alek 9:2c85d7f99a14 154 if (expectingAck1) {
d3alek 9:2c85d7f99a14 155 expectingAck1 = processACKReply(s1c, 1);
d3alek 9:2c85d7f99a14 156 }
d3alek 9:2c85d7f99a14 157 else {
d3alek 9:2c85d7f99a14 158 s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
d3alek 9:2c85d7f99a14 159 }
d3alek 2:e35627187804 160 }
d3alek 2:e35627187804 161 if (ENABLE_2) {
d3alek 2:e35627187804 162 s2c = sensor2.getc();
d3alek 9:2c85d7f99a14 163 if (expectingAck2) {
d3alek 9:2c85d7f99a14 164 expectingAck2 = processACKReply(s2c, 2);
d3alek 9:2c85d7f99a14 165 }
d3alek 9:2c85d7f99a14 166 else {
d3alek 9:2c85d7f99a14 167 s2bytenum = process_sensor_input(s2c, s2bytenum, s2bytes, 1);
d3alek 9:2c85d7f99a14 168 }
d3alek 2:e35627187804 169 }
d3alek 6:83c4801a027d 170 if (ENABLE_3) {
d3alek 9:2c85d7f99a14 171
d3alek 6:83c4801a027d 172 s3c = sensor3.getc();
d3alek 9:2c85d7f99a14 173 if (expectingAck3) {
d3alek 9:2c85d7f99a14 174 expectingAck3 = processACKReply(s3c, 3);
d3alek 9:2c85d7f99a14 175 }
d3alek 9:2c85d7f99a14 176 else {
d3alek 9:2c85d7f99a14 177 s3bytenum = process_sensor_input(s3c, s3bytenum, s3bytes, 2);
d3alek 9:2c85d7f99a14 178 }
d3alek 6:83c4801a027d 179 }
d3alek 2:e35627187804 180 // TODO only prints when both are enabled now
d3alek 6:83c4801a027d 181 if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
d3alek 6:83c4801a027d 182 printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
d3alek 6:83c4801a027d 183 sensorXs[2], sensorYs[2]);
d3alek 6:83c4801a027d 184 sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
d3alek 6:83c4801a027d 185 sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
d3alek 9:2c85d7f99a14 186 awaitingPackets = false;
d3alek 9:2c85d7f99a14 187 }
d3alek 9:2c85d7f99a14 188 }
d3alek 9:2c85d7f99a14 189 }
d3alek 9:2c85d7f99a14 190
d3alek 9:2c85d7f99a14 191 bool processACKReply(int reply, int ind) {
d3alek 9:2c85d7f99a14 192 if (reply < 0) {
d3alek 9:2c85d7f99a14 193 if (DEBUG) printf("%d: Error %d - %d", ind, reply, replyErrors[ind]);
d3alek 9:2c85d7f99a14 194 replyErrors[ind]++;
d3alek 9:2c85d7f99a14 195 if (replyErrors[ind] > MAX_REPLY_ERRORS) {
d3alek 9:2c85d7f99a14 196 if (DEBUG) printf("%d: MAX ERRORS REACHED %d", ind, reply);
d3alek 9:2c85d7f99a14 197 replyErrors[ind] = 0;
d3alek 9:2c85d7f99a14 198 return false;
d3alek 0:94cffad90b69 199 }
d3alek 0:94cffad90b69 200 }
d3alek 9:2c85d7f99a14 201 else if (reply == '\xFA') {
d3alek 9:2c85d7f99a14 202 if (DEBUG) printf("%d: ACK ", ind);
d3alek 9:2c85d7f99a14 203 return false;
d3alek 9:2c85d7f99a14 204 }
d3alek 9:2c85d7f99a14 205 else if (reply == '\xEB') {
d3alek 9:2c85d7f99a14 206 if (DEBUG) printf("%d: READ_DATA ", ind);
d3alek 9:2c85d7f99a14 207 }
d3alek 9:2c85d7f99a14 208 else if (reply == '\xFE') {
d3alek 9:2c85d7f99a14 209 if (DEBUG) printf("%d: RESEND received\n\r", ind);
d3alek 9:2c85d7f99a14 210 return false;
d3alek 9:2c85d7f99a14 211 }
d3alek 9:2c85d7f99a14 212 else if (reply == '\xF2') {
d3alek 9:2c85d7f99a14 213 if (DEBUG) printf("%d: GET_DEVICE_ID received\n\r", ind);
d3alek 9:2c85d7f99a14 214 return false;
d3alek 9:2c85d7f99a14 215 }
d3alek 9:2c85d7f99a14 216 else {
d3alek 9:2c85d7f99a14 217 if (DEBUG) printf("%d: Unexpected ACK Reply %d\n\r", ind, reply);
d3alek 9:2c85d7f99a14 218 }
d3alek 9:2c85d7f99a14 219 return true;
d3alek 0:94cffad90b69 220 }
d3alek 0:94cffad90b69 221
d3alek 8:41b35bda9d48 222 int process_sensor_input(int c, int bytenum, char* bytes, int ind)
d3alek 2:e35627187804 223 {
d3alek 8:41b35bda9d48 224 if (c < 0) {
d3alek 8:41b35bda9d48 225 //printf("%d: 255\n\r", ind);
d3alek 2:e35627187804 226 bytenum = -1;
d3alek 8:41b35bda9d48 227 } else if (bytenum % BYTES_NUM == 0) {
d3alek 2:e35627187804 228 bytes[0] = c;
d3alek 8:41b35bda9d48 229 if (!((c << 5) & 0x100)) {
d3alek 2:e35627187804 230 // not byte[0] wrong offset, skip c
d3alek 9:2c85d7f99a14 231 if (DEBUG) printf("%d: w %d\n\r", ind, c);
d3alek 2:e35627187804 232 bytenum = -1;
d3alek 9:2c85d7f99a14 233 sendError(ind);
d3alek 0:94cffad90b69 234 }
d3alek 8:41b35bda9d48 235 } else if (bytenum % BYTES_NUM == 1) {
d3alek 2:e35627187804 236 bytes[1] = c;
d3alek 8:41b35bda9d48 237 } else if (bytenum % BYTES_NUM == 2) {
d3alek 2:e35627187804 238 bytes[2] = c;
d3alek 8:41b35bda9d48 239 //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 0:94cffad90b69 240
d3alek 2:e35627187804 241 //TODO: check for overflow
d3alek 2:e35627187804 242 if ((1 << 6) & bytes[0]) {
d3alek 9:2c85d7f99a14 243 printf("%d: Overflow x %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
d3alek 9:2c85d7f99a14 244 if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
d3alek 9:2c85d7f99a14 245 sendError(ind);
d3alek 9:2c85d7f99a14 246 }
d3alek 9:2c85d7f99a14 247 else {
d3alek 9:2c85d7f99a14 248 consecutiveOverflows[ind] = 0;
d3alek 9:2c85d7f99a14 249 }
d3alek 3:eb3c3c9587d7 250 bytenum = -1;
d3alek 0:94cffad90b69 251 }
d3alek 3:eb3c3c9587d7 252 else if ((1 << 7) & bytes[0]) {
d3alek 9:2c85d7f99a14 253 printf("%d: Overflow y %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
d3alek 9:2c85d7f99a14 254 if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
d3alek 9:2c85d7f99a14 255 sendError(ind);
d3alek 9:2c85d7f99a14 256 }
d3alek 9:2c85d7f99a14 257 else {
d3alek 9:2c85d7f99a14 258 consecutiveOverflows[ind] = 0;
d3alek 9:2c85d7f99a14 259 }
d3alek 9:2c85d7f99a14 260 bytenum = -1;
d3alek 0:94cffad90b69 261 }
d3alek 2:e35627187804 262 // check x and y signs
d3alek 3:eb3c3c9587d7 263 else {
d3alek 3:eb3c3c9587d7 264 int x = bytes[1] - ((bytes[0] << 4) & 0x100);
d3alek 3:eb3c3c9587d7 265 int y = bytes[2] - ((bytes[0] << 3) & 0x100);
d3alek 3:eb3c3c9587d7 266 //printf("%s: x = %d y = %d\n\r", id, x, y);
d3alek 3:eb3c3c9587d7 267 sensorXs[ind] = x;
d3alek 3:eb3c3c9587d7 268 sensorYs[ind] = y;
d3alek 3:eb3c3c9587d7 269 sensorToPrint[ind] = true;
d3alek 8:41b35bda9d48 270 //printf("%d ", ind);
d3alek 3:eb3c3c9587d7 271 bytenum = -1;
d3alek 3:eb3c3c9587d7 272 }
d3alek 0:94cffad90b69 273 }
d3alek 8:41b35bda9d48 274 return (bytenum + 1) % BYTES_NUM;
d3alek 0:94cffad90b69 275 }
d3alek 9:2c85d7f99a14 276
d3alek 9:2c85d7f99a14 277 void sendError(int ind)
d3alek 9:2c85d7f99a14 278 {
d3alek 9:2c85d7f99a14 279 switch (ind) {
d3alek 9:2c85d7f99a14 280 case 0:
d3alek 9:2c85d7f99a14 281 sensor1_init.send('\xFE');
d3alek 9:2c85d7f99a14 282 expectingAck1 = true;
d3alek 9:2c85d7f99a14 283 break;
d3alek 9:2c85d7f99a14 284 case 1:
d3alek 9:2c85d7f99a14 285 sensor2_init.send('\xFE');
d3alek 9:2c85d7f99a14 286 expectingAck2 = true;
d3alek 9:2c85d7f99a14 287 break;
d3alek 9:2c85d7f99a14 288 case 2:
d3alek 9:2c85d7f99a14 289 sensor3_init.send('\xFE');
d3alek 9:2c85d7f99a14 290 expectingAck3 = true;
d3alek 9:2c85d7f99a14 291 break;
d3alek 9:2c85d7f99a14 292 }
d3alek 9:2c85d7f99a14 293 }