main.c

#include <stdio.h>
#include "asuro.h"

////////////////////////////////////////////////////////////////////////////////
// DEFINES
////////////////////////////////////////////////////////////////////////////////

#define MOTOR_SPEED_MAX 127
#define MOTOR_SPEED 0.5

#define BACKWARDS_TIME 1000
#define TURNING_TIME 400

#define LINE_NUM_AVG 20
#define LINE_THRESHOLD 70

////////////////////////////////////////////////////////////////////////////////
// FORWARD DECLARATIONS
////////////////////////////////////////////////////////////////////////////////

// Is a switch pressed?
int IsAnySwitchPressed(void);

// Drive forward
void DriveForward(void);

// Turn around
void TurnAround(void);

////////////////////////////////////////////////////////////////////////////////
// METHOD DEFINITIONS
////////////////////////////////////////////////////////////////////////////////

//*
int main(void) {
    // Initialization
    Init();

    // Declaration of variables
    unsigned int linedata_init[2] = {0, 0};
    unsigned int linedata_tmp[2];
    unsigned int linedata[2];
    unsigned int j;

    // Turn on Front LED
    FrontLED(ON);

    // Get initial LineData
    for (j = 0; j < LINE_NUM_AVG; j++) {
        LineData(linedata_tmp);

        // Sum up all values
        linedata_init[0] += linedata_tmp[0];
        linedata_init[1] += linedata_tmp[1];
    }

    linedata_init[0] /= LINE_NUM_AVG;
    linedata_init[1] /= LINE_NUM_AVG;

    // DEBUG: Send initial values to PC
    SerPrint("Initial LineData:\r\n");
    SerPrint("Left: ");
    PrintInt(linedata_init[0]);
    SerPrint("\r\n");
    SerPrint("Right: ");
    PrintInt(linedata_init[1]);
    SerPrint("\r\n");
    SerPrint("\r\n");

    // Main loop
    while (1) {
        DriveForward();

        // Heading towards a cliff, turn around
        LineData(linedata);
        if ((int) linedata_init[0] - (int) linedata[0] > LINE_THRESHOLD
            || (int) linedata_init[1] - (int) linedata[1] > LINE_THRESHOLD) {
            TurnAround();

            // DEBUG output
            SerPrint("Left: ");
            PrintInt(linedata[0]);
            SerPrint("\r\n");

            SerPrint("Right: ");
            PrintInt(linedata[1]);
            SerPrint("\r\n");
        }

        // A switch was pressed, turn around
        if (IsAnySwitchPressed()) {
            TurnAround();
        }
    }
    return 0;
}
// */

int IsAnySwitchPressed(void) {
    unsigned char t1, t2;

    t1 = PollSwitch();
    t2 = PollSwitch();

    if(t1 && t2) {
        return TRUE;
    } else {
        return FALSE;
    }
}

void DriveForward(void) {
    SetMotorPower(MOTOR_SPEED_MAX * MOTOR_SPEED, MOTOR_SPEED_MAX * MOTOR_SPEED);
}

void TurnAround(void) {
    // Go back a bit
    SetMotorPower(-MOTOR_SPEED_MAX * MOTOR_SPEED, -MOTOR_SPEED_MAX * MOTOR_SPEED);
    Msleep(BACKWARDS_TIME);

    // Turn around
    SetMotorPower(-MOTOR_SPEED_MAX * MOTOR_SPEED, MOTOR_SPEED_MAX * MOTOR_SPEED);
    Msleep(TURNING_TIME);
}