Marco Oehler / Mbed OS Lab3

ROME2 Lab3

Main.cpp@2:fc9e2aebf9d5, 2020-03-25 (annotated)

Committer:
oehlemar
Date:
Wed Mar 25 14:15:52 2020 +0000
Revision:
2:fc9e2aebf9d5
Parent:
0:6a4d3264c067 
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oehlemar 0:6a4d3264c067 1 /*
oehlemar 0:6a4d3264c067 2 * Main.cpp
oehlemar 0:6a4d3264c067 3 * Copyright (c) 2020, ZHAW
oehlemar 0:6a4d3264c067 4 * All rights reserved.
oehlemar 0:6a4d3264c067 5 */
oehlemar 0:6a4d3264c067 6
oehlemar 0:6a4d3264c067 7 #include <mbed.h>
oehlemar 0:6a4d3264c067 8 #include "IRSensor.h"
oehlemar 0:6a4d3264c067 9 #include "EncoderCounter.h"
oehlemar 0:6a4d3264c067 10 #include "Controller.h"
oehlemar 0:6a4d3264c067 11 #include "StateMachine.h"
oehlemar 0:6a4d3264c067 12
oehlemar 0:6a4d3264c067 13 int main() {
oehlemar 0:6a4d3264c067 14
oehlemar 0:6a4d3264c067 15 // initialise digital inputs and outputs
oehlemar 0:6a4d3264c067 16
oehlemar 0:6a4d3264c067 17 DigitalIn button(USER_BUTTON);
oehlemar 0:6a4d3264c067 18
oehlemar 0:6a4d3264c067 19 DigitalOut ledGreen(LED1);
oehlemar 0:6a4d3264c067 20 DigitalOut ledBlue(LED2);
oehlemar 0:6a4d3264c067 21 DigitalOut ledRed(LED3);
oehlemar 0:6a4d3264c067 22
oehlemar 0:6a4d3264c067 23 DigitalOut led0(PD_4);
oehlemar 0:6a4d3264c067 24 DigitalOut led1(PD_3);
oehlemar 0:6a4d3264c067 25 DigitalOut led2(PD_6);
oehlemar 0:6a4d3264c067 26 DigitalOut led3(PD_2);
oehlemar 0:6a4d3264c067 27 DigitalOut led4(PD_7);
oehlemar 0:6a4d3264c067 28 DigitalOut led5(PD_5);
oehlemar 0:6a4d3264c067 29
oehlemar 0:6a4d3264c067 30 // create distance sensor objects
oehlemar 0:6a4d3264c067 31
oehlemar 0:6a4d3264c067 32 AnalogIn distance(PA_0);
oehlemar 0:6a4d3264c067 33 DigitalOut enable(PG_1);
oehlemar 0:6a4d3264c067 34 DigitalOut bit0(PF_0);
oehlemar 0:6a4d3264c067 35 DigitalOut bit1(PF_1);
oehlemar 0:6a4d3264c067 36 DigitalOut bit2(PF_2);
oehlemar 0:6a4d3264c067 37
oehlemar 0:6a4d3264c067 38 enable = 1;
oehlemar 0:6a4d3264c067 39
oehlemar 0:6a4d3264c067 40 IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
oehlemar 0:6a4d3264c067 41 IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
oehlemar 0:6a4d3264c067 42 IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
oehlemar 0:6a4d3264c067 43 IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
oehlemar 0:6a4d3264c067 44 IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
oehlemar 0:6a4d3264c067 45 IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
oehlemar 0:6a4d3264c067 46
oehlemar 0:6a4d3264c067 47 // create motor controller objects
oehlemar 0:6a4d3264c067 48
oehlemar 0:6a4d3264c067 49 DigitalOut enableMotorDriver(PG_0);
oehlemar 0:6a4d3264c067 50 DigitalIn motorDriverFault(PD_1);
oehlemar 0:6a4d3264c067 51 DigitalIn motorDriverWarning(PD_0);
oehlemar 0:6a4d3264c067 52
oehlemar 0:6a4d3264c067 53 PwmOut pwmLeft(PF_9);
oehlemar 0:6a4d3264c067 54 PwmOut pwmRight(PF_8);
oehlemar 0:6a4d3264c067 55
oehlemar 0:6a4d3264c067 56 // create encoder counter objects
oehlemar 0:6a4d3264c067 57
oehlemar 0:6a4d3264c067 58 EncoderCounter counterLeft(PD_12, PD_13);
oehlemar 0:6a4d3264c067 59 EncoderCounter counterRight(PB_4, PC_7);
oehlemar 0:6a4d3264c067 60
oehlemar 0:6a4d3264c067 61 // create robot controller objects
oehlemar 0:6a4d3264c067 62
oehlemar 0:6a4d3264c067 63 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
oehlemar 0:6a4d3264c067 64 StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
oehlemar 0:6a4d3264c067 65
oehlemar 0:6a4d3264c067 66 // enter main loop
oehlemar 0:6a4d3264c067 67
oehlemar 0:6a4d3264c067 68 while (true) {
oehlemar 0:6a4d3264c067 69
oehlemar 0:6a4d3264c067 70 if (stateMachine.getState() == StateMachine::MOVE_FORWARD) {
oehlemar 0:6a4d3264c067 71
oehlemar 0:6a4d3264c067 72 ledGreen = 1;
oehlemar 0:6a4d3264c067 73 ledBlue = 0;
oehlemar 0:6a4d3264c067 74 ledRed = 0;
oehlemar 0:6a4d3264c067 75
oehlemar 0:6a4d3264c067 76 } else if ((stateMachine.getState() == StateMachine::TURN_LEFT) || (stateMachine.getState() == StateMachine::TURN_RIGHT)) {
oehlemar 0:6a4d3264c067 77
oehlemar 0:6a4d3264c067 78 ledGreen = 1;
oehlemar 0:6a4d3264c067 79 ledBlue = 1;
oehlemar 0:6a4d3264c067 80 ledRed = 0;
oehlemar 0:6a4d3264c067 81
oehlemar 0:6a4d3264c067 82 } else {
oehlemar 0:6a4d3264c067 83
oehlemar 0:6a4d3264c067 84 ledGreen = 0;
oehlemar 0:6a4d3264c067 85 ledBlue = 0;
oehlemar 0:6a4d3264c067 86 ledRed = 1;
oehlemar 0:6a4d3264c067 87 }
oehlemar 0:6a4d3264c067 88
oehlemar 0:6a4d3264c067 89 // print robot pose into terminal
oehlemar 0:6a4d3264c067 90
oehlemar 0:6a4d3264c067 91 printf("%.3f %.3f %.3f\r\n", controller.getX(), controller.getY(), controller.getAlpha());
oehlemar 0:6a4d3264c067 92
oehlemar 0:6a4d3264c067 93 wait(0.1);
oehlemar 0:6a4d3264c067 94 }
oehlemar 0:6a4d3264c067 95 }
oehlemar 0:6a4d3264c067 96