Revision 136:b35f2d9b7402, committed 2017-05-16

Comitter:

aeschsim

Date:

Tue May 16 16:18:29 2017 +0000

Parent:

135:644346924339

Commit message:

changed and added some safety functions

Changed in this revision

--- a/header/Safety.h	Tue May 16 15:24:23 2017 +0000
+++ b/header/Safety.h	Tue May 16 16:18:29 2017 +0000
@@ -3,7 +3,7 @@
 #include "Robot.h"
 #include "Movement.h"
 
-int safety(int state);
+bool safety();
     
 int emergency_shutdown();    
 int colision_detected();  

--- a/source/Main.cpp	Tue May 16 15:24:23 2017 +0000
+++ b/source/Main.cpp	Tue May 16 16:18:29 2017 +0000
@@ -23,7 +23,11 @@
         //This functions will be called every cycle, use for safety and sensor functipons
         //*******************************************************************************
 
-        //state = safety(state);
+        if (safety() && state != 50) { //arm down
+            //irgendwie rückwärts fahren
+            //state = 47
+            state = 61;
+        }
 
 
 

--- a/source/Movement.cpp	Tue May 16 15:24:23 2017 +0000
+++ b/source/Movement.cpp	Tue May 16 16:18:29 2017 +0000
@@ -505,15 +505,23 @@
             }
             rad = (rand()%1 + 1.0f) * 0.7f + 0.3f;
             rad *= sign;
-            dist = (rand()%1 + 1.0f) * 0.5f + 0.5f;
+            dist = (rand()%1 + 1.0f) * 0.5f + 0.2f;
             printf("radius %f || distance %f\r\n", rad, dist);
             move_for_distance_with_radius(dist, rad);
+            //set sensor servos only once because wait is needed
+            set_servo_position(2, 45);
+            set_servo_position(0, -45);
+            wait(0.5f);
             random_state = 1;
             break;
         case 1:
             float dist_ir = getDistanceIR(3);
-            printf("Distance ir3 %f\n\r",dist_ir);
-            if (dist_ir < 0.1f) {
+            float dist_left = 1.0f;
+            float dist_right = 1.0f;
+            dist_left = getDistanceIR(4);
+            dist_right = getDistanceIR(2);
+            printf("Distance left %f || center %f || right %f\n\r",dist_left, dist_ir, dist_right);
+            if (dist_ir < 0.1f || dist_left < 0.1f || dist_right <0.1f) {
                 /*move backwards*/
                 random_state = 2;
             }

--- a/source/Safety.cpp	Tue May 16 15:24:23 2017 +0000
+++ b/source/Safety.cpp	Tue May 16 16:18:29 2017 +0000
@@ -3,36 +3,41 @@
 #include "mbed.h"
 #include "Safety.h"
 
-int safety(int state){
-    if(getDistanceIR(2) < 0.05f && state != 50) {   //state 50: grabbing state
+bool safety()
+{
+    float dist_ir = getDistanceIR(2);
+    if(getDistanceIR(2) < 0.1f) {   //state 50: grabbing state
         stop_move();
         stop_turn();
         printf("Stopped moving to prevent colission!\r\n");
-        return 11;
+        return 1;
     }
-    return state;
+    return 0;
 }
-    
-int emergency_shutdown(){
+
+int emergency_shutdown()
+{
     stop_move();
     stop_turn();
     disable_servos();
     disable_motors();
     return 0;
-    }
-    
-int colision_detected(){
-    
+}
+
+int colision_detected()
+{
+
     return 0;
-    }
-    
-int current_to_high(){
-    
+}
+
+int current_to_high()
+{
+
     return 0;
-    }
-    
-int overheating(){
-    
+}
+
+int overheating()
+{
+
     return 0;
-    }
-    
\ No newline at end of file
+}

--- a/source/StartUp.cpp	Tue May 16 15:24:23 2017 +0000
+++ b/source/StartUp.cpp	Tue May 16 16:18:29 2017 +0000
@@ -57,7 +57,7 @@
         printf("initial turn...\r\n");
         turn_for_deg(180,1);
     } else {
-        printf("turning...\r\n");
+        //printf("turning...\r\n");
         if(turn_for_deg(0,1) < 0) { // check if turning complete
             // printf("turn finished...\r\n");
             //print_map(0);