File content as of revision 12:814db1249a19:

#include "mbed.h"
#include "RawSerial.h"

DigitalIn RGHSinState(P0_11);
DigitalIn RGHCosState(P0_12);
DigitalIn HallSensorState(P0_2);
InterruptIn RGHSinInterrupt(P0_11);
InterruptIn RGHCosInterrupt(P0_12);
InterruptIn HallSensor(P0_2);
DigitalOut ResetLine(P1_29);
PwmOut PhaseA(P0_9);
PwmOut PhaseB(P0_8);
Timer RunningTime;

bool Enabled = false;
volatile bool Homing = false;
volatile bool HallTriggered = false;

RawSerial pc(P1_27, P1_26);

volatile char PinState = 0;

volatile int direction = 0;
volatile double position = 0.0;
double currentPower = 0.0;
volatile int lastTime = 0;

const int arraySize = 50;
volatile int intteruptPeriodArray[arraySize];
volatile int arrayTotal = 0;
volatile char arrayPos = 0;
volatile int interruptPeriod = 0;

char counter = 0;

void SmoothingAdd(int input)
{
    //arrayTotal -= intteruptPeriodArray[arrayPos];
    //arrayTotal += input;
    intteruptPeriodArray[arrayPos] = input * direction;
    
    if (arrayPos == arraySize-1)
    {
        arrayPos = 0;
    }
    else
    {
        arrayPos++;
    }
    
    //interruptPeriod = arrayTotal / 15;
}

int SmoothedInterruptPeriod()
{
    int arrayTotal = 0;
    
    for (char i = 0; i < arraySize; i++)
    {
        arrayTotal += intteruptPeriodArray[i];
    }
    
    return arrayTotal / arraySize;
}

void RGHSinHandler()
{    
    if (PinState == 2)
    {        
        return;
    }
    else if (PinState == 1)
    {
        PinState = 0 |(RGHSinState << 1) | RGHCosState;
        
        if(PinState == 3)
        {
            direction = 1;
            position += 0.04 * direction;
            SmoothingAdd(RunningTime.read_us() - lastTime);
            interruptPeriod = RunningTime.read_us() - lastTime;
            lastTime = RunningTime.read_us();
        }
    }
    else
    {
        PinState = 0 |(RGHSinState << 1) | RGHCosState;
    }
}

void RGHCosHandler()
{    
    if (PinState == 1)
    {        
        return;
    }
    else if (PinState == 2)
    {
        PinState = 0 |(RGHSinState << 1) | RGHCosState;
        
        if (PinState == 3)
        {        
            direction = -1;
            position += 0.04 * direction;
            SmoothingAdd(RunningTime.read_us() - lastTime);
            interruptPeriod = RunningTime.read_us() - lastTime;
            lastTime = RunningTime.read_us();
        }
    }
    else
    {
        PinState = 0 |(RGHSinState << 1) | RGHCosState;
    }
}

void SetPower(double power)
{
    currentPower = power;
    if(!Enabled)
    {
        return;
    }
    
    if (power > 1.0 || power < -1.0)
    {
        return;
    }
    
    PhaseA = (power + 1.0) / 2;
    PhaseB = 1.0 - ((power + 1.0) / 2);
}

void Enable()
{
    SetPower(0.0);
    
    ResetLine = 1;
    
    Enabled = true;
}

void Disable()
{
    ResetLine = 0;
    
    SetPower(0.0);
    
    Enabled = false;
}

void Home()
{
    if (!Enabled)
    {
        Enable();
    }
    
    if (HallSensorState == 1)
    {
        Homing = true;
        HallTriggered = false;
        
        SetPower(-0.6);
        
        while (!HallTriggered)
        {
            wait(0.5);
        }
    }
    
    SetPower(1.0);
    
    while (position < 20.0)
    {
        
    }
    
    Homing = true;
    HallTriggered = false;
    
    SetPower(-0.4);
    
    while (!HallTriggered)
    {
        wait(0.5);
    }
}

double GetSpeed()
{
    //if ((RunningTime.read_us() - lastTime) > 1000 || SmoothedInterruptPeriod() == 0)
    //{
    //    return 0.0;
    //}
    return (0.04)/((double)SmoothedInterruptPeriod() / 1000000.0);
    //return SmoothedInterruptPeriod();
}

double Error = 0;

