roomba.device.nut

imp.configure("Roomba", [], []);

hardware.pin1.configure(DIGITAL_IN_WAKEUP);
hardware.pin2.configure(DIGITAL_OUT_OD_PULLUP);
impWakeupPin <- hardware.pin1;
roombaWakeupPin <- hardware.pin2;
roombaWakeupPin.write(1);

hardware.uart57.configure(115200, 8, PARITY_NONE, 1, NO_CTSRTS);
//Connection will be either 115200 or 57600

imp.sleep(1);

hardware.uart57.flush();

function wakeup()
{
    server.log("wakeup");
    roombaWakeupPin.write(0);
    imp.sleep(0.5);
    roombaWakeupPin.write(1);
    imp.sleep(0.5);
}

function start()
{
    server.log("Start");
    hardware.uart57.write(128); //start
}

function controlMode()
{
    hardware.uart57.write(130);
}
function safeMode()
{
    hardware.uart57.write(131);
}
function fullMode()
{
    hardware.uart57.write(132);
}

function sleep()
{
    server.log("Sleep");
    hardware.uart57.write(133);
}

function lights()
{
    server.log("Lights");
    hardware.uart57.write(139);
    hardware.uart57.write(0);   //which light
    hardware.uart57.write(0);   //main light - green to red
    hardware.uart57.write(0); //main light brightness
    //1 - dirt detect
    //2 - spot lights
    //4 - dock lights
    //8 - error lights
}

function beep()
{
    server.log("Beep");
    
    hardware.uart57.write(140);
    hardware.uart57.write(0);
    hardware.uart57.write(1);
    hardware.uart57.write(62);
    hardware.uart57.write(32);
    
    hardware.uart57.write(141);
    hardware.uart57.write(0);
}

function readAllSensors()
{
    server.log("Fetching all sensor data");
    hardware.uart57.flush();
    
    hardware.uart57.write(142);
    hardware.uart57.write(0);   //0 fetches all the data
    
    local sensorBit = 0;
    local sensorDataArray = array(26);  //If successfull 26 bytes should be sent through

    local b = hardware.uart57.read();
    local failureCheck = 0;
    //Is the data ready yet?
    if (b == -1) {
        //Loop untill we have something
        while(b == -1) {
            b = hardware.uart57.read();
            failureCheck++;
            //If something goes wrong or the roomba is turned off this will stop us getting stuck
            if (failureCheck > 1000)
            {
                server.log("Waited to long");
                return;
            }
        }
    }
    while(b != -1) {
        if (sensorBit == 26)
        {
            //We shouldnt be here, the data is probably bad
            server.log("To many bytes, bad data");
            return;
        }
        sensorDataArray[sensorBit] = b;
        //server.log(sensorBit+": "+b);
        sensorBit++;
        b = hardware.uart57.read();
        
        failureCheck = 0;
        //The next byte of data isnt imediatly available so this waits untill it is
        if ((b == -1) && (sensorBit < 25))
        {
            b = hardware.uart57.read();
            failureCheck++;
            //Another check to make sure we dont get stuck
            if (failureCheck > 1000)
            {
                return;
            }
        }
    }
    //Send the array of data to the agent
    agent.send("sensor_data", sensorDataArray);
}

function mainLoop()
{
    server.log("Main Program");
    local roombaOn = impWakeupPin.read();
    if (!roombaOn)
    {
        server.log("roomba asleep");
        wakeup();
    }
    else
    {
        server.log("roomba awake");
    }
    start();
    readAllSensors();
    
    if (colinOn)
    {
        //The roomba was on, sleep for 10 second and then read the sensors again
        imp.sleep(60);
        mainLoop();
    }
    else
    {
        //The roomba was off, turn back off and sleep for an hour
        sleep();
        imp.sleep(0.5); //This is to ensure the roomba is off and doesnt trigger the imp wakeup
        server.sleepfor((60*60));
    }
}

//Start the process
mainLoop();

//If we are idle run the main loop
imp.onidle(function() {
    mainLoop();
});