Marco Oehler
/
Lab3
ROME2 Lab3
Diff: Main.cpp
- Revision:
- 0:6a4d3264c067
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Main.cpp Tue Mar 24 08:39:54 2020 +0000 @@ -0,0 +1,96 @@ +/* + * 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); + } +} +