Diff: Main.cpp

Revision:

0:20ec9d702676

diff -r 000000000000 -r 20ec9d702676 Main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Main.cpp	Tue Mar 31 11:58:30 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);
+    }
+}
+