GOALS:

Mechanical Part:

-Built from Lego set
The robot has two parallel motor-driven wheels and one balancing wheel.

-Include motors and sensors
Motors are from LEGO company and sensors from Husarion.

-Keeps moving forward, turning 90deg and turning back 180deg

-Follows left wall to exit the labirynth

Software Part:

-C programming language with Husarion/Lego libraries used for microcontroller RoboCore

-The input is taken from sensors and teh distance from walls is measured

-For general cases special functions were created to give information to robot how it should behave

Design:

Our MicroMouse is created from 2 motors, 3 ultrasonic sensor(right, left forward) and 1 RoboCORE. All these thing are conntected and padded using Lego sets. Two motors are used to rotate wheels - the third wheel does not need to be driven. Ultrasonic sensor used to measure the distance between the robot and walls not to let the Micromouse destroy them.

The video of how it is working:

The code:

#include <hFramework.h>
#include <stdio.h>
#include <DistanceSensor.h>
#include "Lego_Touch.h"

using namespace hFramework;
using namespace hSensors;
using namespace hModules;


int left(int direction, int direction2, int a)
{
     a = a + 1;

        direction = 500;
        direction2 = -direction;
        hMot1.setPower(direction2);
        hMot2.setPower(direction);

        sys.delay_ms(755);
        hMot1.setPower(0);
        hMot2.setPower(0);
        //sys.delay_ms(500);

     return a;
}

int right(int direction, int direction2, int a)
{
     a = 0;
     direction = 500;
     direction2 = -direction;
     hMot1.setPower(direction);
     hMot2.setPower(direction2);

     sys.delay_ms(720);
     hMot1.setPower(0);
     hMot2.setPower(0);
     return a;
}

int forward(int direction, int direction2, int a)
{
    a = 0;
    direction = 400;
    direction2 = 400;
    hMot1.setPower(direction);
    hMot2.setPower(direction2);
    return a;
}

int turnback(int direction, int direction2, int a)
{
     a = 0;
     direction = 500;
     direction2 = -direction;
     hMot1.setPower(direction);
     hMot2.setPower(direction2);
     sys.delay_ms(1435);
     hMot1.setPower(0);
     hMot2.setPower(0);
     return a;
}

void wenopodjedz(int direction, int direction2)
{
    direction = 400;
    direction2 = 400;
    hMot1.setPower(direction);
    hMot2.setPower(direction2);
    sys.delay_ms(1200);
    hMot1.setPower(0);
    hMot2.setPower(0);
}

int wenopodjedz2(int direction, int direction2, int a)
{
    if(a==1){
        direction = 400;
        direction2 = 400;
        hMot1.setPower(direction);
        hMot2.setPower(direction2);
        sys.delay_ms(1200);
        hMot1.setPower(0);
        hMot2.setPower(0);
    }
    else {
        direction = 400;
        direction2 = 400;
        hMot1.setPower(direction);
        hMot2.setPower(direction2);
        sys.delay_ms(1500);
        hMot1.setPower(0);
        hMot2.setPower(0);
    }

    a = 0;
    return a;
}

void hMain(void)
{
    sys.setLogDev(&Serial);

    DistanceSensor sensor1(hSens1);
    DistanceSensor sensor2(hSens2);
    DistanceSensor sensor3(hSens3);

    for (;;)
    {

        bool power = false;
        int direction = 0;
        int direction2 = 0;
        int a = 0;

        power = hBtn1.isPressed();

        while(power){

            int distance1 = sensor1.getDistance();
            int distance2 = sensor2.getDistance();
            int distance3 = sensor3.getDistance();

            if(distance2<20 && distance3>12){
                forward(direction, direction2, a);
            }

            if(distance2>30){
                wenopodjedz(direction, direction2);
                sys.delay(100);
                left(direction, direction2, a);
                sys.delay(100);
                wenopodjedz2(direction, direction2, a);
            }

            if(distance2<20 && distance3<=12 && distance1>20){
                right(direction, direction2, a);
            }

            if(distance1<20 && distance2<20 && distance3<12){
                turnback(direction, direction2, a);
                wenopodjedz(direction, direction2);
            }

            if(distance1>=30 && distance2>=30 && distance3>=30){
                wenopodjedz(direction, direction2);
            }
        }
    }
}