Diff: Sources/main.cpp

Revision:

4:67d7177c213f

Parent:

2:01e9de508316

Child:

5:1aaf5de776ff

--- a/Sources/main.cpp	Wed Mar 22 13:33:07 2017 +0000
+++ b/Sources/main.cpp	Wed Apr 19 12:23:52 2017 +0000
@@ -1,9 +1,10 @@
 #include "mbed.h"
 #include "Robot.h"
+#include "Declarations.h"
 
 #include <cstdlib>
 
-//DistanceSensors related:
+//DistanceSensors related bottom:
 Serial pc(SERIAL_TX, SERIAL_RX);
 
 AnalogIn sensorVoltage(PB_1);
@@ -12,6 +13,11 @@
 DigitalOut bit1(PC_2);
 DigitalOut bit2(PC_3);
 
+//DistanceSensors top:
+AnalogIn frontS(A1);
+AnalogIn leftS(A2);
+AnalogIn rightS(A3);
+
 //Leds related:
 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
 
@@ -23,14 +29,17 @@
 DigitalIn       errorSignal(PB_14);
 DigitalIn       overtemperatur(PB_15);
 
+//Arm:
+//Servo arm(PC_7);
+
 //Farbsensor:
 AnalogIn FarbVoltage(A0);
 
-Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage ); //Implement the Farbsensor into the Robot init function!!
+Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS ); //Implement the Farbsensor into the Robot init function!!
 
 void initializeDistanceSensors(){
-    for( int ii = 0; ii<6; ++ii) {
-        sam.sensors[ii].init(&sensorVoltage, &bit0, &bit1, &bit2, ii);
+    for( int ii = 0; ii<9; ++ii) {
+        sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
 
     enable = 1;
     }
@@ -38,19 +47,57 @@
 
 /* */
 int main(){
-    Timer t;
-    t.start();
+    initializeDistanceSensors();        //Initialises IR-Sensors.
+    int counter = 0;                    //Counts how many times the robot has turned, before driving
+    int timer = 0;                      //Is used, to reset the robot. Returns time in [0.1s]
+    //int lastFun = -1;                   //Is used, to check if the same action in Robot::search() was made multiple times.
+    int found = 0;                      //0:= no block available, 1 := a lego is ready to be picked up
     
-    initializeDistanceSensors();
-    int counter = 0;                //counts how many times the robot has turned, before driving
+    while( 1 ){
+        if ( timer > TIMEOUT ){
+            NVIC_SystemReset();         //Resets Sam.
+        }
+        else if (found == 0){
+            sam.search(&counter, &timer, &found);
+        }
+        else{
+            //pick up lego
+            //found = 0;
+        }
         
-    while( 1 ){
-        if ( t.read() > TIMEOUT ){
-            NVIC_SystemReset();     //Resets Sam.
-        }
-            
-        sam.search(&counter, &t);
         wait( 0.1f );
     }
     return 0;
-}
\ No newline at end of file
+}
+
+/* * /
+int main(){
+    
+    initializeDistanceSensors();
+    sam.stop();
+    
+    while ( 1 ){
+        for (int i=0; i<6; i++){
+            sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0;
+        }
+        
+        sam.turnLeftS();
+        
+        while (sam.sensors[FWD] < NEAR){
+            wait(0.05f);
+            sam.stop();
+        }
+        
+    }
+    return 0;
+}
+/* */
+
+/* * /
+int main(){
+    for(float p=0; p<1.0; p += 0.1) {
+      //  arm = p;
+        wait(0.2);
+    }
+}
+/* */
\ No newline at end of file