File content as of revision 5:47262622a9bf:

#include <cmath>
#include "Motion.h"

using namespace std;

const float Motion::SPEEDLEFT = 50.0f;
const float Motion::SPEEDRIGHT = 50.0f;


Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
                EncoderCounter& counterRight, IRSensor& irSensorL,
                IRSensor& irSensorC, IRSensor& irSensorR, 
                DigitalOut& enableMotorDriver) :
                controller(controller), counterLeft(counterLeft),
                counterRight(counterRight), irSensorL(irSensorL),
                irSensorC(irSensorC), irSensorR(irSensorR),
                enableMotorDriver(enableMotorDriver) {
                    
                    countsL = 0;
                    countsR = 0;
                    countsLOld = 0;
                    countsROld = 0 ;
}
               
Motion::~Motion() {}

/**
 * Eine Feldbewegung druchführen
 */
void Motion::move() {
    
        //distanceL = irSensorL.read();
        //distanceC = irSensorC.read();
        //distanceR = irSensorR.read();
        
        while ((countsL - countsLOld)  < 1630 || (countsR - countsROld) > -1630) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            controller.setDesiredSpeedLeft(50.0f);
            controller.setDesiredSpeedRight(-50.0f);
            enableMotorDriver = 1;
        }    
        //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
        
        stop();
        enableMotorDriver = 0;
        countsLOld = countsL + 150;
        countsROld = countsR - 150;
}

/**
 * 90° Rotation nach Links
 */
void Motion::rotateL() {
    
        while ((countsL - countsLOld)  > -760 || (countsR - countsROld) > -760) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            controller.setDesiredSpeedLeft(-50.0f);
            controller.setDesiredSpeedRight(-50.0f);
            enableMotorDriver = 1;
        }    
        
        stop();
        enableMotorDriver = 0;
        countsLOld = countsL - 100;
        countsROld = countsR - 100;
}
    
/**
 * 90° Rotation nach Rechts
 */
void Motion::rotateR() {
    
        while ((countsL - countsLOld)  < 760 || (countsR - countsROld) < 760) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            controller.setDesiredSpeedLeft(50.0f);
            controller.setDesiredSpeedRight(50.0f);
            enableMotorDriver = 1;
        }    
        
        stop();
        enableMotorDriver = 0;
        countsLOld = countsL + 100;
        countsROld = countsR + 100;
}

/**
 * Motor Stop
 */
void Motion::stop() {
       
        controller.setDesiredSpeedLeft(0.0f);
        controller.setDesiredSpeedRight(0.0f);
}
/**
 * 180° Rotation
 */
void Motion::rotate180() {
    
        while ((countsL - countsLOld)  > -1560 || (countsR - countsROld) > -1560) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            controller.setDesiredSpeedLeft(-50.0f);
            controller.setDesiredSpeedRight(-50.0f);
            enableMotorDriver = 1;
            printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
        }    
        
        stop();
        enableMotorDriver = 0;
        countsLOld = countsL - 150;
        countsROld = countsR - 150;
        
}

void Motion::test() {
        
        while (countsL > -1560 || countsR > - 1560) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            controller.setDesiredSpeedLeft(-50.0f);
            controller.setDesiredSpeedRight(-50.0f);
            enableMotorDriver = 1;
            printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);
        }
        
        counterLeft.reset();
        counterRight.reset();
        stop();
        enableMotorDriver = 0;
}