Altino_180728

autonomous.ino

#include <Altino.h>

#define FWD 450
#define BWD -525

#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
#define MAX(X, Y) (((X) > (Y)) ? (X) : (Y))

SensorData sdata;
int ir[6];
int cds;

int runningTime = 0;
int processTime[3];
int process = 0;
int cnt = 0;

void pl(int sound, double t, int pause);
void Auto(int ir[6]);
void Park(int ir[6], int cds);

void setup() {
    Serial.begin(115200);
}

void loop() {
    Sensor(10, &sdata);
    
    for (int i = 0; i < 6; i++) {
        ir[i] = sdata.IRSensor[i];
        if (ir[i] <= 0) ir[i] = 0;
        Serial.print("IR[");
        Serial.print(i);
        Serial.print("]: ");
        Serial.print(ir[i]);
        Serial.print(" ");
    }
    cds = sdata.CDSSensor;
    Serial.print("CDS: ");
    Serial.print(cds);

    runningTime++;

    if (process == 0) {
        if (ir[3] >= 400 && cnt < 2) {
            cnt++;
            pl(53, 1, 400);
        }
        else if (cnt == 2) {
            pl(56, 1, 400);
            process = 1;
        }
    }
    else if (process == 1) {
        Auto(ir);
    }
}

void pl(int sound, double t, int pause) {
    Sound(sound);
    delay(((int) (500 * t)) - pause);
    Sound(0);
    delay(pause);
}

void Auto(int ir[6]) {
    int lsp, rsp, st;

    lsp = FWD;
    rsp = FWD;
    st = 0;

    if (ir[0] > 15 && ir[1] > 15 && ir[2] > 15) {
        lsp = BWD;
        rsp = BWD;
        st = (ir[2] - ir[0]) * 2.4;
    }
    else if (ir[0] > 15 && ir[1] > 15) {
        lsp = BWD;
        rsp = BWD;
        st = ir[0] - ir[1];
        lsp += st;
    }
    else if (ir[1] > 15 && ir[2] > 15) {
        lsp = BWD;
        rsp = BWD;
        st = ir[2] - ir[1];
        rsp -= st;
    }
    else if (ir[0] > 25) {
        lsp = BWD;
        rsp = BWD;
        st = -127;
    }
    else if (ir[2] > 25) {
        lsp = BWD;
        rsp = BWD;
        st = 128;
    }
    else {
        st = (ir[3] - ir[4]) * 1.2;
        if (st > 0 && ir[2] > ir[0]) {
            st *= -1;
        }
        else if (st < 0 && ir[2] < ir[0]) {
            st *= -1;
        }
    }
    
    if (st > 128) st = 128;
    else if (st < - 127) st = -127;
    
    Go(lsp, rsp);
    Steering2(st, 0);
}