void SerialOut(double outputValue)
{
    int outChar = 0;
    
    if (outputValue < 0.0)
    {
        pc.putc('-');
        outputValue *= -1.0;
    }
    if (outputValue >= 100.0)
    {
        outChar = outputValue / 100;
        pc.putc(outChar + 48);
        outputValue -= outChar * 100.0;
    }
    if (outputValue >= 10.0)
    {
        outChar = outputValue / 10;
        pc.putc(outChar + 48);
        outputValue -= outChar * 10.0;
    }
    else if(outChar > 0)
    {
        pc.putc('0');
    }
    if (outputValue >= 1.0)
    {
        outChar = outputValue;
        pc.putc(outChar + 48);
        outputValue -= outChar;
    }
    else
    {
        pc.putc('0');
    }
    if (outputValue >= 0.1)
    {
        pc.putc('.');
        outChar = outputValue * 10;
        pc.putc(outChar + 48);
        outputValue -= (double)outChar / 10.0;
    }
    else
    {
        pc.putc('.');
        pc.putc('0');
    }
    if (outputValue >= 0.01)
    {
        outChar = outputValue * 100;
        pc.putc(outChar + 48);
        outputValue -= (double)outChar / 100.0;
    }
    else
    {
        pc.putc('0');
    }
    if (outputValue >= 0.001)
    {
        outChar= outputValue * 1000;
        pc.putc(outChar + 48);
    }
}

double KpGain = 0.0;
double KiGain = 0.0;
double KdGain = 0.0;

void SerialTransmit()
{    
    SerialOut(RunningTime.read()); 
    
    pc.putc('\t');   
    
    SerialOut(position);
    
    pc.putc('\t');
        
    SerialOut(currentPower);
    
    pc.putc('\t');
    
    SerialOut(GetSpeed());
    
    pc.putc('\t');
    
    SerialOut(Error);
    
    pc.putc('\t');
    
    SerialOut(KpGain);
    
    pc.putc('\t');
    
    SerialOut(KiGain);
    
    pc.putc('\t');
    
    SerialOut(KdGain);
    
    pc.putc(10);
    pc.putc(13);
}

void HallEffectFall()
{    
    RGHSinInterrupt.disable_irq();
    RGHCosInterrupt.disable_irq();
    
    if (direction < 0)
    {        
        SetPower(0.0);
    
        if (Homing)
        {
            HallTriggered = true;
            Homing = false;
            position = 0.0;
        }
    }
    RGHSinInterrupt.enable_irq();
    RGHCosInterrupt.enable_irq();
}

double SetPoint = 50.0; //Millimeter per second

double KpTerm;
double ErrorInt;
double ErrorDer;

int errorcounter;
double PreviousError [10];

double PwmChange=0;

double pwm;
double NewPwm;

int previousTime = 0;

void Controller ()
{
    if (position <= SetPoint || 1)
    {
        int timeStep = RunningTime.read_us() - previousTime;
        previousTime = RunningTime.read_us();
    
        Error = SetPoint - position;

        KpTerm = Error * KpGain;

        ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;           

        ErrorInt = ErrorInt + Error * timeStep;

        NewPwm = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);

        if (NewPwm > 1.0) 
        {
            pwm = 1.0;
        }
    
        else if (NewPwm < -1.0)
        {
            pwm = -1.0;
        }
    
        else
        {
            pwm = NewPwm;
        }
    
        SetPower(pwm);

        errorcounter ++;

        if (errorcounter > 9)
        {
            errorcounter = 0;   
        }

        PreviousError[errorcounter] = Error;
    }
}

int main()

{
    RGHSinInterrupt.rise(&RGHSinHandler);
    RGHCosInterrupt.rise(&RGHCosHandler);
    HallSensor.fall(&HallEffectFall);
    HallSensor.mode(PullUp);
    
    RunningTime.start();
    
    pc.baud(115200);
    
    Home();
    //Enable();
    
    errorcounter = 0;
    PreviousError[errorcounter]=0;
    
    previousTime = RunningTime.read_us();

    while(Enabled)
    {
        //double pow = 0.0;
        while(KpGain < 1.0)
        {
            KpGain += 0.01;
            while(KiGain < 1.0)
            {
                KiGain += 0.01;
                
                while(KdGain < 1.0)
                {
                    KdGain += 0.01;
                    
                    float iterationStart = RunningTime.read();
                    
                    while(RunningTime.read()-iterationStart < 10.0)
                    {
                        SerialTransmit();
                        
                        Controller();
                    }
                }
            }
            
            Home();
        }
        
        Disable();
    }
}

/* Change this throttle value range to 1.0 and 0.0 for car speed
m_throttlechange = (m_kpGain * m_error + m_kdGain * m_ErrorDer + m_ErrorInt);

            double throttle = m_throttle + m_throttlechange;

            if (new_throttle > 1.0) {
                m_throttle = 1.0;
            } else if (new_throttle < 0.0) {
                m_throttle = 0.0;
            } else {
                m_throttle = new_throttle;
            }*/