File content as of revision 36:99f60052c746:

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

using namespace std;

const float Motion::LEFT_MID_VAL = 40.73f; //44.73
const float Motion::RIGHT_MID_VAL = 43.03f; //47.03
const float Motion::KP = 2.0;
const float Motion::KD = 0.0f;
const int Motion::MOVE_DIST = 1620;
const float Motion::MOVE_SPEED = 50.0f;
const float Motion::RUN_SPEED = 70.0f;
const float Motion::ROTATE_SPEED = 80.0f;
const float Motion::ACCEL_CONST = 4.0f; //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() {}

/**
 * Carry out move for one field
 */
void Motion::move() {
        
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
            
        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;}
                
            avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
        
            if (speedRun == true) accel(RUN_SPEED);
            else accel(MOVE_SPEED);
            control();
            
            //Stop at certainn distance before wall
            if ((distanceC) < 40.0f && speedRun == false) {
                countsLOld = countsL;
                countsROld = countsR;
                
                while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
                    countsL = counterLeft.read();
                    countsR = counterRight.read();
                    distanceC = irSensorC.readC(); 
                    
                    if (speedRun == true) accel(RUN_SPEED);
                    else accel(MOVE_SPEED);
                    control();
                    
                    if (distanceC > 40.0f) {
                        stop();
                        break;
                    }
                }
                stop();
                break;    
            
            //Stop at certain distance in speed run
          /*  }else if (distanceC < 140.0f && speedRun == true && lastMove == true) { //Ursprünglich: 180.0f 
                stop();
                break;   
            */
            //Correct distance from wall
            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC <= 130.0f) && (distanceC > 40.0f)) {
                countsLOld += 1000;
                countsROld -= 1000; 
                  
            //Stop after certain distance if side wall and front wall missing
            }else if ( deceleration == true && speedRun == false && ( distanceL > 80.0f || distanceR > 80.0f ) && distanceC > 180.0f ) { //Urspünglich 200.0f

                controller.counterReset();
                countsLOld = counterLeft.read();
                countsROld = counterRight.read();
                countsL = counterLeft.read();
                countsR = counterRight.read();
                
                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 400.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 400.0f) {
                    countsL = counterLeft.read();
                    countsR = counterRight.read(); 
                    
                    deceleration = true;
                    acceleration = false;
                    
                    accel(MOVE_SPEED);
                    control();
                }
                stop();
                break;
            }
            
        }    
        
        t.stop();
        t.reset();
        lastMove = false;
        acceleration = false;
        deceleration = false;
}

/**
 * Carry out move for a half field
 */
void Motion::moveHalf() {
        
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
            
        t.start();
        
        while ((countsL - countsLOld)  < 824 || (countsR - countsROld) > -824) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
            distanceC = irSensorC.readC();
            distanceL = irSensorL.readL();
            distanceR = irSensorR.readR();
            
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
                
            avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
        
            accel(RUN_SPEED);
            control();
            
            //Stop ceratin distance before wall
            if (distanceC < 100.0f && lastMove == false) {
                break;    
            
            }else if ( ((countsL - countsLOld)  >= 824 || (countsR - countsROld) <= -824) && (distanceC >= 100.0f) && (distanceC < 120.0f) )  {
                countsLOld += 100;
                countsROld -= 100; 
                       
            //Stop after certain distance if side wall missing
            }else if ( (distanceL > 80.0f || distanceR > 80.0f) && lastMove == false && deceleration == true && distanceC < 190.0f) {

                controller.counterReset();
                countsLOld = counterLeft.read();
                countsROld = counterRight.read();
                countsL = counterLeft.read();
                countsR = counterRight.read();
                
                while ((countsL - countsLOld)  <  280.0f || (countsR - countsROld) > -280.0f) { //Ursprünglich 250.0f
                    countsL = counterLeft.read();
                    countsR = counterRight.read(); 
                    accel(RUN_SPEED); 
                    control();
                }
                stop();
                break; 
                
            //Stop before wall at target field
            }else if (distanceC < 40.0f && lastMove == true) {
                stop();
                break;
            }
            
        }    
        
        t.stop();
        t.reset();
        lastMove = false;
        acceleration = false;
        deceleration = false;
}

/**
 * Carry out move for one field with finish line detection
 */
