File content as of revision 14:2926932e26df:

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

using namespace std;

const float Motion::LEFT_MID_VAL = 41.73f; //44.73
const float Motion::RIGHT_MID_VAL = 44.03f; //47.03
const float Motion::KP = 2.5;
const float Motion::KD = 0.0f;
const int Motion::MOVE_DIST = 1605;
const float Motion::MOVE_SPEED = 130.0f;
const float Motion::SCAN_SPEED = 50.0f;
const float Motion::ROTATE_SPEED = 80.0f;
const float Motion::ACCEL_CONST = 2.212f;


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() {
        
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        speedLeft = MOVE_SPEED;
        speedRight = -MOVE_SPEED;
        
        controller.setDesiredSpeedLeft(actSpeed);
        controller.setDesiredSpeedRight(-actSpeed);
        
        t.start();
        
        while ((countsL - countsLOld)  < MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
            distanceC = irSensorC.readC();
            distanceL = irSensorL.readL();
            distanceR = irSensorR.readR();
            
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
            
            control();
            accel(MOVE_SPEED);
            /*
            accel(MOVE_SPEED);
            controller.setDesiredSpeedLeft(actSpeed);
            controller.setDesiredSpeedRight(-actSpeed);
            */
            
            if ((distanceC) < 40.0f) {
                countsLOld = countsL;
                countsROld = countsR;
                while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
                    countsL = counterLeft.read();
                    countsR = counterRight.read(); 
                }
                stop();
                break;    
            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 130.0f) && (distanceC > 40.0f)) {
                countsLOld += 500;
                countsROld += 500;   
            }/* else if ( distanceL < 80.0f || distanceR < 80.0f ) {
                countsLOld = countsL;
                countsROld = countsR;
                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f || (countsR - countsROld) > -0.5f*MOVE_DIST) {
                    countsL = counterLeft.read();
                    countsR = counterRight.read(); 
                }
                stop();
                break;
            }*/
            
        }    
        
        t.stop();
        t.reset();
}
/**
 * Eine Feldstrecke mit höherer Geschwindigkeit fahren
 */
void Motion::moveFast() {
        
        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();
            distanceC = irSensorC.readC();
            
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
            
            control();
            
            if (distanceC < 40.0f) {
                stop();
                break;    
            }
        }    
}
/**
 * Eine Feldstrecke mit überprüfung der Ziellinie fahren
 */
void Motion::scanMove() {
        
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        speedLeft = SCAN_SPEED;
        speedRight = -SCAN_SPEED;
        
        controller.setDesiredSpeedLeft(actSpeed);
        controller.setDesiredSpeedRight(-actSpeed);
        acceleration = true;
        
        distanceC = irSensorC.readC();
        t.start();
        
        while ((countsL - countsLOld)  <  MOVE_DIST || (countsR - countsROld) > -MOVE_DIST) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
            distanceC = irSensorC.readC();
            distanceL = irSensorL.readL();
            distanceR = irSensorR.readR();
            
            if (enableMotorDriver == 0) {
                enableMotorDriver = 1;
            }
            
            if (lineSensor.read() == 1.0f) {
                line = 1;  
            }
            
            
            control();
            accel(SCAN_SPEED);
            
            //controller.setDesiredSpeedLeft(actSpeed);
            //controller.setDesiredSpeedRight(-actSpeed);
            
            if ((distanceC) < 40.0f) {
                countsLOld = countsL;
                countsROld = countsR;
                while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
                    countsL = counterLeft.read();
                    countsR = counterRight.read(); 
                }
                stop();
                break;    
            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
                countsLOld += 500;
                countsROld += 500;   
            }/*else if ( distanceL > 80.0f || distanceR > 80.0f ) {
                countsLOld = countsL;
                countsROld = countsR;
                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 330.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 330.0f) {
                    countsL = counterLeft.read();
                    countsR = counterRight.read(); 
                }
                stop();
                break;
            }*/
        }  
        t.stop();
        t.reset();
        acceleration = false;
}
/**
 * 90° Rotation nach Links
 */
void Motion::rotateL() {
    
        stop();
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        controller.setDesiredSpeedLeft(-ROTATE_SPEED);
        controller.setDesiredSpeedRight(-ROTATE_SPEED);
        
        while ((countsL - countsLOld)  > -870 || (countsR - countsROld) > -870) {
            
            //accel();
            
            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(ROTATE_SPEED);
        controller.setDesiredSpeedRight(ROTATE_SPEED);
        
        while ((countsL - countsLOld)  < 870 || (countsR - countsROld) < 870) {
            
            //accel();
            
            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() {
        //1723
        stop();
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        //controller.setDesiredSpeedLeft(-ROTATE_SPEED);
        //controller.setDesiredSpeedRight(-ROTATE_SPEED);
        
        t.start();
        
        while ((countsL - countsLOld)  > -900 || (countsR - countsROld) > -900) {
            
            actSpeed = 3.5f * t.read()*60.0f; 
            
            controller.setDesiredSpeedLeft(-actSpeed);
            controller.setDesiredSpeedRight(-actSpeed);
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
        }    
        
        t.reset();

        avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
        
        while ((countsL - countsLOld)  > -1720 || (countsR - countsROld) > -1720) {
            
            actSpeed = avgSpeed + (-3.5f * t.read()*60.0f);
            
            controller.setDesiredSpeedLeft(-actSpeed);
            controller.setDesiredSpeedRight(-actSpeed);
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
        }            
        t.stop();
        t.reset();
        stop();
}


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

void Motion::runTask(int path[],int task, bool reverse) {
      
        switch(path[task]) {
            
            case 1:
                if (reverse == true && path[task+1] == path[task]) {
                    acceleration = true;
                }else{
                    acceleration = false;
                }
                
                if (reverse == false && path[task-1] == path[task]) {
                    acceleration = true;   
                }else{
                    acceleration = false;  
                }
            
                if (reverse == true && path[task-1] != path[task]) {
                    deceleration = true;
                }else{
                    deceleration = false;
                }
                
                if (reverse == false && path[task+1] != path[task]) {
                    deceleration = true;
                }else{
                    deceleration = false;
                }
                move();
                break;
            case 2:
                rotateL();
                break;
            case 3:
                rotateR();
                break;
            case 4:
                moveFast();
                break;  
            case 5:
                stop();
                break;  
        }
}

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


void Motion::accel(float targetSpeed) { 

        

        avgCounts = ( abs(countsL - countsLOld) + abs(countsR - countsROld)) * 0.5f;
        avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;

        if ( avgSpeed < targetSpeed && acceleration == true) {
                
            actSpeed = ACCEL_CONST * t.read()*60.0f;   
        }
            
        if ( MOVE_DIST - avgCounts < targetSpeed*2 && deceleration == true) {
        
            actSpeed = -ACCEL_CONST * (avgCounts - (MOVE_DIST - targetSpeed*2)) + targetSpeed;
        }
            
            
}