Main.cpp

00001 /*
00002  * Main.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  */
00006  
00007 #include <cstdlib>
00008 #include <mbed.h>
00009 #include "EncoderCounter.h"
00010 #include "IRSensor.h"
00011 #include "IMU.h"
00012 #include "LIDAR.h"
00013 #include "Controller.h"
00014 #include "StateMachine.h"
00015 #include "SerialServer.h"
00016 
00017 int main() {
00018     
00019    
00020     
00021     // create miscellaneous periphery objects
00022     
00023     DigitalOut led(LED1);
00024     DigitalIn button(USER_BUTTON);
00025     
00026     DigitalOut led0(PC_8);
00027     DigitalOut led1(PC_6);
00028     DigitalOut led2(PB_12);
00029     DigitalOut led3(PA_7);
00030     DigitalOut led4(PC_0);
00031     DigitalOut led5(PC_9);
00032     
00033     // create motor control objects
00034     
00035     DigitalOut enableMotorDriver(PB_2);
00036     DigitalIn motorDriverFault(PB_14);
00037     DigitalIn motorDriverWarning(PB_15);
00038     
00039     PwmOut pwmLeft(PA_8);
00040     PwmOut pwmRight(PA_9);
00041     
00042     EncoderCounter counterLeft(PB_6, PB_7);
00043     EncoderCounter counterRight(PA_6, PC_7);
00044     
00045     // create distance sensor objects
00046     
00047     DigitalOut enableIRSensors(PC_1);
00048     enableIRSensors = 1;
00049     
00050     AnalogIn distance(PB_1);
00051     DigitalOut bit0(PH_1);
00052     DigitalOut bit1(PC_2);
00053     DigitalOut bit2(PC_3);
00054     
00055     IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
00056     IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
00057     IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
00058     IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
00059     IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
00060     IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
00061     
00062     // create LIDAR object
00063     
00064     PwmOut pwm(PA_10);
00065     pwm.period(0.00005f);
00066     pwm.write(0.5f);      // 50% duty-cycle
00067     
00068     RawSerial uart(PA_0, PA_1);
00069     
00070     LIDAR lidar(uart);
00071     
00072     // create robot controller objects
00073     
00074     Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
00075     StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
00076     
00077     // create serial server object
00078     
00079     RawSerial serial(PB_10, PC_5);
00080     serial.baud(9600);
00081     serial.format(8, SerialBase::None, 1);
00082     
00083     DigitalOut reset(PB_3);
00084     DigitalOut modes1(PB_4);
00085     
00086     modes1 = 0;
00087     
00088     reset = 1; wait(0.1f);
00089     reset = 0; wait(0.1f);
00090     reset = 1; wait(0.1f);
00091     
00092     
00093     
00094     
00095     SerialServer serialServer(serial, lidar, controller);
00096    
00097     
00098     while (true) {
00099         
00100         led = !led; // toggle led
00101         led0 = !led0;
00102         led1 = !led1;
00103         led2 = !led2;
00104         led3 = !led3;
00105         led4 = !led4;
00106         led5 = !led5;
00107 
00108         //printf("hallo  \r \n ");        
00109         wait(1.0f); // wait for 200 ms
00110         
00111         lidar.lookForBeacon();
00112     }
00113 }
00114