Diff: main.cpp

Revision:

2:f381fc3a8eaf

Parent:

1:8b600c187fe6

diff -r 8b600c187fe6 -r f381fc3a8eaf main.cpp
--- a/main.cpp	Wed Feb 26 12:49:12 2020 +0000
+++ b/main.cpp	Wed Feb 26 14:20:16 2020 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "IRSensor.h"
+#include "Controller.h"
 
 DigitalOut myled(LED1);
 DigitalIn mybutton(BUTTON1); 
@@ -20,9 +21,10 @@
 DigitalOut en_Motor(PG_0);
 DigitalIn Error_Motor(PD_1);
 DigitalIn Warn_Motor(PD_0); 
-PWMOut MotorLeft(PF_9); 
-PWMOut MotorRight(PF_8); 
-
+PwmOut MotorPWMLeft(PF_9); 
+PwmOut MotorPWMRight(PF_8); 
+EncoderCounter cnt_Left(PD_12,PD_13);
+EncoderCounter cnt_Right(PB_4, PC_7);
 
 IRSensor S_hinten(v_distance, bit0 , bit1, bit2, 0);
 IRSensor S_hinten_links(v_distance, bit0 , bit1, bit2, 1);
@@ -31,57 +33,59 @@
 IRSensor S_vorne_rechts(v_distance, bit0 , bit1, bit2, 4);
 IRSensor S_hinten_rechts(v_distance, bit0 , bit1, bit2, 5);
 
+Controller controller(MotorPWMLeft, MotorPWMRight, cnt_Left, cnt_Right);
 int main() { 
     //Set Motor Settings
-    MotorLeft.period(0.00005); 
-    MotorRight.period(0.00005); 
-    
-    MotorLeft = 0.5; 
-    MotorRight = 0.5;
-
     en_Motor = 1; 
     
-    //Set IR Setting
+    //Set IR Settings
     enable = 1; 
     
 
     while(1) {
-        float distance0 = S_hinten.read();  // gibt die Distanz in [m] zurueck  
-        float distance1 = S_hinten_links.read();   
-        float distance2 = S_vorne_links.read();   
-        float distance3 = S_vorne.read();   
-        float distance4 = S_vorne_rechts.read();   
-        float distance5 = S_hinten_rechts.read();
+        float dist_hinten = S_hinten.read();  // gibt die Distanz in [m] zurueck  
+        float dist_hinten_links = S_hinten_links.read();   
+        float dist_vorne_links= S_vorne_links.read();   
+        float dist_vorne = S_vorne.read();   
+        float dist_vorne_rechts = S_vorne_rechts.read();   
+        float dist_hinten_rechts = S_hinten_rechts.read();
+       
         
-        if(distance0 < 0.2f){
+        if(dist_hinten < 0.2f){
             led0 = 1; 
         }else{
             led0 = 0;    
         }
-        if(distance1 < 0.2f){
+        if(dist_hinten_links < 0.2f){
             led1 = 1; 
         }else{
             led1 = 0;    
         }
-        if(distance2 < 0.2f){
+        if(dist_vorne_links < 0.2f){
             led2 = 1; 
         }else{
             led2 = 0;    
         }
-        if(distance3 < 0.2f){
+        if(dist_vorne < 0.2f){
             led3 = 1; 
         }else{
             led3 = 0;    
         }
-        if(distance4 < 0.2f){
+        if(dist_vorne_rechts < 0.2f){
             led4 = 1; 
         }else{
             led4 = 0;    
         }
-        if(distance5 < 0.2f){
+        if(dist_hinten_rechts < 0.2f){
             led5 = 1; 
         }else{
             led5 = 0;    
         }
+        
+        controller.setDesiredSpeedLeft(50.0); 
+        controller.setDesiredSpeedRight(-50.0); 
+        
+        printf("Soll Links:%f    Ist Links%f    Soll Rechts:%f     Ist Rechts:%f \n\r",controller.getDesSpeedLeft(),controller.getSpeedLeft(),controller.getDesSpeedRight(),controller.getSpeedRight());
+        
     }
 }