// Mako Basic Movement Demo, M. Finn May 2018

#include "eyebot.h"

// PSD IDs.
const int PSD_DOWN = 6;

// Thruster IDs.
const int LEFT = 1;
const int FRONT = 2;
const int RIGHT = 3;
const int BACK = 4;

const int SPEED = 50;

void up(float secs)
{
    MOTORDrive(FRONT, SPEED);
    MOTORDrive(BACK, SPEED);
    sleep(secs);
    MOTORDrive(FRONT, 0);
    MOTORDrive(BACK, 0);
}

void down(float secs)
{
    MOTORDrive(FRONT, -SPEED);
    MOTORDrive(BACK, -SPEED);
    sleep(secs);
    MOTORDrive(FRONT, 0);
    MOTORDrive(BACK, 0);
}

void forwards(float secs)
{
    MOTORDrive(LEFT, SPEED);
    MOTORDrive(RIGHT, SPEED);
    sleep(secs);
    MOTORDrive(LEFT, 0);
    MOTORDrive(RIGHT, 0);
}

void backwards(float secs)
{
    MOTORDrive(LEFT, -SPEED);
    MOTORDrive(RIGHT, -SPEED);
    sleep(secs);
    MOTORDrive(LEFT, 0);
    MOTORDrive(RIGHT, 0);
}

void turn_left(float secs)
{
    MOTORDrive(LEFT, -SPEED);
    MOTORDrive(RIGHT, SPEED);
    sleep(secs);
    MOTORDrive(LEFT, 0);
    MOTORDrive(RIGHT, 0);
}

void turn_right(float secs)
{
    MOTORDrive(LEFT, SPEED);
    MOTORDrive(RIGHT, -SPEED);
    sleep(secs);
    MOTORDrive(LEFT, 0);
    MOTORDrive(RIGHT, 0);
}

int main()
{
    float time = 1.0f;
    
    up(time);
    forwards(time);
    turn_left(time);
    turn_right(time);
    backwards(time);
    down(time);
}