void Motion::scanMove() {
        
        acceleration = false;
        deceleration = false;
        longMove = false;
        
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        bool sideWalls = false;
        
        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();
            
            //for distance correcture with side wall
            if (distanceL < 80.0f && distanceR < 80.0f) sideWalls = true;
            
            if (enableMotorDriver == 0) {
                enableMotorDriver = 1;
            }
            
            if (lineSensor.read() > 0.85f) {
                line = 1;  
            }
            
            
            accel(MOVE_SPEED);
            control();
            
            if ((distanceC) < 40.0f) {
                countsLOld = countsL;
                countsROld = countsR;
                while ((countsL - countsLOld)  <  70 || (countsR - countsROld) > -70) {
                    countsL = counterLeft.read();
                    countsR = counterRight.read(); 
                    if (distanceC > 40.0f) {
                        stop();
                        break;
                    }
                }
                
                stop();
                break;   
                 
            }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
                countsLOld += 500;
                countsROld += 500;   
            
            //Stop after certain distance if side wall and front wall missing
            }else if (sideWalls == true && speedRun == false && ( distanceL > 80.0f || distanceR > 80.0f ) && distanceC > 200.0f ) {
                countsLOld = counterLeft.read();
                countsROld = counterRight.read();
                
                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 320.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 320.0f) {
                    countsL = counterLeft.read();
                    countsR = counterRight.read(); 
                    
                    if (speedRun == true) accel(RUN_SPEED);
                    else accel(MOVE_SPEED);
                    control();
                }
                break;
            }
        } 
         
        t.stop();
        t.reset();
        lastMove = false;
}
/**
 * 90° Rotation left
 */
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 right
 */
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();
}
/**
 * Turn left
 */
void Motion::turnL() {     
        
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        controller.setDesiredSpeedLeft(24.15f);
        controller.setDesiredSpeedRight(-115.85f);  
        
        /*  Velocity Settings:
            50rpm   ->  17.25   :   82.75
            60rpm   ->  20.7    :   99.3
            70rpm   ->  24.15   :   115.85
            80rpm   ->  27.6    :   132.4
            90rpm   ->  31.05   :   148.95
            100rpm  ->  34.5    :   165.5
        */
        
        while ((countsL - countsLOld)  < 446 || (countsR - countsROld) > -2141) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
        }         
}
/**
 * Turn right
 */
void Motion::turnR() {    
        
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        controller.setDesiredSpeedLeft(115.85f);                                            
        controller.setDesiredSpeedRight(-24.15f);
        
        while ((countsL - countsLOld)  < 2141 || (countsR - countsROld) > -446) {
            
            countsL = counterLeft.read();
            countsR = counterRight.read();
        
            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
        }        
}
/**
 * Motor Stop
 */
void Motion::stop() {
        
        controller.setDesiredSpeedLeft(0.0f);
        controller.setDesiredSpeedRight(0.0f);
        actSpeed = 0.0f;
        
        float sL = controller.getSpeedLeft();
        float ticks = 0.08f*sL;
        
        waitStop = 0;
        while( waitStop < ticks) {
            controller.setDesiredSpeedLeft(0.0f);
            controller.setDesiredSpeedRight(0.0f);
            waitStop+= 1;
        }
}
/**
 * 180° Rotation
 */
void Motion::rotate180() {
        //1723
        stop();
        controller.counterReset();
        countsLOld = counterLeft.read();
        countsROld = counterRight.read();
        countsL = counterLeft.read();
        countsR = counterRight.read();
        
        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 = 47.0f; //48.73
        float wallRight = 44.0f; //51.03f;
        
        distanceL = irSensorL.readL();
        distanceR = irSensorR.readR();
        
        if (distanceL < wallLeft && distanceR > wallRight) {
                
            errorP = distanceL - wallLeft;
            
        }else if (distanceL > wallLeft && distanceR < wallRight) {
                
            errorP = wallRight - distanceR;
            
        }else if (distanceL < wallLeft+10.0f && distanceL > wallLeft && distanceR > wallRight) {
                
            errorP = distanceL - wallLeft;
            
        }else if (distanceR < wallRight+10.0f && distanceL > wallLeft && distanceR > wallRight) {
                
            errorP = wallRight - distanceR;
            
        }else{
            
            errorP = 0; 
            errorD = 0; 
        }
        
        errorD = errorP - oldErrorP;  
        
        oldErrorP = errorP;
        
        if (abs(errorP) < 80.0f) {
            totalError = KP*errorP + KD*errorD;
        }
        
        controller.setDesiredSpeedLeft(actSpeed - totalError);
        controller.setDesiredSpeedRight(-actSpeed - totalError);
}

