StateMachine.cpp

00001 
00002 // Statemachine
00003 //V04.18
00004 // V. Ahlers
00005 
00006 #include <mbed.h>
00007 #include "StateMachine.h"
00008 
00009 using namespace std;
00010 
00011 StateMachine::StateMachine(int IrR, int IrL, int IrF) : IrR(IrR), IrL(IrL), IrF(IrF) {}
00012 
00013 StateMachine::~StateMachine (){}
00014 
00015 int StateMachine :: drive() {
00016     
00017     if((IrR==0) && (IrL==0) && (IrF==1)){
00018         caseDrive = 2;
00019     }else { if ((IrR==0) && (IrL==1) && (IrF==0)){
00020         caseDrive = 2;
00021     }else { if ((IrR==0) && (IrL==1) && (IrF==1)){
00022         caseDrive = 2;
00023     }else { if ((IrR==1) && (IrL==0) && (IrF==0)){
00024         caseDrive = 1;
00025     }else { if ((IrR==1) && (IrL==1) && (IrF==1)){
00026         caseDrive = 3;
00027     }else { if ((IrR==1) && (IrL==1) && (IrF==0)){
00028         caseDrive = 1;
00029     }else { if ((IrR==1) && (IrL==1) && (IrF==1)){
00030         caseDrive = 4;
00031     }else{ caseDrive=0;
00032     }}}}}}}
00033 
00034     return caseDrive;
00035 }