File content as of revision 3:076dd7ec7eb4:

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

using namespace std;

const float Motion::LEFT_MID_VAL = 44.73f;
const float Motion::RIGHT_MID_VAL = 47.03f;
const float Motion::KP = 3.0f;
const float Motion::KD = 0.00f;


Motion::Motion(Controller& controller, EncoderCounter& counterLeft, 
                EncoderCounter& counterRight, IRSensor& irSensorL,
                IRSensor& irSensorC, IRSensor& irSensorR, AnalogIn& lineSensor,
                DigitalOut& enableMotorDriver) :
                controller(controller), counterLeft(counterLeft),
                counterRight(counterRight), irSensorL(irSensorL),
                irSensorC(irSensorC), irSensorR(irSensorR), lineSensor(lineSensor),
                enableMotorDriver(enableMotorDriver) {}
               
Motion::~Motion() {}

/**
 * Eine Feldstrecke druchführen
 */
void Motion::move() {
        
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        speedLeft = 50.0f;
        speedRight = -50.0f;
        
        controller.setDesiredSpeedLeft(speedLeft);
        controller.setDesiredSpeedRight(speedRight);
        
        while ((countsL - countsLOld)  < 1647 || (countsR - countsROld) > -1647) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
            
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
            
            control();
            
        }    
}
/**
 * Eine Feldstrecke mit höherer Geschwindigkeit fahren
 */
void Motion::moveFast() {
        
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        speedLeft = 100.0f;
        speedRight = -100.0f;
        
        controller.setDesiredSpeedLeft(speedLeft);
        controller.setDesiredSpeedRight(speedRight);
        
        while ((countsL - countsLOld)  < 1647 || (countsR - countsROld) > -1647) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
            
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
            
            control();
            
        }    
}
/**
 * Eine Feldstrecke mit überprüfung der Ziellinie fahren
 */
void Motion::scanMove() {
        
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        speedLeft = 50.0f;
        speedRight = -50.0f;
        
        controller.setDesiredSpeedLeft(speedLeft);
        controller.setDesiredSpeedRight(speedRight);
        
        while ((countsL - countsLOld)  <  1647 || (countsR - countsROld) > -1647) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
            distanceC = irSensorC.readC();
            
            if (enableMotorDriver == 0) {
                enableMotorDriver = 1;
            }
            
            if (lineSensor.read() == 1.0f) {
                line = 1;  
            }else{
                line = 0;
            }
            
            control();
            
            if (distanceC < 40.0f) {
                stop();
                break;    
            }
            
        }    
        
}
/**
 * 90° Rotation nach Links
 */
void Motion::rotateL() {
    
        stop();
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        controller.setDesiredSpeedLeft(-20.0f);
        controller.setDesiredSpeedRight(-20.0f);
        
        while ((countsL - countsLOld)  > -862 || (countsR - countsROld) > -862) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
            
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
        }    
        
        stop();
}
    
/**
 * 90° Rotation nach Rechts
 */
void Motion::rotateR() {
    
        stop();
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        controller.setDesiredSpeedLeft(20.0f);
        controller.setDesiredSpeedRight(20.0f);
        
        while ((countsL - countsLOld)  < 862 || (countsR - countsROld) < 862) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
        }    
        
        stop();
}
/**
 * Links abbiegen
 */
void Motion::turnL() {     
        
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        controller.setDesiredSpeedLeft(17.0f);
        controller.setDesiredSpeedRight(-83.0f);  
        
        while ((countsL - countsLOld)  < 440 || (countsR - countsROld) > -2148) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
        }         
}
/**
 * Rechts abbiegen
 */
void Motion::turnR() {    
        
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        controller.setDesiredSpeedLeft(83.0f);
        controller.setDesiredSpeedRight(-17.0f);
        
        while ((countsL - countsLOld)  < 2148 || (countsR - countsROld) > -440) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
        }        
}
/**
 * Motor Stop
 */
void Motion::stop() {
        
        controller.setDesiredSpeedLeft(0.0f);
        controller.setDesiredSpeedRight(0.0f);
        
        float sL = controller.getSpeedLeft();
        float ticks = 0.08f*sL;
        
        waitStop = 0;
        while( waitStop < ticks) {
            waitStop+= 1;
        }
}
/**
 * 180° Rotation
 */
void Motion::rotate180() {
    
        stop();
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        controller.setDesiredSpeedLeft(-50.0f);
        controller.setDesiredSpeedRight(-50.0f);
        
        while ((countsL - countsLOld)  > -1723 || (countsR - countsROld) > -1723) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
        }    
        
        stop();
}


void Motion::control() {
        
    //float wallLeft = 70.0f;
    //float wallRight = 70.0f;
    
    distanceL = irSensorL.readL();
    distanceR = irSensorR.readR();
    
    if (distanceL < LEFT_MID_VAL && distanceR > RIGHT_MID_VAL) {
        
        errorP = LEFT_MID_VAL - distanceL;
           
    }else if (distanceL > LEFT_MID_VAL && distanceR < RIGHT_MID_VAL) {
        
        errorP = distanceR - RIGHT_MID_VAL;
         
    }else{
        
        errorP = 0;   
    }
    
    errorD = errorP - oldErrorP;  
    
    oldErrorP = errorP;
    
    totalError = KP*errorP + KD*errorD;
    
    controller.setDesiredSpeedLeft(speedLeft + totalError);
    controller.setDesiredSpeedRight(speedRight + totalError);
}

void Motion::runTask(int task) {
      
    switch(task) {
        
        case 1:
            move();
            break;
        case 2:
            rotateL();
            break;
        case 3:
            rotateR();
            break;
        case 4:
            moveFast();
            break;  
        case 5:
            stop();
            break;  
    }
}

int Motion::finish() {
    
    return line;   
}