for Bertl2014 HTL-Graz-Göoesting

Steuerung.cpp

Committer:
hemmer_matthias
Date:
2015-12-29
Revision:
0:50c6cfc49ead
Child:
1:18c4a2bc1228

File content as of revision 0:50c6cfc49ead:

#include "mbed.h"

// decleration for left engine
PwmOut MotorL_EN(P1_15);    
DigitalOut MotorL_FORWARD(P1_1);   
DigitalOut MotorL_REVERSE(P1_0); 
  
// decleration for right engine
PwmOut MotorR_EN(P0_21);     
DigitalOut MotorR_FORWARD(P1_3); 
DigitalOut MotorR_REVERSE(P1_4);

void bertl_engine (int left, int right)
{    
    MotorL_EN = MotorR_EN = 1;      
    
    int index_Engine = 0;
    
         if (left > 0 && right > 0)  // decleration for left and right engine
            index_Engine = 1;
        
        else if (left < 0 && right > 0)
            index_Engine = 2;
    
        else if (left > 0 && right < 0)
            index_Engine = 3;
        
        else if (left < 0 && right < 0)
            index_Engine = 4;
    
        else if (left == 0 && right < 0)
            index_Engine = 5;
     
        else if (left == 0 && right > 0)
             index_Engine = 6;    
        
        else if (left < 0 && right == 0)
             index_Engine = 7;
    
        else if (left > 0 && right == 0)
             index_Engine = 8;
        else
            index_Engine = 9;    
             
switch (index_Engine)  //begin Switch
   { 
        case 1:
            MotorR_EN = 1;
            MotorL_EN = 1;
            MotorL_FORWARD = 1;
            MotorR_FORWARD = 1;           
       
        break;
            
        case 2:
            MotorR_EN = 1;
            MotorL_EN = 1;
            MotorL_REVERSE = 1;
            MotorR_FORWARD = 1;           
        break;

        case 3:
            MotorR_EN = 1;
            MotorL_EN = 1;
            MotorL_FORWARD = 1;
            MotorR_REVERSE = 1;            
        break;
            
        case 4:
            MotorR_EN = 1;
            MotorL_EN = 1;
            MotorL_REVERSE = 1;
            MotorR_REVERSE = 1;       
        break;  
            
        case 5:
            MotorL_EN = 0;
            MotorR_EN = 1;
            MotorR_REVERSE = 1;
        break;  
        
        case 6:
            MotorL_EN = 0;
            MotorR_EN = 1;
            MotorR_FORWARD = 1;
        break; 
    
         case 7:
            MotorR_EN = 0;
            MotorR_EN = 1;
            MotorL_REVERSE = 1;          
        break;    
 
         case 8:
            MotorR_EN = 0;
            MotorL_EN = 1;
            MotorL_FORWARD = 1;                    
         break;    
         
        case 9:
            MotorR_EN = 0;
            MotorL_EN = 0;
         break;
    }//end Switch 
}//end bertl_engine