File content as of revision 2:70607c6dad36:

#include "MotorController.h"

MotorController::MotorController (HBridge &motor, AnalogIn &motorInput) :
    motor ( motor ), motorInput ( motorInput ), currentStatus ( MotorController::STATUS_OK ),
    bufferCount ( 0 ), bufferIndex ( 0 ) {
}

void MotorController::start () {
    this->motor.start();
}

void MotorController::stop () {
    this->motor.stop();
}

void MotorController::turnLeft () {
    this->CheckMotorStatus();
    
    this->motor.start();
    this->motor.backward();
}


void MotorController::turnRight () {
    this->CheckMotorStatus();
    
    this->motor.start();
    this->motor.forward();
}

void MotorController::setPosition (float position) {
    float current_position = this->getPosition();
    
    if (current_position <= position * (1 + MARGIN_PERCENTAGE) + MARGIN_FIXED &&
            current_position >= position * (1 - MARGIN_PERCENTAGE) - MARGIN_FIXED) {
        this->stop();
        this->currentStatus = STATUS_OK;
        return;
    }
    else {
        this->CheckMotorStatus();
    }
    
    
    if (current_position > position) {
        this->turnLeft();
    }
    else {
        this->turnRight();
    }
}


float MotorController::getPosition () {
    return this->motorInput.read() * 100.0f;
}

void MotorController::setCurrentAction(char c) {
    this->currentAction = c;
}

char MotorController::getCurrentAction() {
    return this->currentAction;
}

void MotorController::setCurrentStatus(char c) {
    this->currentStatus = c;
}

char MotorController::getCurrentStatus () {
    return this->currentStatus;
}

void MotorController::addLatestPosition (float position) {
    this->bufferIndex = (this->bufferIndex + 1) % BUFFER_SIZE;
    
    this->positionBuffer[bufferIndex] = position;
    
    if (this->bufferCount < BUFFER_SIZE) {
        bufferCount += 1;
    }
}

bool MotorController::isMotorStuck () {
    float sum = 0;
    for (int i = 0; i < bufferCount; i++) {
        sum += this->positionBuffer[i];
    }
    
    float average = sum / bufferCount;
    float allowed_deviation = average * MARGIN_PERCENTAGE + 0.03f;
    float deviation = 0;
    
    for (int i = 0; i < bufferCount; i++) {
        deviation += (average - this->positionBuffer[i]) * (average - this->positionBuffer[i]);
    }
    
    printf("Status: %f < %f\r\n", deviation, allowed_deviation);
    
    return deviation < allowed_deviation;
}

void MotorController::CheckMotorStatus () {
    this->addLatestPosition(this->getPosition());
        
    if (this->isMotorStuck()) {
        this->currentStatus = STATUS_STUCK;    
    }
    else {
        this->currentStatus = STATUS_OK;
    }
}