File content as of revision 3:09f77ebfd2e5:

/*
 * Main.cpp
 * Copyright (c) 2018, ZHAW
 * All rights reserved.
 */

#include <cstdlib>
#include <mbed.h>
#include "EncoderCounter.h"
#include "IRSensor.h"
#include "IMU.h"
#include "LIDAR.h"
#include "Controller.h"
#include "StateMachine.h"
#include "SerialServer.h"
#define TOLERANCE 0.1f

int main() {
    
    // create miscellaneous periphery objects
    float dist,angle,x_measured_r,y_measured_r, x_measured,y_measured,x,y,angle_pos;
    
    // tolerance values for tube 1 and tube 2
    const float min_x1 = 0.99f - TOLERANCE;
    const float max_x1 = 0.99f + TOLERANCE;
    const float min_y1 = 0.54f - TOLERANCE;
    const float max_y1 = 0.54f + TOLERANCE;
    const float min_x2 = 2.52f - TOLERANCE;
    const float max_x2 = 2.52f + TOLERANCE;
    const float min_y2 = 0.32f - TOLERANCE;
    const float max_y2 = 0.32f + TOLERANCE;
    
    DigitalOut led(LED1);
    DigitalIn button(USER_BUTTON);
    
    DigitalOut led0(PC_8);
    DigitalOut led1(PC_6);
    DigitalOut led2(PB_12);
    DigitalOut led3(PA_7);
    DigitalOut led4(PC_0);
    DigitalOut led5(PC_9);
    
    // create motor control objects
    
    DigitalOut enableMotorDriver(PB_2);
    DigitalIn motorDriverFault(PB_14);
    DigitalIn motorDriverWarning(PB_15);
    
    PwmOut pwmLeft(PA_8);
    PwmOut pwmRight(PA_9);
    
    EncoderCounter counterLeft(PB_6, PB_7);
    EncoderCounter counterRight(PA_6, PC_7);
    
    // create distance sensor objects
    
    DigitalOut enableIRSensors(PC_1);
    enableIRSensors = 1;
    
    AnalogIn distance(PB_1);
    DigitalOut bit0(PH_1);
    DigitalOut bit1(PC_2);
    DigitalOut bit2(PC_3);
    
    IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
    IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
    IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
    IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
    IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
    IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
    
    // create LIDAR object
    
    PwmOut pwm(PA_10);
    pwm.period(0.00005f);
    pwm.write(0.5f);      // 50% duty-cycle
    
    RawSerial uart(PA_0, PA_1);
    
    LIDAR lidar(uart);
    
    // create robot controller objects
    
    Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
    StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
    
    // create serial server object
    
    RawSerial serial(PB_10, PC_5);
    serial.baud(9600);
    serial.format(8, SerialBase::None, 1);
    
    DigitalOut reset(PB_3);
    DigitalOut modes1(PB_4);
    
    modes1 = 0;
    
    reset = 1; wait(0.1f);
    reset = 0; wait(0.1f);
    reset = 1; wait(0.1f);
    
    SerialServer serialServer(serial, lidar, controller);
    
    printf("created new waypoint,task");
    // move to waypoints
   TaskMoveToWaypoint taskMoveToWaypoint(controller,2,0.3,10);
    while (true) {
        
        led = !led; // toggle led
        
        wait(0.2f); // wait for 200 ms
        
        lidar.lookForBeacon();
        dist =  (float)lidar.getDistanceOfBeacon()/1000;
        angle = (float)lidar.getAngleOfBeacon()*3.14/180;
        //relatives cord
        if(dist!=0)
        {
            x_measured_r = distance*cos(angle);
            y_measured_r = distance*cos(angle);
            //global cord
            angle_pos = controller.getAlpha();
            x= controller.getX(); 
            y= controller.getY();
            x_measured = cos(angle_pos)*x_measured_r-sin(angle_pos)*y_measured_r+x;
            y_measured = sin(angle_pos)*x_measured_r+cos(angle_pos)*y_measured_r+y;
        }
       //check if measured tube is valid with reference
       if(x_measured>=min_x1 && x_measured<= max_x1 && y_measured>=min_y1 && y_measured<=max_y1)
        {
            controller.correctPoseWithBeacon(0.99f,0.54f,x_measured,y_measured);
            //turn led on
            printf("found tube 1 at %f %f",x_measured,y_measured);
        }
       if(x_measured>=min_x2 && x_measured<= max_x2 && y_measured>=min_y1 && y_measured<=max_y2)
        {
            controller.correctPoseWithBeacon(2.52f,0.32f,x_measured,y_measured);
            printf("found tube 2 at %f %f",x_measured,y_measured);
            //turn led on
        }
    }
    
}