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@0:94cffad90b69, 2013-10-20 (annotated)

Committer:
d3alek
Date:
Sun Oct 20 11:32:15 2013 +0000
Revision:
0:94cffad90b69
Child:
1:d290d6a34bef
debug version

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 0:94cffad90b69 4
d3alek 0:94cffad90b69 5 DigitalOut myled(LED1);
d3alek 0:94cffad90b69 6 DigitalInOut clk(p23);
d3alek 0:94cffad90b69 7 DigitalInOut dat(p22);
d3alek 0:94cffad90b69 8 Serial pc(USBTX, USBRX); // tx, rx
d3alek 0:94cffad90b69 9 static const int MAX_RETRY = 1000000;
d3alek 0:94cffad90b69 10 /*
d3alek 0:94cffad90b69 11 * 0xFF: Reset command.
d3alek 0:94cffad90b69 12 * 0xF3: Set sample rate.
d3alek 0:94cffad90b69 13 * 0xF2: Read device type.
d3alek 0:94cffad90b69 14 * 0xE8: Set resolution.
d3alek 0:94cffad90b69 15 * 0xE6: Set scaling.
d3alek 0:94cffad90b69 16 * 0xF4: Enable device.
d3alek 0:94cffad90b69 17 */
d3alek 0:94cffad90b69 18
d3alek 0:94cffad90b69 19 int send(uint8_t c);
d3alek 0:94cffad90b69 20 int recv();
d3alek 0:94cffad90b69 21
d3alek 0:94cffad90b69 22 PS2MS_INIT ps2ms_init(p23, p22);
d3alek 0:94cffad90b69 23 PS2MS ps2ms(p23, p22);
d3alek 0:94cffad90b69 24
d3alek 0:94cffad90b69 25 int main() {
d3alek 0:94cffad90b69 26 int bytenum = 0;
d3alek 0:94cffad90b69 27 char byte1, byte2, byte3;
d3alek 0:94cffad90b69 28 char c;
d3alek 0:94cffad90b69 29 while(1) {
d3alek 0:94cffad90b69 30 //char c = pc.getc();
d3alek 0:94cffad90b69 31 //send(c);
d3alek 0:94cffad90b69 32 c = ps2ms.getc();
d3alek 0:94cffad90b69 33 //TODO: c overflows, c >= 0 is always true, check c < 25? !
d3alek 0:94cffad90b69 34 while(c >= 0) {
d3alek 0:94cffad90b69 35 if (c == 255) {
d3alek 0:94cffad90b69 36 bytenum = 0;
d3alek 0:94cffad90b69 37 while (ps2ms.getc() >= 0) {};
d3alek 0:94cffad90b69 38 break;
d3alek 0:94cffad90b69 39 }
d3alek 0:94cffad90b69 40 if (bytenum % 3 == 0) {
d3alek 0:94cffad90b69 41 byte1 = c;
d3alek 0:94cffad90b69 42 // not byte1, wrong offset, clear remaining buffer
d3alek 0:94cffad90b69 43 if (!((1 << 3) & byte1)) {
d3alek 0:94cffad90b69 44 //while (ps2ms.getc() >= 0 && c != 255) {};
d3alek 0:94cffad90b69 45 ps2ms.init_work();
d3alek 0:94cffad90b69 46 bytenum = 0;
d3alek 0:94cffad90b69 47 c = ps2ms.getc();
d3alek 0:94cffad90b69 48 continue;
d3alek 0:94cffad90b69 49 }
d3alek 0:94cffad90b69 50 }
d3alek 0:94cffad90b69 51 else if (bytenum % 3 == 1) {
d3alek 0:94cffad90b69 52 byte2 = c;
d3alek 0:94cffad90b69 53 }
d3alek 0:94cffad90b69 54 else if (bytenum % 3 == 2) {
d3alek 0:94cffad90b69 55 byte3 = c;
d3alek 0:94cffad90b69 56
d3alek 0:94cffad90b69 57 //TODO: check for overflow
d3alek 0:94cffad90b69 58 if ((1 << 6) & byte1) {
d3alek 0:94cffad90b69 59 printf("Overflow x!\n\r");
d3alek 0:94cffad90b69 60 }
d3alek 0:94cffad90b69 61 if ((1 << 7) & byte1) {
d3alek 0:94cffad90b69 62 printf("Overflow y!\n\r");
d3alek 0:94cffad90b69 63 printf("Byte1 is %d\n\r", byte1);
d3alek 0:94cffad90b69 64 }
d3alek 0:94cffad90b69 65 // check x and y signs
d3alek 0:94cffad90b69 66 int x = ((1 << 4) & byte1) ? int(byte2) - 255 : int(byte2);
d3alek 0:94cffad90b69 67 int y = ((1 << 5) & byte1) ? int(byte3) - 255 : int(byte3);
d3alek 0:94cffad90b69 68 x = byte2 - ((byte1 << 4) & 0x100);
d3alek 0:94cffad90b69 69 y = byte3 - ((byte1 << 3) & 0x100);
d3alek 0:94cffad90b69 70 printf("x=%d y=%d\n\r", x, y);
d3alek 0:94cffad90b69 71 bytenum = 0;
d3alek 0:94cffad90b69 72 break;
d3alek 0:94cffad90b69 73 }
d3alek 0:94cffad90b69 74 bytenum = (bytenum + 1) % 3;
d3alek 0:94cffad90b69 75 c = ps2ms.getc();
d3alek 0:94cffad90b69 76 printf("c=%d butenum=%d\n\r", c, bytenum);
d3alek 0:94cffad90b69 77 }
d3alek 0:94cffad90b69 78 }
d3alek 0:94cffad90b69 79 }
d3alek 0:94cffad90b69 80 /**
d3alek 0:94cffad90b69 81 * Wait a clock down edge.
d3alek 0:94cffad90b69 82 *
d3alek 0:94cffad90b69 83 * @return true if wait done.
d3alek 0:94cffad90b69 84 */
d3alek 0:94cffad90b69 85 bool waitClockDownEdge(void) {
d3alek 0:94cffad90b69 86 int cnt;
d3alek 0:94cffad90b69 87 /*
d3alek 0:94cffad90b69 88 * Wait until clock is low.
d3alek 0:94cffad90b69 89 */
d3alek 0:94cffad90b69 90 cnt = 0;
d3alek 0:94cffad90b69 91 while (clk.read() == 0) {
d3alek 0:94cffad90b69 92 cnt++;
d3alek 0:94cffad90b69 93 if (MAX_RETRY < cnt) {
d3alek 0:94cffad90b69 94 return false;
d3alek 0:94cffad90b69 95 }
d3alek 0:94cffad90b69 96 wait_us(1);
d3alek 0:94cffad90b69 97 }
d3alek 0:94cffad90b69 98 /*
d3alek 0:94cffad90b69 99 * Wait until clock is high.
d3alek 0:94cffad90b69 100 */
d3alek 0:94cffad90b69 101 cnt = 0;
d3alek 0:94cffad90b69 102 while (clk.read() == 1) {
d3alek 0:94cffad90b69 103 cnt++;
d3alek 0:94cffad90b69 104 if (MAX_RETRY < cnt) {
d3alek 0:94cffad90b69 105 return false;
d3alek 0:94cffad90b69 106 }
d3alek 0:94cffad90b69 107 wait_us(1);
d3alek 0:94cffad90b69 108 }
d3alek 0:94cffad90b69 109 return true;
d3alek 0:94cffad90b69 110 }
d3alek 0:94cffad90b69 111
d3alek 0:94cffad90b69 112 /**
d3alek 0:94cffad90b69 113 * Wait a clock up level.
d3alek 0:94cffad90b69 114 *
d3alek 0:94cffad90b69 115 * @return true if wait done.
d3alek 0:94cffad90b69 116 */
d3alek 0:94cffad90b69 117 bool waitClockUpLevel(void) {
d3alek 0:94cffad90b69 118 int cnt;
d3alek 0:94cffad90b69 119 /*
d3alek 0:94cffad90b69 120 * Wait until clock is low.
d3alek 0:94cffad90b69 121 */
d3alek 0:94cffad90b69 122 cnt = 0;
d3alek 0:94cffad90b69 123 while (clk.read() == 0) {
d3alek 0:94cffad90b69 124 cnt++;
d3alek 0:94cffad90b69 125 if (MAX_RETRY < cnt) {
d3alek 0:94cffad90b69 126 return false;
d3alek 0:94cffad90b69 127 }
d3alek 0:94cffad90b69 128 wait_us(1);
d3alek 0:94cffad90b69 129 }
d3alek 0:94cffad90b69 130 return true;
d3alek 0:94cffad90b69 131 }
d3alek 0:94cffad90b69 132
d3alek 0:94cffad90b69 133 /**
d3alek 0:94cffad90b69 134 * Send a byte data.
d3alek 0:94cffad90b69 135 *
d3alek 0:94cffad90b69 136 * @param c a character.
d3alek 0:94cffad90b69 137 *
d3alek 0:94cffad90b69 138 * @return Negative value is a error number.
d3alek 0:94cffad90b69 139 */
d3alek 0:94cffad90b69 140 int send(uint8_t c) {
d3alek 0:94cffad90b69 141 clk.output();
d3alek 0:94cffad90b69 142 dat.output();
d3alek 0:94cffad90b69 143
d3alek 0:94cffad90b69 144 clk.write(0);
d3alek 0:94cffad90b69 145 wait_us(200);
d3alek 0:94cffad90b69 146
d3alek 0:94cffad90b69 147 dat.write(0);
d3alek 0:94cffad90b69 148 wait_us(10);
d3alek 0:94cffad90b69 149 clk.write(1);
d3alek 0:94cffad90b69 150 wait_us(10);
d3alek 0:94cffad90b69 151
d3alek 0:94cffad90b69 152 clk.input();
d3alek 0:94cffad90b69 153 int parcnt = 0;
d3alek 0:94cffad90b69 154 for (int i = 0; i < 10; i++) {
d3alek 0:94cffad90b69 155 if (!waitClockDownEdge()) {
d3alek 0:94cffad90b69 156 return -1;
d3alek 0:94cffad90b69 157 }
d3alek 0:94cffad90b69 158 if ((0 <= i) && (i <= 7)) {
d3alek 0:94cffad90b69 159 /*
d3alek 0:94cffad90b69 160 * Data bit.
d3alek 0:94cffad90b69 161 */
d3alek 0:94cffad90b69 162 if ((c & (1 << i)) == 0) {
d3alek 0:94cffad90b69 163 dat.write(0);
d3alek 0:94cffad90b69 164 } else {
d3alek 0:94cffad90b69 165 dat.write(1);
d3alek 0:94cffad90b69 166 parcnt++;
d3alek 0:94cffad90b69 167 }
d3alek 0:94cffad90b69 168 }
d3alek 0:94cffad90b69 169 if (i == 8) {
d3alek 0:94cffad90b69 170 /*
d3alek 0:94cffad90b69 171 * Parity bit.
d3alek 0:94cffad90b69 172 */
d3alek 0:94cffad90b69 173 if ((parcnt % 2) == 0) {
d3alek 0:94cffad90b69 174 dat.write(1);
d3alek 0:94cffad90b69 175 } else {
d3alek 0:94cffad90b69 176 dat.write(0);
d3alek 0:94cffad90b69 177 }
d3alek 0:94cffad90b69 178 }
d3alek 0:94cffad90b69 179 if (i == 9) {
d3alek 0:94cffad90b69 180 /*
d3alek 0:94cffad90b69 181 * Stop bit.
d3alek 0:94cffad90b69 182 */
d3alek 0:94cffad90b69 183 dat.write(1);
d3alek 0:94cffad90b69 184 }
d3alek 0:94cffad90b69 185 }
d3alek 0:94cffad90b69 186 dat.input();
d3alek 0:94cffad90b69 187
d3alek 0:94cffad90b69 188 /*
d3alek 0:94cffad90b69 189 * Check a ACK.
d3alek 0:94cffad90b69 190 */
d3alek 0:94cffad90b69 191 if (!waitClockDownEdge()) {
d3alek 0:94cffad90b69 192 return -2;
d3alek 0:94cffad90b69 193 }
d3alek 0:94cffad90b69 194 if (dat.read() != 0) {
d3alek 0:94cffad90b69 195 return -3;
d3alek 0:94cffad90b69 196 }
d3alek 0:94cffad90b69 197
d3alek 0:94cffad90b69 198 if (!waitClockUpLevel()) {
d3alek 0:94cffad90b69 199 return -4;
d3alek 0:94cffad90b69 200 }
d3alek 0:94cffad90b69 201
d3alek 0:94cffad90b69 202 return 0;
d3alek 0:94cffad90b69 203 }
d3alek 0:94cffad90b69 204
d3alek 0:94cffad90b69 205 /**
d3alek 0:94cffad90b69 206 * Receive a byte data.
d3alek 0:94cffad90b69 207 *
d3alek 0:94cffad90b69 208 * @return return a data. Negative value is a error number.
d3alek 0:94cffad90b69 209 */
d3alek 0:94cffad90b69 210 int recv(void) {
d3alek 0:94cffad90b69 211 uint8_t c = 0;
d3alek 0:94cffad90b69 212 clk.input();
d3alek 0:94cffad90b69 213 dat.input();
d3alek 0:94cffad90b69 214 int parcnt = 0;
d3alek 0:94cffad90b69 215 for (int i = 0; i < 11; i++) {
d3alek 0:94cffad90b69 216 if (!waitClockDownEdge()) {
d3alek 0:94cffad90b69 217 return -1;
d3alek 0:94cffad90b69 218 }
d3alek 0:94cffad90b69 219 if (i == 0) {
d3alek 0:94cffad90b69 220 /*
d3alek 0:94cffad90b69 221 * Start bit.
d3alek 0:94cffad90b69 222 */
d3alek 0:94cffad90b69 223 if (dat.read() != 0) {
d3alek 0:94cffad90b69 224 return -2;
d3alek 0:94cffad90b69 225 }
d3alek 0:94cffad90b69 226 }
d3alek 0:94cffad90b69 227 if ((1 <= i) && (i <= 8)) {
d3alek 0:94cffad90b69 228 /*
d3alek 0:94cffad90b69 229 * Data bit.
d3alek 0:94cffad90b69 230 */
d3alek 0:94cffad90b69 231 if (dat.read() == 0) {
d3alek 0:94cffad90b69 232 c &= ~(1 << (i - 1));
d3alek 0:94cffad90b69 233 } else {
d3alek 0:94cffad90b69 234 c |= (1 << (i - 1));
d3alek 0:94cffad90b69 235 parcnt++;
d3alek 0:94cffad90b69 236 }
d3alek 0:94cffad90b69 237 }
d3alek 0:94cffad90b69 238 if (i == 9) {
d3alek 0:94cffad90b69 239 /*
d3alek 0:94cffad90b69 240 * Parity bit.
d3alek 0:94cffad90b69 241 */
d3alek 0:94cffad90b69 242 if (dat.read() == 0) {
d3alek 0:94cffad90b69 243 if ((parcnt % 2) != 1) {
d3alek 0:94cffad90b69 244 return -3;
d3alek 0:94cffad90b69 245 }
d3alek 0:94cffad90b69 246 } else {
d3alek 0:94cffad90b69 247 if ((parcnt % 2) != 0) {
d3alek 0:94cffad90b69 248 return -4;
d3alek 0:94cffad90b69 249 }
d3alek 0:94cffad90b69 250 }
d3alek 0:94cffad90b69 251 }
d3alek 0:94cffad90b69 252 if (i == 10) {
d3alek 0:94cffad90b69 253 /*
d3alek 0:94cffad90b69 254 * Stop bit.
d3alek 0:94cffad90b69 255 */
d3alek 0:94cffad90b69 256 if (dat.read() != 1) {
d3alek 0:94cffad90b69 257 return -5;
d3alek 0:94cffad90b69 258 }
d3alek 0:94cffad90b69 259 }
d3alek 0:94cffad90b69 260 }
d3alek 0:94cffad90b69 261 return (int)c;
d3alek 0:94cffad90b69 262 }
d3alek 0:94cffad90b69 263
d3alek 0:94cffad90b69 264