File content as of revision 0:a1bb4583940a:

#include "TFC.h"
#include "MotorControl.h"

Motors::Motors()
{
    motorCurrentIndex = 0;
    batVoltageIndex = 0;
    
    steeringAngle = 0;
}

void Motors::start()
{
    TFC_HBRIDGE_ENABLE;
}

void Motors::stop()
{
    TFC_HBRIDGE_DISABLE;
}

void Motors::saveBatteryVoltageMeasure(uint16_t ADCresult)
{
    batVoltageIndex++;
    if(batVoltageIndex >= BATSAMPLECOUNT)
        batVoltageIndex = 0;
    
    batteryVoltage[batVoltageIndex] = ADCresult;
}

void Motors::saveMotorCurrentMeasure(uint16_t MotA_ADCresult, uint16_t MotB_ADCresult)
{
    motorCurrentIndex++;
    if(motorCurrentIndex >= MOTSAMPLECOUNT)
        motorCurrentIndex = 0;
    
    motorACurrent[motorCurrentIndex] = MotA_ADCresult;
    motorBCurrent[motorCurrentIndex] = MotB_ADCresult;
}

void Motors::saveSteering(float angle)
{
    steeringAngle = angle;
}

float Motors::getWheelRPS(char mot)
{
    float current,meanVoltage;
    
    if(mot == 'A')
        current = (float)motorACurrent[motorCurrentIndex]/1240.9; // *3.3/4095
    else
        current = (float)motorBCurrent[motorCurrentIndex]/1240.9; // *3.3/4095
    
    meanVoltage = currentPWM*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
    
    // freeSpeed = 9.8004*meanVoltage-7.0023
    // speed = (freeSpeed - (2.255*meanVoltage+10.51)*current)
    return ((9.8004*meanVoltage-7.0023)-(2.255*meanVoltage+10.51)*current); // rps
}

float Motors::getWheelSpeed(char mot)
{
    return getWheelRPS(mot)*0.157;
}

void Motors::processTasks()
{
    
}

float Motors::getAverageMotCurrent(char mot)
{
    uint8_t i;
    uint32_t sum = 0;
    
    if(mot == 'A')
    {
        for(i=0;i<MOTSAMPLECOUNT;i++)
        {
            sum += motorACurrent[i];
        }
    }
    else
    {
        for(i=0;i<MOTSAMPLECOUNT;i++)
        {
            sum += motorBCurrent[i];
        }
    }
    
    return ((float)sum)/MOTSAMPLECOUNT/1240.9;
}

float Motors::getAverageBatteryVoltage()
{
    uint8_t i;
    uint32_t sum = 0;
    
    for(i=0;i<BATSAMPLECOUNT;i++)
    {
        sum += batteryVoltage[i];
    }
    return ((float)sum)/BATSAMPLECOUNT/217.7;
}


void Motors::setFixedPWMValue(float pwm)
{
    currentPWM = pwm;
    TFC_SetMotorPWM(currentPWM,currentPWM);
}