sketch_jul28_mission.ino

#include <Altino.h>

#define FWD 450
#define BWD -525

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

int runningTime = 0;
int process = 0;
int processTime = 0;

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

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;
    }
    cds = sdata.CDSSensor;

    runningTime++;

    if (process == 0) {
        if (ir[5] >= 250) {
            process = 1;
            processTime = runningTime;
        }
    }
    else if (process == 1) {
        if (cds <= 300 && runningTime >= processTime + 300 && ir[0] > 12 && ir[1] > 12 && ir[2] > 12) {
            Go(0, 0);
            Steering2(0, 0);
            DisplayLine(0x3c, 0x42, 0x3c, 0x00, 0x7e, 0x18, 0x24, 0x42);
            pl(51, 0.5, 100);
            pl(51, 0.5, 100);
            halt();
        }
        else {
            Auto(ir);
        }
    }
}

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

void halt() {
    Go(0, 0);
    Steering2(0, 0);
    Display(0);
    Sound(0);
    Led(0);
    exit(0);
}

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 = 127;
    }
    else if (ir[0] > 15 && ir[1] > 15) {
        lsp = BWD;
        rsp = BWD;
        st = -127;
        lsp += st;
    }
    else if (ir[1] > 15 && ir[2] > 15) {
        lsp = BWD;
        rsp = BWD;
        st = 128;
        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[4] / 3);
    }
    
    if (st > 128) st = 128;
    else if (st < - 127) st = -127;
    
    Go(lsp, rsp);
    Steering2(st, 0);
}