File content as of revision 28:18d85029a91b:

#include "mbed.h"
#include "EquatorStrutController.h"
#include "LightWeightSerialTransmit.h"

Timer RunningTime;

LWSerialTX SerialOutput(115200);

double vMax = 300;
double vStep = 50;
double pStep = 0.5;
double G1 = vStep / pStep;
double G2 = (vMax - vStep)/(1.0-pStep);
double Glin = vMax;
double pLinStep = pStep * G1 / Glin;
double TargetPwm;

EquatorStrut strut;

void LinearizePower(double power)
{
    // Compute the corrected pwm value to linearise the velocity profile
    double correctedPwm;
    
    if (fabs(power) < 1.0)
    {
        if (fabs(power) < pLinStep)
        {
            correctedPwm = (fabs(power) * Glin) / G1;  
        }
        else
        {
            correctedPwm = pStep + (fabs(power) - pLinStep) * Glin / G2;   
        }
    }
    else
    {
        correctedPwm = 1.0;
    }
    
    // Make sure our corrected value has the correct sign.
    if(power < 0) correctedPwm *= -1.0;
    
    if (correctedPwm > 1.0) 
    {
        correctedPwm = 1.0;
    }

    else if (correctedPwm < -1.0)
    {
        correctedPwm = -1.0;
    }
    
    strut.SetPower(correctedPwm);
}

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

double PosKpGain = 0.0;
double PosKiGain = 0.0;
double PosKdGain = 0.0;

void SerialTransmit()
{    
    SerialOutput.Transmit(RunningTime.read()); 
    SerialOutput.Delimiter(LWSerialTX::Tab);   
    
    SerialOutput.Transmit(strut.GetPosition());
    SerialOutput.Delimiter(LWSerialTX::Tab);
        
    SerialOutput.Transmit(strut.CurrentPower());
    SerialOutput.Delimiter(LWSerialTX::Tab);
    
    SerialOutput.Transmit(strut.CurrentSpeed());
    SerialOutput.Delimiter(LWSerialTX::Tab);
    
    SerialOutput.Transmit(PosError);
    SerialOutput.Delimiter(LWSerialTX::Tab);
    
    SerialOutput.Transmit(PosKpGain);
    SerialOutput.NewLine();
}

double SetPoint = 200.0; //Target Position in Millimeters

double PosProError;
double PosIntError;
double PosDifError;

int errorcounter;
double PosPreviousError [10];

double PwmChange=0;

double pwm;

double TargetVelocity;

double PosiState;

int PreviousTime = 0;

void Controller ()
{
    ///////////////////////////////////////////////////////////////////////////////////////////////
    //Position PID
    ///////////////////////////////////////////////////////////////////////////////////////////////
    int timeStep = RunningTime.read_us() - PreviousTime;
    PreviousTime = RunningTime.read_us();
    
    double integral_velmax = vMax/PosKiGain;
    double integral_velmin = -vMax/PosKiGain ;
       
    PosError = SetPoint - (strut.GetPosition());

    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 > 1.0) 
    {
        TargetPwm = 1.0;
    }

    else if (TargetPwm < -1.0)
    {
        TargetPwm = -1.0;
    }
    
    LinearizePower(TargetPwm);
    
    errorcounter++;

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

    PosPreviousError[errorcounter] = PosError;        
}

int main()
{    
    RunningTime.start();
    
    strut.Home();
    
    errorcounter = 0;
    PosPreviousError[errorcounter]=0;
    PreviousTime = RunningTime.read_us();

    while(strut.IsEnabled())
    {
        int counter = 0;
        PosKpGain = 0.0;
        
        while(PosKpGain < 1.0)
        {
            counter++;
            
            PosKpGain += 0.01;
          
            SerialOutput.NewFile();
          
            RunningTime.reset();
                                
            float iterationStart = RunningTime.read();
                
            while(RunningTime.read()-iterationStart < 10.0)
            {
                SerialTransmit();
                
                Controller();
            }
            
            strut.Home();
            
            if (counter == 10)
            {
                counter = 0;
                strut.Disable();
                wait(60);
                strut.Enable();
            }
        }
        
        strut.Disable();
    }
}