Lab 4: Extra Practice

/* 

 * File:   FinalRoachMain.c

 * Author: COSMOS

 *

 * Created on July 27, 2018, 11:31 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"

#include "Debounce.h"

#define TIMER_A 0

#define T_360 1100 //change as appropriate

#define T_FOOT 800 //change as appropriate

#define MAX_SPEED 100

//#define BUTTON_UNPRESSED 0

//#define BUTTON_PRESSED 1

void main(void) {

    TIMERS_Init();

    SERIAL_Init();

    BOARD_Init();

    Roach_Init();

    /*

    while (1) {

        if (Roach_ReadBumpers() != 0) {

            break;

        }

    }

     */

    //    TIMERS_InitTimer(TIMER_A, 100);

    //    char is_in_light = 1;

    //    while (1) {

    //        if (TIMERS_IsTimerExpired(0)) {

    //            switch (is_in_light) {

    //                case 1: //in light

    //                    TIMERS_InitTimer(0, 100);

    //                    Forward(100);

    //                    if (!CheckLightSensor()) {

    //                        is_in_light = 0;

    //                    }

    //                    break;

    //                case 0: //in dark

    //                    TIMERS_InitTimer(0, 100);

    //                    Stop();

    //                    if (CheckLightSensor()) {

    //                        is_in_light = 1;

    //                    }

    //                    break;

    //            }

    //        }

    //    }

    int step = 1;

    while (1) {

        switch (step) {

            case 1:

                //going forward

                if (DebounceFL() == BUTTON_PRESSED) {

                    RotateRight(100);

                }

                if (DebounceFR() == BUTTON_PRESSED) {

                    RotateLeft(100);

                }

                if (CheckWhichBump() == FL_DOWN) {

                    RotateRight(100);

                }

                if (CheckWhichBump() == FR_DOWN) {

                    RotateLeft(100);

                }

                step = 0;

                break;

            case 0: //turning away from the wall

                Forward(100);

                step = 1;

                break;

        }

    }

    /*

    int wastetime = 0;

    Forward(100);

    while (1) {

        if (CheckWhichBump() == 1) {

            TIMERS_InitTimer(0, T_FOOT);

            RotateLeft(MAX_SPEED);

            wastetime = 1;

        }

        if (CheckWhichBump() == 2) {

            TIMERS_InitTimer(0, T_FOOT);

            RotateRight(MAX_SPEED);

            wastetime = 1;

        }

        if (CheckWhichBump() == 3) {

            TIMERS_InitTimer(0, 2 * T_FOOT);

            RotateLeft(MAX_SPEED);

            wastetime = 1;

        }

        if (CheckWhichBump() == 4) {

            TIMERS_InitTimer(0, T_FOOT);

            RotateRight(MAX_SPEED);

            wastetime = 1;

        }

        if (CheckWhichBump() == 8) {

            TIMERS_InitTimer(0, T_FOOT);

            RotateLeft(MAX_SPEED);

            wastetime = 1;

        }

        if ((wastetime == 1) && (TIMERS_IsTimerExpired(0))) {

            Forward(100);

        }

    } */

}