for Bertl2014 HTL-Graz-Göoesting

Steuerung.cpp

Committer:
hemmer_matthias
Date:
2016-01-11
Revision:
1:18c4a2bc1228
Parent:
0:50c6cfc49ead

File content as of revision 1:18c4a2bc1228:

#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 period_ms(int ms) 
{
    MotorL_EN.period_ms(ms);
    MotorR_EN.period_ms(ms);
}

void bertl_engine(int left, int right, int seconds)
{          
    
   /* int index_Engine = 0;
                
    // decleration for left and right engine
         if (left > 0 && right > 0)  
            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.pulsewidth_ms(seconds); 
            MotorL_EN.pulsewidth_ms(seconds);
            MotorL_REVERSE = 0;
            MotorR_REVERSE = 0;
            MotorL_FORWARD = 1;
            MotorR_FORWARD = 1;    
       
        break;
            
        case 2:
            MotorR_EN = 1;
            MotorL_EN = 1;
            MotorL_FORWARD = 0;
            MotorR_REVERSE = 0;
            MotorL_REVERSE = 1;
            MotorR_FORWARD = 1;           
        break;

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