Lab 4: Square

/* 

 * File:   Square.c

 * Author: COSMOS

 *

 * Created on July 27, 2018, 10:58 AM

 */

#include <stdio.h>

#include <stdlib.h>

#include "serial.h"

#include "BOARD.h"

#include "roach.h"

#include "timers.h"

#include "Navigation.h"

#include "Bumpers.h"

#include "CheckLightSensor.h"

#define T_360 1100 //change as appropriate

#define T_FOOT 800 //change as appropriate

#define MAX_SPEED 100

/*

 * 

 */

void main(void) {

    TIMERS_Init();

    SERIAL_Init();

    BOARD_Init();

    Roach_Init();

    while (1) {

        if (Roach_ReadBumpers() != 0) {

            break;

        }

    }

    char steps_remaining = 0;

    TIMERS_InitTimer(0, 100);

    while (1) {

        if (TIMERS_IsTimerExpired(0)) {

            switch (steps_remaining) {

                case 0:

                case 6:

                case 4:

                case 2:

                    TIMERS_InitTimer(0, T_FOOT);

                    Forward(MAX_SPEED);

                    steps_remaining++;

                    steps_remaining %= 8;

                    break;

                case 7:

                case 5:

                case 3:

                case 1:

                    TIMERS_InitTimer(0, T_360 / 4);

                    RotateLeft(MAX_SPEED);

                    steps_remaining++;

                    steps_remaining %= 8;

                    break;

            }

        }

    }

}