File content as of revision 8:41b35bda9d48:

#include "PS2MS.h"
#include "PS2MS_INIT.h"
#include "mbed.h"
#include "Servo.h"

#define ENABLE_1 true
#define ENABLE_2 true
#define ENABLE_3 true

#define SENSORS_NUM 3
#define BYTES_NUM 3

Servo servo1(p21);
Servo servo2(p24);

DigitalOut myled(LED1);
Serial pc(USBTX, USBRX); // tx, rx
/*
 * 0xFF: Reset command.
 * 0xF3: Set sample rate.
 * 0xF2: Read device type.
 * 0xE8: Set resolution.
 * 0xE6: Set scaling.
 * 0xF4: Enable device.
 */

//TODO should Iuse sensor1_init? maybe no 255s?
PS2MS_INIT sensor1_init(p18, p17);
PS2MS sensor1(p18, p17);

PS2MS_INIT sensor2_init(p23, p22);
PS2MS sensor2(p23, p22);

PS2MS_INIT sensor3_init(p26, p25);
PS2MS sensor3(p26, p25);

int process_sensor_input(int c, int bytenum, char* bytes, int ind);

int sensorXs[SENSORS_NUM];
int sensorYs[SENSORS_NUM];
bool sensorToPrint[SENSORS_NUM];

int main()
{

    float range = 0.00085;
    float position1 = 0.5;
    float position2 = 0.5;
    float position_adj = 0.03;
    
    float newPos1 = position1, newPos2 = position2;
    
    servo1.calibrate(range, 45.0); 
    servo2.calibrate(range, 45.0); 
    servo1 = position1;
    servo2 = position2;
    printf("position = %.3f, range = +/-%0.5f\n", position1, range, position_adj);

    printf("IMHERE START\n");
    int s1bytenum = 0;
    char s1bytes[BYTES_NUM];

    int s2bytenum = 0;
    char s2bytes[BYTES_NUM];
    
    int s3bytenum = 0;
    char s3bytes[BYTES_NUM];

    int s1c, s2c, s3c;
    printf("IMHERE GET SENSORS\n");
    s1c = sensor1.getc();
    printf("IMHERE GOT S1\n");
    s2c = sensor2.getc();
    printf("IMHERE GOT S2\n");
    s3c = sensor2.getc();
    printf("IMHERE GOT S3\n");
    sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
    int dir;
    float servoSpeed = 0.001;
    
    while(1) {
        if (pc.readable()) {
            switch(pc.getc()) {
            case '1': newPos1 = 0.0 + position_adj; break; //position1 = 0.0 + position_adj; break;
            case '2': newPos1 = 0.5 + position_adj; break; //position1 = 0.5 + position_adj; break;
            case '3': newPos1 = 0.6 + position_adj; break; //position1 = 1.0 + position_adj; break;
            case '4': newPos2 = 0.3 + position_adj; break;
            case '5': newPos2 = 0.5 + position_adj; break;
            case '6': newPos2 = 0.7 + position_adj; break;
            }
            //printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
            //dir = position1 < newPos1 ? 1 : -1;
            /*printf("IMHERE %f %f %d\n", position1, newPos1, dir);
            for (float i = position1; abs(i - newPos1) > servoSpeed; i += servoSpeed * dir) {
                servo1 = i;
                printf("%f %f\n", i, newPos1);
            }
            position1 = newPos1;*/
            //servo1 = position1;
            //servo2 = position2;
        }
        
        if (abs(position1 - newPos1) > servoSpeed) {
            dir = position1 < newPos1 ? 1 : -1;
            position1 += servoSpeed * dir;
            servo1 = position1;
        }
        else {
            position1 = newPos1;
        }
        
        if (abs(position2 - newPos2) > servoSpeed) {
            dir = position2 < newPos2 ? 1 : -1;
            position2 += servoSpeed * dir;
            servo2 = position2;
        }
        else {
            position2 = newPos2;
        }
    
        if (ENABLE_1) {
            s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
            s1c = sensor1.getc();
        }
        if (ENABLE_2) {
            s2bytenum = process_sensor_input(s2c, s2bytenum, s2bytes, 1);
            s2c = sensor2.getc();
        }
        if (ENABLE_3) {
            s3bytenum = process_sensor_input(s3c, s3bytenum, s3bytes, 2);
            s3c = sensor3.getc();
        }
        // TODO only prints when both are enabled now
        if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
            printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
                sensorXs[2], sensorYs[2]);
            sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
            sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
        }
    }
}

int process_sensor_input(int c, int bytenum, char* bytes, int ind)
{
    if (c < 0) {
        //printf("%d: 255\n\r", ind);
        bytenum = -1;
    } else if (bytenum % BYTES_NUM == 0) {
        bytes[0] = c;
        if (!((c << 5) & 0x100)) {
            // not byte[0] wrong offset, skip c
            //printf("%d: w %d\n\r", ind, c);
            bytenum = -1;
        }
    } else if (bytenum % BYTES_NUM == 1) {
        bytes[1] = c;
    } else if (bytenum % BYTES_NUM == 2) {
        bytes[2] = c;
        //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);

        //TODO: check for overflow
        if ((1 << 6) & bytes[0]) {
            printf("Overflow x! %d\n\r", ind);
            bytenum = -1;
        }
        else if ((1 << 7) & bytes[0]) {
            printf("Overflow y! %d\n\r", ind);
            //printf("Byte1 is %d\n\r", bytes[0]);
            bytenum = -1;   
        }
        // check x and y signs
        else {
            int x = bytes[1] - ((bytes[0] << 4) & 0x100);
            int y = bytes[2] - ((bytes[0] << 3) & 0x100);
            //printf("%s: x = %d   y = %d\n\r", id, x, y);
            sensorXs[ind] = x;
            sensorYs[ind] = y;
            sensorToPrint[ind] = true;
            //printf("%d        ", ind);
            bytenum = -1;
        }
    }
    return (bytenum + 1) % BYTES_NUM;
}