Main.cpp

00001 /*
00002  * Main.cpp
00003  * Copyright (c) 2020, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include <mbed.h>
00008 #include "IRSensor.h"
00009 #include "EncoderCounter.h"
00010 #include "Controller.h"
00011 #include "StateMachine.h"
00012 
00013 int main() {
00014     
00015     // initialise digital inputs and outputs
00016     
00017     DigitalIn button(USER_BUTTON);
00018     
00019     DigitalOut ledGreen(LED1);
00020     DigitalOut ledBlue(LED2);
00021     DigitalOut ledRed(LED3);
00022     
00023     DigitalOut led0(PD_4);
00024     DigitalOut led1(PD_3);
00025     DigitalOut led2(PD_6);
00026     DigitalOut led3(PD_2);
00027     DigitalOut led4(PD_7);
00028     DigitalOut led5(PD_5);
00029     
00030     // create distance sensor objects
00031     
00032     AnalogIn distance(PA_0);
00033     DigitalOut enable(PG_1);
00034     DigitalOut bit0(PF_0);
00035     DigitalOut bit1(PF_1);
00036     DigitalOut bit2(PF_2);
00037     
00038     enable = 1;
00039     
00040     IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
00041     IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
00042     IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
00043     IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
00044     IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
00045     IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
00046     
00047     // create motor controller objects
00048     
00049     DigitalOut enableMotorDriver(PG_0); 
00050     DigitalIn motorDriverFault(PD_1);
00051     DigitalIn motorDriverWarning(PD_0);
00052     
00053     PwmOut pwmLeft(PF_9);
00054     PwmOut pwmRight(PF_8);
00055     
00056     // create encoder counter objects
00057     
00058     EncoderCounter counterLeft(PD_12, PD_13);
00059     EncoderCounter counterRight(PB_4, PC_7);
00060     
00061     // create robot controller objects
00062     
00063     Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
00064     StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
00065     
00066     // enter main loop
00067     
00068     while (true) {
00069         
00070         if (stateMachine.getState() == StateMachine::MOVE_FORWARD) {
00071             
00072             ledGreen = 1;
00073             ledBlue = 0;
00074             ledRed = 0;
00075             
00076         } else if ((stateMachine.getState() == StateMachine::TURN_LEFT) || (stateMachine.getState() == StateMachine::TURN_RIGHT)) {
00077             
00078             ledGreen = 1;
00079             ledBlue = 1;
00080             ledRed = 0;
00081             
00082         } else {
00083             
00084             ledGreen = 0;
00085             ledBlue = 0;
00086             ledRed = 1;
00087         }
00088         
00089         // print robot pose into terminal
00090         
00091         printf("%.3f %.3f %.3f\r\n", controller.getX(), controller.getY(), controller.getAlpha());
00092         
00093         wait(0.1);
00094     }
00095 }
00096