File content as of revision 2:fc9e2aebf9d5:

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

#include <mbed.h>
#include "IRSensor.h"
#include "EncoderCounter.h"
#include "Controller.h"
#include "StateMachine.h"

int main() {
    
    // initialise digital inputs and outputs
    
    DigitalIn button(USER_BUTTON);
    
    DigitalOut ledGreen(LED1);
    DigitalOut ledBlue(LED2);
    DigitalOut ledRed(LED3);
    
    DigitalOut led0(PD_4);
    DigitalOut led1(PD_3);
    DigitalOut led2(PD_6);
    DigitalOut led3(PD_2);
    DigitalOut led4(PD_7);
    DigitalOut led5(PD_5);
    
    // create distance sensor objects
    
    AnalogIn distance(PA_0);
    DigitalOut enable(PG_1);
    DigitalOut bit0(PF_0);
    DigitalOut bit1(PF_1);
    DigitalOut bit2(PF_2);
    
    enable = 1;
    
    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 motor controller objects
    
    DigitalOut enableMotorDriver(PG_0); 
    DigitalIn motorDriverFault(PD_1);
    DigitalIn motorDriverWarning(PD_0);
    
    PwmOut pwmLeft(PF_9);
    PwmOut pwmRight(PF_8);
    
    // create encoder counter objects
    
    EncoderCounter counterLeft(PD_12, PD_13);
    EncoderCounter counterRight(PB_4, PC_7);
    
    // 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);
    
    // enter main loop
    
    while (true) {
        
        if (stateMachine.getState() == StateMachine::MOVE_FORWARD) {
            
            ledGreen = 1;
            ledBlue = 0;
            ledRed = 0;
            
        } else if ((stateMachine.getState() == StateMachine::TURN_LEFT) || (stateMachine.getState() == StateMachine::TURN_RIGHT)) {
            
            ledGreen = 1;
            ledBlue = 1;
            ledRed = 0;
            
        } else {
            
            ledGreen = 0;
            ledBlue = 0;
            ledRed = 1;
        }
        
        // print robot pose into terminal
        
        printf("%.3f %.3f %.3f\r\n", controller.getX(), controller.getY(), controller.getAlpha());
        
        wait(0.1);
    }
}