File content as of revision 2:0d5c994d8135:

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

#define INT_MAX 32767
#define INT_MIN -32768

Motors::Motors()
{
    uint8_t i;
    
    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;
    
    motorACurrent[motorCurrentIndex] = MotA_ADCresult;
    motorBCurrent[motorCurrentIndex] = MotB_ADCresult;
    
/*    index = motorCurrentIndex;
    for(i=0;i<10;i++)
    {
        sum += motorACurrent[index] - motorBCurrent[index];
        if(index == 0)
            index = MOTSAMPLECOUNT;
        index--;
    }
    
    torqueDiffAvg = sum;*/
}

float Motors::getWheelRPS(char mot)
{
    float current,meanVoltage;
    uint8_t index = motorCurrentIndex;
    
    
    // TODO: optimise computation time
    if(mot == 'A')
    {
        current = (float)motorACurrent[index]/1240.9; // *3.3/4095
        meanVoltage = ((float)motorAPWM[index])/INT_MAX*(float)batteryVoltage[batVoltageIndex]/217.7; // *3.3/4095*5.7
    }
    else
    {
        current = (float)motorBCurrent[index]/1240.9; // *3.3/4095
        meanVoltage = ((float)motorBPWM[index])/INT_MAX*(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; // in m/s (pi*0.05)
}

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

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

    if(motorDriveIndex != motorCurrentIndex)
    {
        motorDriveIndex = motorCurrentIndex;
        
        torqueDiffAvg = 0;
        PWMSumAvg = 0;
        motorSumAvg = 0;
        index = motorDriveIndex;
        // TODO: verify indexes
        for(i=0;i<10;i++)
        {
            torqueDiffAvg += motorACurrent[index] - motorBCurrent[index];
            PWMSumAvg += motorAPWM[index]+motorBPWM[index];
            motorSumAvg += motorACurrent[index]+motorBCurrent[index];
            if(index == 0)
                index = MOTSAMPLECOUNT;
            index--;
        }
        
/*        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;
*/
        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;*/
        
        nextPWM = 14745;
        nextDelta = (int16_t)(torqueDiffAvg/2); // delta to apply is about half the sum (0x7fff/(10*1240.9)/5)

        // compute MotA PWM
/*        if ((nextDelta > 0 && nextPWM < INT_MIN + nextDelta) ||
           (nextDelta < 0 && nextPWM > INT_MAX + nextDelta))
            motorAPWM[motorDriveIndex] = INT_MIN;
        else
            motorAPWM[motorDriveIndex] = nextPWM - nextDelta;*/
        motorAPWM[motorDriveIndex] = nextPWM-nextDelta;
    
        // compute MotB PWM
/*        if (((nextDelta > 0) && (nextPWM > (INT_MAX - nextDelta))) ||
           ((nextDelta < 0) && (nextPWM < (INT_MIN - nextDelta))))
            motorBPWM[motorDriveIndex] = INT_MAX;
        else
            motorBPWM[motorDriveIndex] = nextPWM + nextDelta;*/
        motorBPWM[motorDriveIndex] = nextPWM+nextDelta;

        currentMotAPWM = ((float)motorAPWM[motorDriveIndex])/INT_MAX;
        currentMotBPWM = ((float)motorBPWM[motorDriveIndex])/INT_MAX;
        TFC_SetMotorPWM(currentMotAPWM,currentMotBPWM);
    }
}

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::getMotCurrent(char mot)
{
    if(mot == 'A')
        return (float)motorACurrent[motorCurrentIndex]/1240.9;
    else
        return (float)motorBCurrent[motorCurrentIndex]/1240.9;
}

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

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

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

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