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@6:83c4801a027d, 2014-03-04 (annotated)

Committer:
d3alek
Date:
Tue Mar 04 13:28:04 2014 +0000
Revision:
6:83c4801a027d
Parent:
5:43d5529fbe1e
Child:
7:04ddad10a741
3 sensors 2 servos but fast

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