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@5:43d5529fbe1e, 2014-03-04 (annotated)

Committer:
d3alek
Date:
Tue Mar 04 12:06:27 2014 +0000
Revision:
5:43d5529fbe1e
Parent:
4:73f9fd797965
Child:
6:83c4801a027d
2 working

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 2:e35627187804 8
d3alek 5:43d5529fbe1e 9 Servo myservo(p21);
d3alek 5:43d5529fbe1e 10
d3alek 0:94cffad90b69 11 DigitalOut myled(LED1);
d3alek 0:94cffad90b69 12 DigitalInOut clk(p23);
d3alek 0:94cffad90b69 13 DigitalInOut dat(p22);
d3alek 0:94cffad90b69 14 Serial pc(USBTX, USBRX); // tx, rx
d3alek 2:e35627187804 15 /*
d3alek 2:e35627187804 16 * 0xFF: Reset command.
d3alek 2:e35627187804 17 * 0xF3: Set sample rate.
d3alek 2:e35627187804 18 * 0xF2: Read device type.
d3alek 2:e35627187804 19 * 0xE8: Set resolution.
d3alek 2:e35627187804 20 * 0xE6: Set scaling.
d3alek 2:e35627187804 21 * 0xF4: Enable device.
d3alek 2:e35627187804 22 */
d3alek 0:94cffad90b69 23
d3alek 0:94cffad90b69 24 int send(uint8_t c);
d3alek 0:94cffad90b69 25 int recv();
d3alek 0:94cffad90b69 26
d3alek 2:e35627187804 27 //TODO should Iuse sensor1_init? maybe no 255s?
d3alek 2:e35627187804 28 PS2MS_INIT sensor1_init(p23, p22);
d3alek 2:e35627187804 29 PS2MS sensor1(p23, p22);
d3alek 2:e35627187804 30
d3alek 2:e35627187804 31 PS2MS_INIT sensor2_init(p26, p25);
d3alek 2:e35627187804 32 PS2MS sensor2(p26, p25);
d3alek 2:e35627187804 33
d3alek 2:e35627187804 34 int process_sensor_input(char c, int bytenum, char* bytes, int ind);
d3alek 2:e35627187804 35
d3alek 2:e35627187804 36 int sensorXs[2];
d3alek 2:e35627187804 37 int sensorYs[2];
d3alek 2:e35627187804 38 bool sensorToPrint[2];
d3alek 0:94cffad90b69 39
d3alek 2:e35627187804 40 int main()
d3alek 2:e35627187804 41 {
d3alek 5:43d5529fbe1e 42
d3alek 5:43d5529fbe1e 43 float range = 0.00085;
d3alek 5:43d5529fbe1e 44 float position = 0.5;
d3alek 5:43d5529fbe1e 45 float position_adj = 0.03;
d3alek 5:43d5529fbe1e 46
d3alek 5:43d5529fbe1e 47 myservo.calibrate(range, 45.0);
d3alek 5:43d5529fbe1e 48 myservo = position;
d3alek 5:43d5529fbe1e 49 printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
d3alek 5:43d5529fbe1e 50
d3alek 4:73f9fd797965 51 printf("IMHERE START\n");
d3alek 2:e35627187804 52 int s1bytenum = 0;
d3alek 2:e35627187804 53 char s1bytes[3];
d3alek 2:e35627187804 54
d3alek 2:e35627187804 55 int s2bytenum = 0;
d3alek 2:e35627187804 56 char s2bytes[3];
d3alek 2:e35627187804 57
d3alek 2:e35627187804 58 char s1c, s2c;
d3alek 4:73f9fd797965 59 printf("IMHERE GET SENSORS\n");
d3alek 2:e35627187804 60 s1c = sensor1.getc();
d3alek 4:73f9fd797965 61 printf("IMHERE GOT S1\n");
d3alek 2:e35627187804 62 s2c = sensor2.getc();
d3alek 4:73f9fd797965 63 printf("IMHERE GOT S2\n");
d3alek 2:e35627187804 64 sensorToPrint[0] = sensorToPrint[1] = false;
d3alek 2:e35627187804 65
d3alek 0:94cffad90b69 66 while(1) {
d3alek 5:43d5529fbe1e 67 if (pc.readable()) {
d3alek 5:43d5529fbe1e 68 switch(pc.getc()) {
d3alek 5:43d5529fbe1e 69 case '1': position = 0.0 + position_adj; break;
d3alek 5:43d5529fbe1e 70 case '2': position = 0.5 + position_adj; break;
d3alek 5:43d5529fbe1e 71 case '3': position = 1.0 + position_adj; break;
d3alek 5:43d5529fbe1e 72 }
d3alek 5:43d5529fbe1e 73 //printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
d3alek 5:43d5529fbe1e 74 myservo = position;
d3alek 5:43d5529fbe1e 75 }
d3alek 5:43d5529fbe1e 76
d3alek 2:e35627187804 77 if (ENABLE_1) {
d3alek 2:e35627187804 78 s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
d3alek 2:e35627187804 79 s1c = sensor1.getc();
d3alek 2:e35627187804 80 }
d3alek 2:e35627187804 81 if (ENABLE_2) {
d3alek 2:e35627187804 82 s2bytenum = process_sensor_input(s2c, s2bytenum, s2bytes, 1);
d3alek 2:e35627187804 83 s2c = sensor2.getc();
d3alek 2:e35627187804 84 }
d3alek 2:e35627187804 85 // TODO only prints when both are enabled now
d3alek 4:73f9fd797965 86 if (sensorToPrint[0] && sensorToPrint[1]) {
d3alek 2:e35627187804 87 printf("%d : %d %d %d %d\n\r", 2, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1]);
d3alek 2:e35627187804 88 sensorToPrint[0] = sensorToPrint[1] = false;
d3alek 2:e35627187804 89 sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = 0;
d3alek 0:94cffad90b69 90 }
d3alek 0:94cffad90b69 91 }
d3alek 0:94cffad90b69 92 }
d3alek 0:94cffad90b69 93
d3alek 2:e35627187804 94 int process_sensor_input(char c, int bytenum, char* bytes, int ind)
d3alek 2:e35627187804 95 {
d3alek 2:e35627187804 96 if (c == 255) {
d3alek 2:e35627187804 97 bytenum = -1;
d3alek 2:e35627187804 98 } else if (bytenum % 3 == 0) {
d3alek 2:e35627187804 99 bytes[0] = c;
d3alek 2:e35627187804 100 if (!((1 << 3) & c)) {
d3alek 2:e35627187804 101 // not byte[0] wrong offset, skip c
d3alek 2:e35627187804 102 bytenum = -1;
d3alek 0:94cffad90b69 103 }
d3alek 2:e35627187804 104 } else if (bytenum % 3 == 1) {
d3alek 2:e35627187804 105 bytes[1] = c;
d3alek 2:e35627187804 106 } else if (bytenum % 3 == 2) {
d3alek 2:e35627187804 107 bytes[2] = c;
d3alek 0:94cffad90b69 108
d3alek 2:e35627187804 109 //TODO: check for overflow
d3alek 2:e35627187804 110 if ((1 << 6) & bytes[0]) {
d3alek 3:eb3c3c9587d7 111 //printf("Overflow x!\n\r");
d3alek 3:eb3c3c9587d7 112 bytenum = -1;
d3alek 0:94cffad90b69 113 }
d3alek 3:eb3c3c9587d7 114 else if ((1 << 7) & bytes[0]) {
d3alek 3:eb3c3c9587d7 115 //printf("Overflow y!\n\r");
d3alek 3:eb3c3c9587d7 116 //printf("Byte1 is %d\n\r", bytes[0]);
d3alek 3:eb3c3c9587d7 117 bytenum = -1;
d3alek 0:94cffad90b69 118 }
d3alek 2:e35627187804 119 // check x and y signs
d3alek 3:eb3c3c9587d7 120 else {
d3alek 3:eb3c3c9587d7 121 int x = bytes[1] - ((bytes[0] << 4) & 0x100);
d3alek 3:eb3c3c9587d7 122 int y = bytes[2] - ((bytes[0] << 3) & 0x100);
d3alek 3:eb3c3c9587d7 123 //printf("%s: x = %d y = %d\n\r", id, x, y);
d3alek 3:eb3c3c9587d7 124 sensorXs[ind] = x;
d3alek 3:eb3c3c9587d7 125 sensorYs[ind] = y;
d3alek 3:eb3c3c9587d7 126 sensorToPrint[ind] = true;
d3alek 3:eb3c3c9587d7 127 bytenum = -1;
d3alek 3:eb3c3c9587d7 128 }
d3alek 0:94cffad90b69 129 }
d3alek 2:e35627187804 130 return (bytenum + 1) % 3;
d3alek 0:94cffad90b69 131 }