File content as of revision 20:04432d03b46c:

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

DigitalIn HallSensorState(P0_2);
InterruptIn RGHSinInterrupt(P0_11);
InterruptIn RGHCosInterrupt(P0_12);
InterruptIn RGHSinFallingInterrupt(P0_13);
InterruptIn RGHCosFallingInterrupt(P0_14);
InterruptIn HallSensor(P0_2);
DigitalOut ResetLine(P1_29);
DigitalOut PulseOut(P1_22);
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 int direction = 1;
volatile int position = 0;
double currentPower = 0.0;
int lastTime = 0;

double SpeedInterval = 0.0;
int LastPosition = 0;

char counter = 0;

volatile bool SinHigh = false;
volatile bool CosHigh = false;
volatile bool LastSin = false;
volatile bool LastHigh = false;

void ActionEvent(bool CurrHigh, bool CurrSin)
{    
    // Same event again - DO NOTHING 
    if (CurrHigh == LastHigh && CurrSin == LastSin)
    {
        return;
    }
    
    if (CurrSin != LastSin) // Otherwave
    {
        // Other wave
        if ((CurrSin && CurrHigh == LastHigh) || 
            (!CurrSin && CurrHigh != LastHigh))
        {
            //Forwards
            direction = 1;
        }
        else
        {
            //Backwards
            direction = -1;
        }
        
        
    }
    else
    {
        // Reversal
        direction = -direction;
    }
    
    position += direction;
    
    // Set the state for the wave that fired
    if(CurrSin) SinHigh = CurrHigh;
    else        CosHigh = CurrHigh;
    
    // Set the last event values
    LastHigh = CurrHigh;
    LastSin = CurrSin;
}

void RGHSinRisingHandler()
{
    PulseOut = 1;
    ActionEvent(true, true);
    PulseOut = 0;
}

void RGHSinFallingHandler()
{
    ActionEvent(false, true);
}

void RGHCosRisingHandler()
{    
    ActionEvent(true, false);
}

void RGHCosFallingHandler()
{
    ActionEvent(false, false);
}

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;
}

double GetSpeed()
{
    double interval = RunningTime.read() - SpeedInterval;
    int positionDiff = position - LastPosition;
    
    SpeedInterval = RunningTime.read();
    LastPosition = position;
    
    return (positionDiff * 0.01)/interval;
}

double PosError = 0;    //This has been defined here as it's being used in the serial transmit function
//double VelError = 0;

void SerialOut(double outputValue)
{
    int outChar = 0;
    
    if (outputValue < 0.0)
    {
        pc.putc('-');
        outputValue *= -1.0;
    }
    if (outputValue >= 1000.0)
    {
        outChar = outputValue / 1000;
        pc.putc(outChar + 48);
        outputValue -= outChar * 1000.0;
    }
    if (outputValue >= 100.0)
    {
        outChar = outputValue / 100;
        pc.putc(outChar + 48);
        outputValue -= outChar * 100.0;
    }
    else if(outChar > 0)
    {
        pc.putc('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 PosKpGain = 0.0;
double PosKiGain = 0.0;
double PosKdGain = 0.0;

//double VelKpGain = 0.01;
//double VelKiGain = 0.0;
//double VelKdGain = 0.0;

void SerialTransmit()
{    
    SerialOut(RunningTime.read()); 
    
    pc.putc('\t');   
    
    SerialOut((double)position*0.01);
    
    pc.putc('\t');
        
    SerialOut(currentPower);
    
    pc.putc('\t');
    
    SerialOut(GetSpeed());
    
    pc.putc('\t');
    
    SerialOut(PosError);
    
    pc.putc('\t');
    
    SerialOut(PosKpGain);
    
    pc.putc('\t');
    
 //   SerialOut(VelKpGain);
    
    //pc.putc('\t');
    
    //SerialOut(PosKdGain);
    
    pc.putc(10);
    pc.putc(13);
}

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

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


double SetPoint = 70.0; //Target Position in Millimeter per second

double PosProError;
double PosIntError;
double PosDifError;

//double VelProError;
//double VelIntError;
//double VelDifError;

int errorcounter;
double PosPreviousError [10];
//double VelPreviousError [10];

double PwmChange=0;

double pwm;
double TargetPwm;
double newTargetPwm;
double TargetVelocity;

double PosiState;
//double VeliState;

int PreviousTime = 0;

double vMax = 300;
double vStep = 50;
double pStep = 0.5;

void Controller ()
{
    
    ///////////////////////////////////////////////////////////////////////////////////////////////
    //Position PID
    ///////////////////////////////////////////////////////////////////////////////////////////////
    
    //if (position <= SetPoint || 1)
    
    
    //{
        int timeStep = RunningTime.read_us() - PreviousTime;
        PreviousTime = RunningTime.read_us();
        
        double integral_velmax = vMax/PosKiGain;
        double integral_velmin = -vMax/PosKiGain ;
           
        PosError = SetPoint - (position * 0.01);

        PosProError = PosError * PosKpGain;

        PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep;           
        
        PosiState += PosError;
        
        if (PosiState > integral_velmax)     
        {
            PosiState = integral_velmax;   
        }
        else if (PosiState < integral_velmin)   
        {
            PosiState = integral_velmin;
        }
        PosIntError = PosKiGain * PosiState;
        
        TargetPwm = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError);
        
        if (TargetPwm < pStep)
        
            {
                newTargetPwm = (TargetPwm * 300) / 100;  
            }
        
        if (TargetPwm > pStep)
            {
                newTargetPwm = pStep + (TargetPwm - pStep) * 100 / 500;   
            }
        
        if (newTargetPwm > 1.0) 
        {
            pwm = 1.0;
        }
    
        else if (newTargetPwm < -1.0)
        {
            pwm = -1.0;
        }
    
        else
        {
            pwm = newTargetPwm;
        }
    
        SetPower(pwm);

        errorcounter ++;

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

        PosPreviousError[errorcounter] = PosError;
 //       VelPreviousError[errorcounter] = VelError;
   // }
        
}

int main()

{
    RGHSinInterrupt.rise(&RGHSinRisingHandler);
    RGHCosInterrupt.rise(&RGHCosRisingHandler);
    RGHSinFallingInterrupt.fall(&RGHSinFallingHandler);
    RGHCosFallingInterrupt.fall(&RGHCosFallingHandler);
    HallSensor.fall(&HallEffectFall);
    HallSensor.mode(PullUp);
    
    RGHSinFallingInterrupt.mode(PullNone);
    RGHCosFallingInterrupt.mode(PullNone);
    
    RunningTime.start();
    
    pc.baud(115200);
    
    Home();
    //Enable();
    
    errorcounter = 0;
    PosPreviousError[errorcounter]=0;
    
    PreviousTime = RunningTime.read_us();

    while(Enabled)
    {
        //double pow = 0.4;
        //while(pow < 1.0)
        PosKpGain = 10.0;
        while(PosKpGain < 50.0)
        {
            //pow += 0.05;
            PosKpGain += 1.0;
            
          //  VelKpGain = 0.003;
          //  while (VelKpGain < 0.008)
          //  {
          //      VelKpGain += 0.001;
                                
                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_PosDifError + m_PosIntError);

            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;
            }*/