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


using namespace hSensors;

void hMain(void)
{
    sys.setLogDev(&Serial);
    Lego_Touch sensor(hSens5);
    int count = 0;
    bool stan = false;

    for (;;)
    {
        bool btn1Pressed = hBtn1.isPressed();
        bool btn2Pressed = hBtn2.isPressed();
        bool pressed = sensor.isPressed();

        if(btn1Pressed && !stan){
            hMot1.setPower(500);
            printf("bt1");
        }
        else if(btn2Pressed && !stan){
            hMot1.setPower(-500);
                printf("bt2");
        }

        if(pressed && !stan){
            count++;
            if(!stan){
                LED1.toggle();

            }
        }

        sys.delay(10);
        stan = pressed;

    }
}