void Motion::runTask(int path[],int task, bool reverse, int junction) {
        
        //reverse happens only in search run
        if (reverse == false) speedRun = true;
        else speedRun = false;
      
        switch(path[task]) {
            
            case 1:
            
                //Acceleration
                if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction && task-1 != junction) {     //if next move() and previous no move() and step no junction
                    
                    acceleration = true;
                    longMove = true;
                    deceleration = false;
                    
                }else if (reverse == false && path[task+1] == path[task] && ( path[task-1] != path[task] || task == 0 || path[task-1] != 4) ) {  //same as above, also if start field
                    
                    acceleration = true;
                    longMove = true;
                    deceleration = false;
                    
                }else{
                    acceleration = false; 
                    avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
                }
                
                //Deceleration
                if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) {  //next step no move() or junction and speed above 120rpm
                    
                    deceleration = true;
                    acceleration = false;
                    lastMove = true;
                    longMove = false;
                    
                }else if (reverse == false && path[task+1] != path[task] && path[task+1] != 4 && avgSpeed > 2.4f*MOVE_SPEED) { //next step no move() and no moveHalf() and speed above 120rpm
                    
                    deceleration = true;
                    acceleration = false;
                    lastMove = true;
                    longMove = false;
                    
                }else if (reverse == false && path[task+1] != path[task] && path[task+1] == 4 && avgSpeed > 2.4f*MOVE_SPEED) {
                    
                    lastMove = true;
                    
                }else{
                    deceleration = false;
                }
                
                //printf("\nSchritt: %d Befehl: %d Reverse: %d acceleration: %d deceleration: %d\n", task, path[task], reverse, acceleration, deceleration);
                //printf("\nVor: %d Nach: %d Speed: %f\n\n", path[task+1], path[task-1], avgSpeed);
                
                move();
                break;
            case 2:
                rotateL();
                break;
            case 3:
                rotateR();
                break;
            case 4:
            
                if (reverse == false && path[task] == 4 && path[task+1] == 1) {     //accelerate if next step is move()
                    
                    acceleration = true;
                    longMove = true;
                    deceleration = false;
                    
                }else{
                    acceleration = false; 
                    avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
                }
                
                if (reverse == false && path[task-1] == 1 && path[task] == 4 && path[task+1] != 0) {    //decelerate if previous step was move()
                    
                    deceleration = true;
                    acceleration = false;
                    longMove = false;
                    
                }else{
                    deceleration = false;
                }
                
                if (reverse == false && path[task+1] == 0) {
                    
                    lastMove = true;
                }
                
                moveHalf();
                break; 
            case 5:
                turnL();
                break;
            case 6:
                turnR();
                break;
            case 7:
                break;   
        }
}

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


void Motion::accel(float targetSpeed) { 

        float fastSpeed;
        
        //Acclerated Target Speed
        if (speedRun == true) fastSpeed = targetSpeed*2.0f;
        else fastSpeed = targetSpeed*2.8f;
        
        avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
        
        //Acceleration logic
        
        //Short distance
        if (avgSpeed < targetSpeed && deceleration == false && acceleration == false && longMove == false) {
                
            actSpeed = ACCEL_CONST * t.read()*60.0f;                //Acceleration equation
            
        }else if(avgSpeed > targetSpeed+5.0f && deceleration == false && acceleration == false && longMove == false) {
            
            actSpeed =  targetSpeed+5.0f;                           //Keep Speed steady in case of overshooting
            
        //Long distance
        }else if (avgSpeed < fastSpeed && acceleration == true && deceleration == false) {
            
            actSpeed = ACCEL_CONST * t.read()*60.0f;    
               
        }else if (avgSpeed > targetSpeed+5.0f && acceleration == false && deceleration == true) {
        
            actSpeed = fastSpeed - ACCEL_CONST * t.read()*60.0f;    //Deceleration equation
            
        }else if (avgSpeed < targetSpeed && acceleration == false && deceleration == true) {
            
            actSpeed = targetSpeed+5.0f;                            //Limit deceleration in case of overshooting
            
        }    
}