File content as of revision 13:582064cfb9b2:

#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);

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];

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.097, 0.024}; // 0.097, 0.024
bool mCommandCompleted[2] = {true, true};

const float SERVO_SPEED_ADJ = 0.001;

float servoSpeed = 1 * SERVO_SPEED_ADJ;

Timer timer;

int mDiscardedCount[3] = {0, 0, 0};
const int MAX_DISCARDED = 3;

PinName sensorPorts[3][2] = {
    {p18, p17}, // sensor 0
    {p15, p14}, // sensor 1
    {p26, p25} // sensor 2
};

void initializeSensor(int ind) {
    PS2MS_INIT s1(sensorPorts[ind][0], sensorPorts[ind][1]);
    printf("SENSOR_INIT % DONE on ports %d and %d\n\r", ind, sensorPorts[ind][0], sensorPorts[ind][1]);
    sensor[ind] = new PS2MS(sensorPorts[ind][0], sensorPorts[ind][1]);
}

bool notTooFarOff() {
    int maxAbs1 = abs(sensorXs[0]) > abs(sensorYs[0]) ? abs(sensorXs[0]) : abs(sensorYs[0]);
    int maxAbs2 = abs(sensorXs[1]) > abs(sensorYs[1]) ? abs(sensorXs[1]) : abs(sensorYs[1]);
    int maxAbs3 = abs(sensorXs[2]) > abs(sensorYs[2]) ? abs(sensorXs[2]) : abs(sensorYs[1]);
    
    if (maxAbs1 == 0) maxAbs1 = 1;
    if (maxAbs2 == 0) maxAbs2 = 1;
    if (maxAbs3 == 0) maxAbs3 = 1;
    
    if (maxAbs1 > maxAbs2 * 20) {
        return false;
    }
    
    if (maxAbs2 > maxAbs3 * 20) {
        return false;
    }
    
    if (maxAbs3 > maxAbs1 * 20) {
        return false;
    }
    
    return true;
}

//TODO switch to Tickers instead of checking these in a while loop, much cleaner
long lastTickMs;
long lastSendMs;
const int TICK_EVERY_MS = 10;
const int SEND_EVERY_MS = 30;

int main()
{
    printf("MAIN START\n\r");
    initializeSensor(0);
    initializeSensor(1);
    initializeSensor(2);
    //TODO: receive all pending packets here

    timer.start();

    float range = 0.00084;
    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];

    sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
    int dir;

    bool awaitingPackets = false;
    
    lastTickMs = 0;
    lastSendMs = 0;
    
    int mTooFarOffFor = 0;
    const int MAX_TOO_FAR_OFF_COUNT = 2;
    
    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 %d\n\r", timer.read_ms());
                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 %d\n\r", timer.read_ms());
                mCommandCompleted[SERVO_PITCH] = true;
            }
            position2 = servoPos[SERVO_PITCH];
        }
        
        if (lastSendMs + SEND_EVERY_MS <= timer.read_ms()) {
            //printf("SEND %d\n\r", timer.read_ms());
            lastSendMs = timer.read_ms();
            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
                if (notTooFarOff()) {
                    printf("%d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
                       sensorXs[2], sensorYs[2]);
                }
                else {
                    mTooFarOffFor++;
                    if (mTooFarOffFor < MAX_TOO_FAR_OFF_COUNT) {
                        printf("Too far off %d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
                           sensorXs[2], sensorYs[2]);
                        sendResend(0); expectingAck1 = true;
                        sendResend(1); expectingAck2 = true;
                        sendResend(2); expectingAck3 = true;
                        continue;
                    }
                    else {
                        printf("Ignoring a TooFarOff packet %d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
                           sensorXs[2], sensorYs[2]);
                    }
                }
            }
            mTooFarOffFor = 0;
            sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
            sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
            awaitingPackets = false;
        }
        while (lastTickMs + TICK_EVERY_MS > timer.read_ms()) {
            wait_ms(1);
        }
        lastTickMs = timer.read_ms();
        //printf("TICK %d\n\r", timer.read_ms());
    }
    /*
    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

void processSerial()
{
    char c = pc.getc();
    if (c <= '9' && c >= '0') {
        int servoNum = int(c - '0');
        if (servoNum == SERVO_YAW || servoNum == SERVO_PITCH) {
             char rotateAngleStr[MAX_ANGLE_STR_SIZE];
             pc.gets(rotateAngleStr, MAX_ANGLE_STR_SIZE);
             
             double rotateAngleNum = atoi(rotateAngleStr);
             if (rotateAngleNum > 90 || rotateAngleNum < -90) {
                 return;
             }
             rotateAngleNum = 0.5 + rotateAngleNum / 180.;
             servoPos[servoNum] = rotateAngleNum + servoAdj[servoNum];
             mCommandCompleted[servoNum] = false;
             printf("Command started %d\n\r", timer.read_ms());
        }
    }
    else if (c == 's') {
        char buffer[100];
        pc.gets(buffer, 100);
        int speedStep = atoi(buffer);
        servoSpeed = speedStep * SERVO_SPEED_ADJ;
        printf("Servo speed set to %d\n\r", speedStep);
    }
}

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) {
            if (DEBUG) 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);
        mDiscardedCount[ind]++;
        if (mDiscardedCount[ind] > MAX_DISCARDED) {
            initializeSensor(ind);
        }
        //if (DEBUG) {
        sendCommand(ind, '\xEB');
        //}
        return false;
    }
    mDiscardedCount[ind] = 0;
    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
                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;
    }
}*/