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@8:41b35bda9d48, 2014-03-06 (annotated)

Committer:
d3alek
Date:
Thu Mar 06 10:38:43 2014 +0000
Revision:
8:41b35bda9d48
Parent:
7:04ddad10a741
Child:
9:2c85d7f99a14
eof bug fixed, stream mode

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 6:83c4801a027d 13 Servo servo1(p21);
d3alek 6:83c4801a027d 14 Servo servo2(p24);
d3alek 5:43d5529fbe1e 15
d3alek 0:94cffad90b69 16 DigitalOut myled(LED1);
d3alek 0:94cffad90b69 17 Serial pc(USBTX, USBRX); // tx, rx
d3alek 2:e35627187804 18 /*
d3alek 2:e35627187804 19 * 0xFF: Reset command.
d3alek 2:e35627187804 20 * 0xF3: Set sample rate.
d3alek 2:e35627187804 21 * 0xF2: Read device type.
d3alek 2:e35627187804 22 * 0xE8: Set resolution.
d3alek 2:e35627187804 23 * 0xE6: Set scaling.
d3alek 2:e35627187804 24 * 0xF4: Enable device.
d3alek 2:e35627187804 25 */
d3alek 0:94cffad90b69 26
d3alek 6:83c4801a027d 27 //TODO should Iuse sensor1_init? maybe no 255s?
d3alek 6:83c4801a027d 28 PS2MS_INIT sensor1_init(p18, p17);
d3alek 6:83c4801a027d 29 PS2MS sensor1(p18, p17);
d3alek 0:94cffad90b69 30
d3alek 6:83c4801a027d 31 PS2MS_INIT sensor2_init(p23, p22);
d3alek 6:83c4801a027d 32 PS2MS sensor2(p23, p22);
d3alek 2:e35627187804 33
d3alek 6:83c4801a027d 34 PS2MS_INIT sensor3_init(p26, p25);
d3alek 6:83c4801a027d 35 PS2MS sensor3(p26, p25);
d3alek 2:e35627187804 36
d3alek 8:41b35bda9d48 37 int process_sensor_input(int c, int bytenum, char* bytes, int ind);
d3alek 2:e35627187804 38
d3alek 6:83c4801a027d 39 int sensorXs[SENSORS_NUM];
d3alek 6:83c4801a027d 40 int sensorYs[SENSORS_NUM];
d3alek 6:83c4801a027d 41 bool sensorToPrint[SENSORS_NUM];
d3alek 0:94cffad90b69 42
d3alek 2:e35627187804 43 int main()
d3alek 2:e35627187804 44 {
d3alek 5:43d5529fbe1e 45
d3alek 5:43d5529fbe1e 46 float range = 0.00085;
d3alek 6:83c4801a027d 47 float position1 = 0.5;
d3alek 6:83c4801a027d 48 float position2 = 0.5;
d3alek 5:43d5529fbe1e 49 float position_adj = 0.03;
d3alek 5:43d5529fbe1e 50
d3alek 7:04ddad10a741 51 float newPos1 = position1, newPos2 = position2;
d3alek 7:04ddad10a741 52
d3alek 6:83c4801a027d 53 servo1.calibrate(range, 45.0);
d3alek 6:83c4801a027d 54 servo2.calibrate(range, 45.0);
d3alek 6:83c4801a027d 55 servo1 = position1;
d3alek 6:83c4801a027d 56 servo2 = position2;
d3alek 6:83c4801a027d 57 printf("position = %.3f, range = +/-%0.5f\n", position1, range, position_adj);
d3alek 5:43d5529fbe1e 58
d3alek 4:73f9fd797965 59 printf("IMHERE START\n");
d3alek 2:e35627187804 60 int s1bytenum = 0;
d3alek 8:41b35bda9d48 61 char s1bytes[BYTES_NUM];
d3alek 2:e35627187804 62
d3alek 2:e35627187804 63 int s2bytenum = 0;
d3alek 8:41b35bda9d48 64 char s2bytes[BYTES_NUM];
d3alek 6:83c4801a027d 65
d3alek 6:83c4801a027d 66 int s3bytenum = 0;
d3alek 8:41b35bda9d48 67 char s3bytes[BYTES_NUM];
d3alek 2:e35627187804 68
d3alek 8:41b35bda9d48 69 int s1c, s2c, s3c;
d3alek 4:73f9fd797965 70 printf("IMHERE GET SENSORS\n");
d3alek 2:e35627187804 71 s1c = sensor1.getc();
d3alek 4:73f9fd797965 72 printf("IMHERE GOT S1\n");
d3alek 2:e35627187804 73 s2c = sensor2.getc();
d3alek 4:73f9fd797965 74 printf("IMHERE GOT S2\n");
d3alek 6:83c4801a027d 75 s3c = sensor2.getc();
d3alek 6:83c4801a027d 76 printf("IMHERE GOT S3\n");
d3alek 6:83c4801a027d 77 sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
d3alek 7:04ddad10a741 78 int dir;
d3alek 7:04ddad10a741 79 float servoSpeed = 0.001;
d3alek 2:e35627187804 80
d3alek 0:94cffad90b69 81 while(1) {
d3alek 5:43d5529fbe1e 82 if (pc.readable()) {
d3alek 5:43d5529fbe1e 83 switch(pc.getc()) {
d3alek 7:04ddad10a741 84 case '1': newPos1 = 0.0 + position_adj; break; //position1 = 0.0 + position_adj; break;
d3alek 7:04ddad10a741 85 case '2': newPos1 = 0.5 + position_adj; break; //position1 = 0.5 + position_adj; break;
d3alek 8:41b35bda9d48 86 case '3': newPos1 = 0.6 + position_adj; break; //position1 = 1.0 + position_adj; break;
d3alek 7:04ddad10a741 87 case '4': newPos2 = 0.3 + position_adj; break;
d3alek 7:04ddad10a741 88 case '5': newPos2 = 0.5 + position_adj; break;
d3alek 7:04ddad10a741 89 case '6': newPos2 = 0.7 + position_adj; break;
d3alek 5:43d5529fbe1e 90 }
d3alek 5:43d5529fbe1e 91 //printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
d3alek 7:04ddad10a741 92 //dir = position1 < newPos1 ? 1 : -1;
d3alek 7:04ddad10a741 93 /*printf("IMHERE %f %f %d\n", position1, newPos1, dir);
d3alek 7:04ddad10a741 94 for (float i = position1; abs(i - newPos1) > servoSpeed; i += servoSpeed * dir) {
d3alek 7:04ddad10a741 95 servo1 = i;
d3alek 7:04ddad10a741 96 printf("%f %f\n", i, newPos1);
d3alek 7:04ddad10a741 97 }
d3alek 7:04ddad10a741 98 position1 = newPos1;*/
d3alek 7:04ddad10a741 99 //servo1 = position1;
d3alek 7:04ddad10a741 100 //servo2 = position2;
d3alek 7:04ddad10a741 101 }
d3alek 7:04ddad10a741 102
d3alek 7:04ddad10a741 103 if (abs(position1 - newPos1) > servoSpeed) {
d3alek 7:04ddad10a741 104 dir = position1 < newPos1 ? 1 : -1;
d3alek 7:04ddad10a741 105 position1 += servoSpeed * dir;
d3alek 6:83c4801a027d 106 servo1 = position1;
d3alek 7:04ddad10a741 107 }
d3alek 7:04ddad10a741 108 else {
d3alek 7:04ddad10a741 109 position1 = newPos1;
d3alek 7:04ddad10a741 110 }
d3alek 7:04ddad10a741 111
d3alek 7:04ddad10a741 112 if (abs(position2 - newPos2) > servoSpeed) {
d3alek 7:04ddad10a741 113 dir = position2 < newPos2 ? 1 : -1;
d3alek 7:04ddad10a741 114 position2 += servoSpeed * dir;
d3alek 6:83c4801a027d 115 servo2 = position2;
d3alek 5:43d5529fbe1e 116 }
d3alek 7:04ddad10a741 117 else {
d3alek 7:04ddad10a741 118 position2 = newPos2;
d3alek 7:04ddad10a741 119 }
d3alek 5:43d5529fbe1e 120
d3alek 2:e35627187804 121 if (ENABLE_1) {
d3alek 2:e35627187804 122 s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
d3alek 2:e35627187804 123 s1c = sensor1.getc();
d3alek 2:e35627187804 124 }
d3alek 2:e35627187804 125 if (ENABLE_2) {
d3alek 2:e35627187804 126 s2bytenum = process_sensor_input(s2c, s2bytenum, s2bytes, 1);
d3alek 2:e35627187804 127 s2c = sensor2.getc();
d3alek 2:e35627187804 128 }
d3alek 6:83c4801a027d 129 if (ENABLE_3) {
d3alek 6:83c4801a027d 130 s3bytenum = process_sensor_input(s3c, s3bytenum, s3bytes, 2);
d3alek 6:83c4801a027d 131 s3c = sensor3.getc();
d3alek 6:83c4801a027d 132 }
d3alek 2:e35627187804 133 // TODO only prints when both are enabled now
d3alek 6:83c4801a027d 134 if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
d3alek 6:83c4801a027d 135 printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
d3alek 6:83c4801a027d 136 sensorXs[2], sensorYs[2]);
d3alek 6:83c4801a027d 137 sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
d3alek 6:83c4801a027d 138 sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
d3alek 0:94cffad90b69 139 }
d3alek 0:94cffad90b69 140 }
d3alek 0:94cffad90b69 141 }
d3alek 0:94cffad90b69 142
d3alek 8:41b35bda9d48 143 int process_sensor_input(int c, int bytenum, char* bytes, int ind)
d3alek 2:e35627187804 144 {
d3alek 8:41b35bda9d48 145 if (c < 0) {
d3alek 8:41b35bda9d48 146 //printf("%d: 255\n\r", ind);
d3alek 2:e35627187804 147 bytenum = -1;
d3alek 8:41b35bda9d48 148 } else if (bytenum % BYTES_NUM == 0) {
d3alek 2:e35627187804 149 bytes[0] = c;
d3alek 8:41b35bda9d48 150 if (!((c << 5) & 0x100)) {
d3alek 2:e35627187804 151 // not byte[0] wrong offset, skip c
d3alek 8:41b35bda9d48 152 //printf("%d: w %d\n\r", ind, c);
d3alek 2:e35627187804 153 bytenum = -1;
d3alek 0:94cffad90b69 154 }
d3alek 8:41b35bda9d48 155 } else if (bytenum % BYTES_NUM == 1) {
d3alek 2:e35627187804 156 bytes[1] = c;
d3alek 8:41b35bda9d48 157 } else if (bytenum % BYTES_NUM == 2) {
d3alek 2:e35627187804 158 bytes[2] = c;
d3alek 8:41b35bda9d48 159 //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);
d3alek 0:94cffad90b69 160
d3alek 2:e35627187804 161 //TODO: check for overflow
d3alek 2:e35627187804 162 if ((1 << 6) & bytes[0]) {
d3alek 8:41b35bda9d48 163 printf("Overflow x! %d\n\r", ind);
d3alek 3:eb3c3c9587d7 164 bytenum = -1;
d3alek 0:94cffad90b69 165 }
d3alek 3:eb3c3c9587d7 166 else if ((1 << 7) & bytes[0]) {
d3alek 8:41b35bda9d48 167 printf("Overflow y! %d\n\r", ind);
d3alek 3:eb3c3c9587d7 168 //printf("Byte1 is %d\n\r", bytes[0]);
d3alek 3:eb3c3c9587d7 169 bytenum = -1;
d3alek 0:94cffad90b69 170 }
d3alek 2:e35627187804 171 // check x and y signs
d3alek 3:eb3c3c9587d7 172 else {
d3alek 3:eb3c3c9587d7 173 int x = bytes[1] - ((bytes[0] << 4) & 0x100);
d3alek 3:eb3c3c9587d7 174 int y = bytes[2] - ((bytes[0] << 3) & 0x100);
d3alek 3:eb3c3c9587d7 175 //printf("%s: x = %d y = %d\n\r", id, x, y);
d3alek 3:eb3c3c9587d7 176 sensorXs[ind] = x;
d3alek 3:eb3c3c9587d7 177 sensorYs[ind] = y;
d3alek 3:eb3c3c9587d7 178 sensorToPrint[ind] = true;
d3alek 8:41b35bda9d48 179 //printf("%d ", ind);
d3alek 3:eb3c3c9587d7 180 bytenum = -1;
d3alek 3:eb3c3c9587d7 181 }
d3alek 0:94cffad90b69 182 }
d3alek 8:41b35bda9d48 183 return (bytenum + 1) % BYTES_NUM;
d3alek 0:94cffad90b69 184 }