File content as of revision 0:122d7bf3bbf0:

/*
 * main.cpp
 *
 *  Created on: Aug 23, 2016
 *      Author: tanpt
 */

#include "common.h"
#include "MotorController.h"
#include "SensorController.h"

//#include "ble/BLE.h"
//#include "ble/services/iBeacon.h"
//#include "ble/services/UARTService.h"

MotorController motorController;
SensorController sensors;

//// Sensor
#define NOISE 5
//#define DEBUG

#ifdef DEBUG
Serial pc(p10, p11);
#endif
Ticker ticker;

#define VREF    3300.0  //mV

//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////
void periodicCallback(void) {

}

//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
int main(void) {
    wait(0.1);
//Init Hardware

    sensors.init();

//Init interupt Timer
    ticker.attach(periodicCallback, 0.00015);
    wait(0.5);

    while (true) {
        uint8_t sen_in = sensors.distanceSensorRight();
        if ((sen_in > 0) && (sen_in <= 40)) {
            motorController.moveForward(500);
            wait(1);
        } else {
            motorController.moveForward(500);
            wait(1);
        }
    }

//    while (true) {
//        //rotator_right(20);
//        uint8_t sen_in = sensors.distanceSensor();
//        if ((sensors.frontLeftSensor() == SensorController::SENSOR_OFF)
//                && (sensors.frontRightSensor() == SensorController::SENSOR_OFF)) {
//            motorController.moveBackward(1000);
//            wait(1);
//        } else if (sensors.frontLeftSensor() == SensorController::SENSOR_OFF) {
//            motorController.moveBackRight(1000);
//            wait(1);
//        } else if (sensors.frontRightSensor() == SensorController::SENSOR_OFF) {
//            motorController.moveBackLeft(1000);
//            wait(1);
//        } else if (sensors.backSensor() == SensorController::SENSOR_OFF) {
//            motorController.moveForward(1000);
//            wait_ms(1);
//        } else {
//            if ((sen_in > 0) && (sen_in <= 80)) {
//                motorController.moveForward(1000);
//                wait_ms(200);
//            } else {
//                motorController.turnRight(1000);
//                wait_us(200);
//            }
//        }
//    }

}