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     // create miscellaneous periphery objects
00020     
00021     DigitalOut led(LED1);
00022     DigitalIn button(USER_BUTTON);
00023     
00024     DigitalOut led0(PC_8);
00025     DigitalOut led1(PC_6);
00026     DigitalOut led2(PB_12);
00027     DigitalOut led3(PA_7);
00028     DigitalOut led4(PC_0);
00029     DigitalOut led5(PC_9);
00030     
00031     // create motor control objects
00032     
00033     DigitalOut enableMotorDriver(PB_2);
00034     DigitalIn motorDriverFault(PB_14);
00035     DigitalIn motorDriverWarning(PB_15);
00036     
00037     PwmOut pwmLeft(PA_8);
00038     PwmOut pwmRight(PA_9);
00039     
00040     EncoderCounter counterLeft(PB_6, PB_7);
00041     EncoderCounter counterRight(PA_6, PC_7);
00042     
00043     // create distance sensor objects
00044     
00045     DigitalOut enableIRSensors(PC_1);
00046     enableIRSensors = 1;
00047     
00048     AnalogIn distance(PB_1);
00049     DigitalOut bit0(PH_1);
00050     DigitalOut bit1(PC_2);
00051     DigitalOut bit2(PC_3);
00052     
00053     IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
00054     IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
00055     IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
00056     IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
00057     IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
00058     IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
00059     
00060     // create LIDAR object
00061     
00062     PwmOut pwm(PA_10);
00063     pwm.period(0.00005f);
00064     pwm.write(0.5f);      // 50% duty-cycle
00065     
00066     RawSerial uart(PA_0, PA_1);
00067     
00068     LIDAR lidar(uart);
00069     
00070     // create robot controller objects
00071     
00072     Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
00073     StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
00074     
00075     // create serial server object
00076     
00077     RawSerial serial(PB_10, PC_5);
00078     serial.baud(9600);
00079     serial.format(8, SerialBase::None, 1);
00080     
00081     DigitalOut reset(PB_3);
00082     DigitalOut modes1(PB_4);
00083     
00084     modes1 = 0;
00085     
00086     reset = 1; wait(0.1f);
00087     reset = 0; wait(0.1f);
00088     reset = 1; wait(0.1f);
00089     
00090     SerialServer serialServer(serial, lidar, controller);
00091     
00092     while (true) {
00093         
00094         led = !led; // toggle led
00095         
00096         wait(0.2f); // wait for 200 ms
00097         
00098         lidar.lookForBeacon();
00099         
00100         // bitte Lagekorrektur implementieren!
00101     }
00102 }
00103