File content as of revision 10:37e7c46837dc:

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

#define SENSORS_NUM 3
#define BYTES_NUM 3

#define DEBUG false

#define MAX_OVERFLOWS 3

#define MAX_REPLY_ERRORS 3

Servo servoYaw(p21);
Servo servoPitch(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* sensor[3];
PS2MS_INIT* sensor_init[3];


int process_sensor_input(int c, int bytenum, char* bytes, int ind);
bool processACKReply(int ind);
void sendError(int ind);
bool getPacket(int ind);
bool getMovementPacket(int ind);
void sendResend(int ind);
void sendError(int ind);
int sendCommand(int ind, char command);
void processSerial();

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

char last_command[3];

bool expectingAck1 = false, expectingAck2 = false, expectingAck3 = false;

int replyErrors[3];

static const int SERVO_YAW = 0;
static const int SERVO_PITCH = 1;
float servoPos[2];
float servoAdj[2] = {0.03, 0.03};
bool mCommandCompleted[2] = {true, true};

int main()
{
    sensor_init[0] = new PS2MS_INIT(p18, p17);
    sensor_init[1] = new PS2MS_INIT(p23, p22);
    sensor_init[2] = new PS2MS_INIT(p26, p25);
    sensor[0] = new PS2MS(p18, p17);
    sensor[1] = new PS2MS(p23, p22);
    sensor[2] = new PS2MS(p26, p25);

    //TODO: receive all pending packets here

    float range = 0.00085;
    float position1 = 0.5;
    float position2 = 0.5;

    servoPos[SERVO_YAW] = position1 + servoAdj[SERVO_YAW];
    servoPos[SERVO_PITCH] = position2 + servoAdj[SERVO_PITCH];

    replyErrors[0] = replyErrors[1] = replyErrors[2] = 0;

    servoYaw.calibrate(range, 45.0);
    servoPitch.calibrate(range, 45.0);
    servoYaw = servoPos[SERVO_YAW];
    servoPitch = servoPos[SERVO_PITCH];
    printf("position = %.3f, range = +/-%0.5f\n", position1, range);

    printf("IMHERE START\n");

    sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
    int dir;
    float servoSpeed = 0.002;

    bool awaitingPackets = false;

    while(1) {
        if (pc.readable()) {
            processSerial();
        }

        if (abs(position1 - servoPos[SERVO_YAW]) > servoSpeed) {
            dir = position1 < servoPos[SERVO_YAW] ? 1 : -1;
            position1 += servoSpeed * dir;
            servoYaw = position1;
        } else {
            if (mCommandCompleted[SERVO_YAW] == false) {
                printf("Command completed\n\r");
                mCommandCompleted[SERVO_YAW] = true;
            }
            position1 = servoPos[SERVO_YAW];
        }

        if (abs(position2 - servoPos[SERVO_PITCH]) > servoSpeed) {
            dir = position2 < servoPos[SERVO_PITCH] ? 1 : -1;
            position2 += servoSpeed * dir;
            servoPitch = position2;
        } else {
            if (mCommandCompleted[SERVO_PITCH] == false) {
                printf("Command completed\n\r");
                mCommandCompleted[SERVO_PITCH] = true;
            }
            position2 = servoPos[SERVO_PITCH];
        }
        int res;
        if (!awaitingPackets) {
            //TODO: check for errors on send
            res = sendCommand(0, '\xEB');

            if (res) {
                if (DEBUG) {
                    printf("%d: send error %d\n\r", 0, res);
                }
                res = sendCommand(0, '\xEB');
                if (DEBUG) {
                    printf("%d: two failed sends %d\n\r", 0, res);
                }
            }
            expectingAck1 = true;
            res = sendCommand(1, '\xEB');
            if (res) {
                if (DEBUG) {
                    printf("%d: send error %d\n\r", 1, res);
                }
                res = sendCommand(1, '\xEB');
                if (DEBUG) {
                    printf("%d: two failed sends %d\n\r", 1, res);
                }
            }
            expectingAck2 = true;
            res = sendCommand(2, '\xEB');
            if (res) {
                if (DEBUG) {
                    printf("%d: send error %d\n\r", 2, res);
                }
                res = sendCommand(2, '\xEB');
                if (res) {
                    if (DEBUG) {
                        printf("%d: two failed sends %d\n\r", 2, res);
                    }
                }
            }
            expectingAck3 = true;
            awaitingPackets = true;
        }

        if (expectingAck1) {
            expectingAck1 = !getPacket(0);
        } else if (expectingAck2) {
            expectingAck2 = !getPacket(1);
        } else if (expectingAck3) {
            expectingAck3 = !getPacket(2);
        }
        // TODO only prints when both are enabled now
        if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
            if ((sensorXs[0] | sensorYs[0] | sensorXs[1] | sensorYs[1] | sensorXs[2] | sensorYs[2]) ) {
                // some of the velocities are not 0
                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;
            awaitingPackets = false;
        }
    }\
    /*
    TODO: Deallocate those somewhere

    delete[] sensor_init[0];
    delete[] sensor_init[1];
    delete[] sensor_init[2];
    delete[] sensor[0];
    delete[] sensor[1];
    delete[] sensor[2];
    */
}

#define MAX_ANGLE_STR_SIZE 100
char rotateAngleStr[MAX_ANGLE_STR_SIZE];

void processSerial()
{
    char c = pc.getc();
    if (c <= '9' && c >= '0') {
        int servoNum = int(c - '0');
        if (servoNum == SERVO_YAW || servoNum == SERVO_PITCH) {
             pc.gets(rotateAngleStr, MAX_ANGLE_STR_SIZE);
             
             double rotateAngleNum = atoi(rotateAngleStr);
             
             if (rotateAngleNum > 90 || rotateAngleNum < -90) {
                 return;
             }
             rotateAngleNum = 0.5 + rotateAngleNum / 90.;
             servoPos[servoNum] = rotateAngleNum + servoAdj[servoNum];
             mCommandCompleted[servoNum] = false;
        }
    }
}

int sendCommand(int ind, char command)
{
    int res;
    last_command[ind] = command;
    //res = sensor_init[ind]->send(command);
    res = sensor[ind]->sendCommand(command);
    //__enable_irq();
    if (res) {
        if (DEBUG) printf("error sending command %#x to %d\n\r", command, ind);
    }
    return res;
}

bool getPacket(int ind)
{
    bool successful = processACKReply(ind);
    if (!successful) {
        sendResend(ind);
        successful = processACKReply(ind);
        if (!successful) {
            printf("DUNNO WHAT TO DO ACK Reply %d\n\r", ind);
            //wait(1);
            sendCommand(ind, '\xEB');
            return getPacket(ind);
        }
    }
    successful = getMovementPacket(ind);

    if (!successful) {
        printf("DISCARDING PACKET %d\n\r", ind);
        if (DEBUG) {
            sendCommand(ind, '\xEB');
            //wait(1);
        }
        return false;
    }
    return true;
}

void sendError(int ind)
{
    sendCommand(ind, '\xFC');
}

void sendResend(int ind)
{
    sendCommand(ind, '\xFE');
}

bool getMovementPacket(int ind)
{
    char bytes[3];
    int c;
    for (int i = 0; i < 3; ++i) {
        c = sensor[ind]->getc();
        if (c < 0) {
            //printf("%d: 255\n\r", ind);
            return false;
        } else if (i == 0) {
            bytes[0] = c;
            if (!((c << 5) & 0x100)) {
                // not byte[0] wrong offset, skip e
                if (DEBUG) printf("%d: w %d ", ind, c);
                c = sensor[ind]->getc();
                while (c >= 0) {
                    printf("%d ", c);
                    c = sensor[ind]->getc();
                }
                printf("\n\r");
                return false;
            }
        } else if (i == 1) {
            bytes[1] = c;
        } else if (i == 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("%d: Overflow x %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
                return false;
            } else if ((1 << 7) & bytes[0]) {
                printf("%d: Overflow y %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
                return false;
            }
            // 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);
            }
        }
    }
    return true;
}

bool processACKReply(int ind)
{
    int reply = sensor[ind]->getc();
    if (reply < 0) {
        if (DEBUG) printf("%d: Error %d", ind, reply);
        return false;
    } else if (reply == '\xFA') {
        //if (DEBUG) printf("%d: ACK ", ind);
        return true;
    } else if (reply == '\xEB') {
        if (DEBUG) printf("%d: READ_DATA ", ind);
        //TODO: inf loop possible here cause recursion
        return processACKReply(ind);
    } else if (reply == '\xFE') {
        if (last_command[ind] == '\xFE') {
            if (DEBUG) printf("%d: REPEATING_COMMAND BUT REPEAT_COMMAND!", ind, last_command[ind]);
            //wait(1);
            sendCommand(ind, '\xEB');
            return processACKReply(ind);
        }
        // repeat command
        if (DEBUG) printf("%d: REPEATING_COMMAND %#x", ind, last_command[ind]);
        //wait(1);
        sendCommand(ind, last_command[ind]);
        return processACKReply(ind);
    } else if (reply == '\xF2') {
        // get device ID??
        if (DEBUG) printf("%d: GET_DEVICE_ID %#x", ind, reply);
        //wait(1);
        return processACKReply(ind);
    } else {
        if (DEBUG) printf("%d: Unexpected ACK Reply %d\n\r", ind, reply);
        //wait(1);
        return processACKReply(ind);
    }
}
/*
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
            if (DEBUG) printf("%d: w %d\n\r", ind, c);
            bytenum = -1;
            sendError(ind);
        }
    } 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("%d: Overflow x %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
            if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
                sendError(ind);
            } else {
                consecutiveOverflows[ind] = 0;
            }
            bytenum = -1;
        } else if ((1 << 7) & bytes[0]) {
            printf("%d: Overflow y %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
            if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
                sendError(ind);
            } else {
                consecutiveOverflows[ind] = 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;
}
*/
/*
void sendError(int ind)
{
    switch (ind) {
        case 0:
            sensor1_init.send('\xFE');
            expectingAck1 = true;
            break;
        case 1:
            sensor2_init.send('\xFE');
            expectingAck2 = true;
            break;
        case 2:
            sensor3_init.send('\xFE');
            expectingAck3 = true;
            break;
    }
}*/