File content as of revision 4:66154ae82263:

#include "TFC.h"
#include "MotorControl.h"
//#include "limits.h"

#define INT_MAX 32767
#define INT_MIN -32768

Motors::Motors()
{
    uint8_t i;
    
    PWMRegulatedMode = false;
    
    motorCurrentIndex = 0;
    batVoltageIndex = 0;
    
    for(i=0;i<MOTSAMPLECOUNT;i++)
    {
        motorACurrent[i] = 0;
        motorBCurrent[i] = 0;
        motorAPWM[i] = 0;
        motorAPWM[i] = 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)
{
//    uint8_t i,index;
//    int16_t sum = 0;
    
    motorCurrentIndex++;
    if(motorCurrentIndex >= MOTSAMPLECOUNT)
        motorCurrentIndex = 0;
    
    // current is always measured as positive. Need to check direction of motor drive to know the sign
    // current value stored is directly value in volt at input of ADC, not real current in Amp
    if(currentMotAPWM >= 0)
        motorACurrent[motorCurrentIndex] = (float)MotA_ADCresult/1240.9090; // *3.3/4095
    else
        motorACurrent[motorCurrentIndex] = -(float)MotA_ADCresult/1240.9090; // *3.3/4095
    
    if(currentMotBPWM >= 0)
        motorBCurrent[motorCurrentIndex] = (float)MotB_ADCresult/1240.9090; // *3.3/4095
    else
        motorBCurrent[motorCurrentIndex] = -(float)MotB_ADCresult/1240.9090; // *3.3/4095
    
    motorAPWM[motorCurrentIndex] = currentMotAPWM;
    motorBPWM[motorCurrentIndex] = currentMotBPWM;
/*    index = motorCurrentIndex;
    for(i=0;i<10;i++)
    {
        sum += motorACurrent[index] - motorBCurrent[index];
        if(index == 0)
            index = MOTSAMPLECOUNT;
        index--;
    }
    
    torqueDiffAvg = sum;*/
}

float Motors::getAverageSpeed()
{
    float currentA = 0, currentB = 0, meanVoltageA = 0, meanVoltageB = 0;
    uint8_t index = motorCurrentIndex;
    uint8_t i,avg = 10;
    
    for(i=0;i<avg;i++)
    {
        currentA += motorACurrent[index];
        meanVoltageA += ((float)motorAPWM[index])*(float)batteryVoltage[batVoltageIndex]; // *3.3/4095*5.7
        currentB += motorBCurrent[index];
        meanVoltageB += ((float)motorBPWM[index])*(float)batteryVoltage[batVoltageIndex]; // *3.3/4095*5.7
        if(index == 0)
            index = MOTSAMPLECOUNT;
        index--;
    }

    // freeSpeed = 9.8004*meanVoltage-7.0023
    // speed = (freeSpeed - (2.255*meanVoltage+10.51)*current)
    return (((9.8004/INT_MAX/217.7*meanVoltageA-7.0023)-(2.255/INT_MAX/217.7*meanVoltageA+10.51)*currentA)+((9.8004/INT_MAX/217.7*meanVoltageB-7.0023)-(2.255/INT_MAX/217.7*meanVoltageB+10.51)*currentB))/avg/2*0.157; // m/s
}

float Motors::getWheelRPS(char mot)
{
    float current,meanVoltage;
    uint8_t index = motorCurrentIndex;
        
    // TODO: optimise computation time
    if(mot == 'A')
    {
        meanVoltage = ((float)motorAPWM[index])/INT_MAX*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
        return ((9.829651257*meanVoltage)-(19.33709258*motorACurrent[index])-6.435176945);
    }
    else
    {
        meanVoltage = ((float)motorBPWM[index])/INT_MAX*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
        return ((9.829651257*meanVoltage)-(19.33709258*motorBCurrent[index])-6.435176945);
    }
 }

float Motors::getWheelSpeed(char mot)
{
    return getWheelRPS(mot)*0.15707963267; // in m/s (pi*0.05)
}

float Motors::getCarSpeed()
{
    return (getWheelSpeed('A')+getWheelSpeed('B'))/2.0;
}

void Motors::setFixedPWMMode(void)
{
    PWMRegulatedMode = false;
}

void Motors::setRegulatedPWMMode(void)
{
    PWMRegulatedMode = true;
}

void Motors::processTasks()
{
    int16_t nextDelta;
    int32_t torqueDiffAvg,PWMSumAvg,motorSumAvg,meanVoltage,nextp;
    uint8_t i,index;

    if(motorDriveIndex != ((motorCurrentIndex+1)%MOTSAMPLECOUNT)) // process data only if a new data is available
    {
        motorDriveIndex = motorCurrentIndex+1; // current is saved with the PWM, compute the next PWM now
        
        torqueDiffAvg = 0;
        PWMSumAvg = 0;
        motorSumAvg = 0;
        index = motorDriveIndex;
        // TODO: verify indexes
        for(i=0;i<10;i++)
        {
            // compute average values over 10 last measures
            torqueDiffAvg += motorACurrent[index] - motorBCurrent[index];
            PWMSumAvg += motorAPWM[index]+motorBPWM[index];
            motorSumAvg += motorACurrent[index]+motorBCurrent[index];
            if(index == 0)
                index = MOTSAMPLECOUNT;
            index--;
        }
        
        if(PWMRegulatedMode)
        {
/*
            a0 = 1000000;
            a1 = 93;        // 10/a0/9.8004/pi/0.05*2*INT_MAX/3.3*4095/5.7;
            a2 = 1099919;   // a0*7.0023*pi*0.05;
            a3 = 9996209;   // 10/a0/2.255/pi/0.05*2*INT_MAX/3.3*4095/5.7/3.3*4095*2*10;
            a4 = 67;        // a0*10.51*pi*0.05*3.3/4095/2/10;
*/
            // compute current speed to get error when compared to target
            meanVoltage = PWMSumAvg*batteryVoltage[batVoltageIndex];
//          speederror1000000 = meanVoltage/a1-a2-(meanVoltage/a3+a4)*motorSumAvg - speedTarget*a0;
            speederror1000000 = meanVoltage/93-1099919-(meanVoltage/9996209+67)*motorSumAvg - speedTarget;
        
            /*nextp = speederror1000000/-20;
            if(nextp > INT_MAX)
                nextPWM = INT_MAX;
            else if (nextp < INT_MIN)
                nextPWM = INT_MIN;
            else
                nextPWM = (int16_t)nextp;
            nextDelta = 0;*/
        }
        else
        {
            nextPWM = fixedPWM; // 14745
        }
        
        nextDelta = (int16_t)(torqueDiffAvg/2); // delta to apply is about half the sum (0x7fff/(10*1240.9)/5)
        nextDelta = 0;
        
        // compute MotA PWM
/*        if ((nextDelta > 0 && nextPWM < INT_MIN + nextDelta) ||
           (nextDelta < 0 && nextPWM > INT_MAX + nextDelta))
            currentMotAPWM = INT_MIN;
        else
            currentMotAPWM = nextPWM - nextDelta;*/
        currentMotAPWM = nextPWM-nextDelta;
    
        // compute MotB PWM
/*        if (((nextDelta > 0) && (nextPWM > (INT_MAX - nextDelta))) ||
           ((nextDelta < 0) && (nextPWM < (INT_MIN - nextDelta))))
            currentMotBPWM = INT_MAX;
        else
            currentMotBPWM = nextPWM + nextDelta;*/
        currentMotBPWM = nextPWM+nextDelta;

        TFC_SetMotorPWM(getMotPWM('A'),getMotPWM('B'));
    }
}

float Motors::getAverageMotCurrent(char mot)
{
    uint8_t i;
    float 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 sum/MOTSAMPLECOUNT;
}

float Motors::getMotCurrent(char mot)
{
    if(mot == 'A')
        return motorACurrent[motorCurrentIndex];
    else
        return motorBCurrent[motorCurrentIndex];
}

float Motors::getMotPWM(char mot)
{
    if(mot == 'A')
        return ((float)currentMotAPWM)/INT_MAX;
    else
        return ((float)currentMotBPWM)/INT_MAX;
}

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)
{
    fixedPWM = (int16_t)(pwm*INT_MAX);
}

void Motors::setSpeedTargetValue(float speed)
{
    speedTarget = (int32_t)(speed*1e6);
}

float Motors::getSpeedError()
{
    return ((float)speederror1000000)/1e6;
}