Lab 4: Navigation Functions

/* 

 * File:   Writing_Functions_main.c

 * Author: COSMOS

 *

 * Created on July 24, 2018, 10:53 AM

 */

#include <stdio.h>

#include <stdlib.h>

#include "serial.h"

#include "BOARD.h"

#include "roach.h"

#include "timers.h"

#define INTERVAL 2000 //why?

//Make Roach go Forward at speed, speed between -100 and 100

/*

void Forward(int speed) {

    Roach_LeftMtrSpeed(speed);

    Roach_RightMtrSpeed(speed);

}

void Reverse(int speed) {

    Roach_LeftMtrSpeed(-speed);

    Roach_RightMtrSpeed(-speed);

}

void Stop(void) {

    Roach_LeftMtrSpeed(0);

    Roach_RightMtrSpeed(0);

}

void RotateRight(int speed) {

    Roach_LeftMtrSpeed(speed);

    Roach_RightMtrSpeed(-speed);

}

void RotateLeft(int speed) {

    Roach_LeftMtrSpeed(-speed);

    Roach_RightMtrSpeed(speed);

}

void PivotLeft(int speed) {

    Roach_LeftMtrSpeed(0);

    Roach_RightMtrSpeed(speed);

}

void PivotRight(int speed) {

    Roach_LeftMtrSpeed(speed);

    Roach_RightMtrSpeed(0);

}

void ArcRight(int speed) {

    Roach_LeftMtrSpeed(speed);

    Roach_RightMtrSpeed(speed * 2 / 3);

}

void ArcLeft(int speed) {

    Roach_LeftMtrSpeed(speed * 2 / 3);

    Roach_RightMtrSpeed(speed);

}

 */

void main(void) {

    //Init functions here

    TIMERS_Init();

    SERIAL_Init();

    BOARD_Init();

    Roach_Init();

    printf("Test...\r\n");

    int speed = 100;

    TIMERS_InitTimer(0, 1);

    int counter = 0;

    while (1) {

        if (TIMERS_IsTimerExpired(0)) {

            counter++;

            counter %= 10;

            TIMERS_InitTimer(0, INTERVAL);

            switch (counter) {

                case 1:

                    Forward(100);

                    break;

                case 2:

                    Reverse(100);

                    break;

                case 3:

                    Stop();

                    break;

                case 4:

                    RotateRight(70);

                    break;

                case 5:

                    RotateLeft(70);

                    break;

                case 6:

                    PivotLeft(70);

                    break;

                case 7:

                    PivotRight(70);

                    break;

                case 8:

                    ArcRight(70);

                    break;

                case 9:

                    ArcLeft(70);

                    break;

                default:

                    Stop();

            }

        }

    }

}