File content as of revision 1:cb2859df7a4c:

#include "mbed.h"
#include "math.h"

#include "QEI.h"

#include "rtos.h"
#include "FastPWM.h"

#define ERROR_LIMIT_POS 0.008 //limits for when any error measurement is considered to be zero.
#define ERROR_LIMIT_NEG -0.008

#define CHAN_1 0x80
#define CHAN_2 0x90
#define CHAN_3 0xA0
#define CHAN_4 0xB0

#define PRESSURE_CHAN 1
#define POSITION_CHAN 3



const float MAX_SPEED_MMPS = 24.3457;

SPISlave slave(PA_7, PA_6, PA_5, PA_4 ); // mosi, miso, sclk, ssel

QEI wheel (PB_5, PB_4, NC, 256, QEI::X4_ENCODING);

DigitalOut Mntr(D3);
   
   #define EXTERNAL_CLOCK_MODE 0x08
   #define RANGE_CONFIG 0x03

#define EXTERNAL_CLOCK_MODE 0x08
#define RANGE_CONFIG 0x03 //config for 1.5*Vref = 6.144V

#define PRESSURE_BIAS_VOLTAGE 0.15151515151515 
#define MAX_ACTUATOR_LENGTH 52.2

#define POT_2_MM 0.006750412 //convert potentiometer reading to mm (Tested and is right)
#define POT_OFFSET 7500//6666

Serial pc(USBTX, USBRX); // tx, rx

Thread PositionControlThread(osPriorityHigh);
Thread DebugThread(osPriorityNormal);
Thread GateControlThread(osPriorityNormal);

Thread DutyCycleThread(osPriorityRealtime);
Mutex mutDutyCycle;
Semaphore semDutyCycle(1);
Timer timerDutyCycle;

volatile unsigned int intT;
volatile unsigned int intDeltaT;
double dblDutyCycle;

double dblSensorDriftError;

short randomvar;

volatile int dataRx;

void CalculateDutyCycle()
{
    while(1)
    {
        semDutyCycle.wait();
        mutDutyCycle.lock();
        dblDutyCycle = (double) intDeltaT/intT;
        mutDutyCycle.unlock();
    }
}

Semaphore semPosCtrl(1);
Semaphore semDebug(1);
Semaphore semGate(1);

Timer timer;

long pulsesTest;

//define all pins
    SPI spi(PB_15, PB_14, PB_13); // mosi, miso, sclk //DO NOT USE D4 OR D5 !!!
       
    DigitalOut cs_ADC(PB_12);
    
    DigitalOut pinGate(PA_8);
    
    AnalogIn pinDemand1(PA_0);
    AnalogIn pinDemand2(PA_1);
    
    AnalogIn pinCurSense(PB_0);
    
    FastPWM pinPwmOutput(PC_8);
    
    FastPWM pinSigOut(D2);
    
    DigitalOut pinDirectionFwd(PA_13);//INB
    DigitalOut pinDirectionRev(PA_14);//INA
    
    //define all variables

    int ii,jj,kk,nn, spiTick; //counting variables
    
    volatile int slaveReceivePos;
    volatile int slaveReceiveVel;
        
    volatile int dataFlag;
    volatile int intFeedBack_pos;
    volatile int intFeedBack_pres;
    
    int intChkSum_pos;
    int intChkSum_vel;
    
    int intPar_pos;
    int intPar_vel;
    
    //raw Data Readings
    int intPotRead = 0;
    int intPressureRead = 0;
    double analogReadingDemPos;
    double analogReadingDemVel;
    double analogReadingCurSens;
    
    char buf[10];
    int dataReceived;
    
    //sample time variables
    double dblSampleTime_s = 0.001;
    
    
    //system state variables 
    double dblPos[10];//current position in mm
    double dblPosFil[10];//current position in mm
    double dblLastPos;//previous position in mm
    double dblVel[10];//velocity in mm/s
    double dblAccel; //acceleration in mm/s^2
    double dblIntegral = 0;//integral of position;
    
    double dblPotPositionRead;
    
    double dblPosBias = 48.0;
    
    double dblStartingPos;
    
    //demand variables
    double dblPosD[10];
    double dblLastPosD;
    double dblVelD[10];
    double dblAccelD;
    
    double dblTargetPos;
    double dblTargetVel;
    
    double dblMaxPos = 41.0; //maximum actuator position position in mm

    
    double dblError;
    double dblLastError;
    double dblErrorDot;
    
    //filter Variables
    int intPosFilOrder = 1;
    int intVelFilOrder = 1;
    int intDemPosFilOrder = 6;
    int intDemVelFilOrder = 6;
    int intOutFilOrder = 0;
    
    //controller variables 
    double omega;
    double zeta;
    
    double Kp = 20.0;
    double Ki = 2.0;
    double Kd = 1.5;
    double dblIntTerm; 
    double dblIntLimit = 0.8;
    
    double dblControlBias = 0.0;
    
    double dblMotorVoltage = 12.0;
    
    double output[10];//controller output 
    
    //current limit variables
    double CurrentLimitSet;
    double dblCurrentLimitAmps = 3.0;
    double currentBuck;
    double currentBuckGain = 3.0;

    //define custom Functions
   
    int Read14BitADC(int channel, DigitalOut CSpin)
    {
        char message;
        unsigned int outputA;
        unsigned int outputB;
        int output;

        switch(channel)
        {
            case 1:
                message = CHAN_1;
                break;
        
            case 2:
               message = CHAN_2;
               break;
        
            case 3:
               message = CHAN_3;
               break;
            case 4:
               message = CHAN_4;
               break;
               
            default:
            message = CHAN_1;
        }
        
        spi.format(8,0);
        spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
        
        CSpin.write(0);
        spi.write(message);
        spi.write(0x00);
        outputA = spi.write(0x00);
        outputB = spi.write(0x00);
        CSpin.write(1);
        
        //convert result to sensible value
        outputA = outputA<<8;
        output =  outputA | outputB;
        output = output>>2;
        
        return output;
    }
    
    void ADC_Config()
    {
        
         unsigned int msg;
        spi.format(8,0);
        spi.frequency(3000000);//3MHz clock speed (3.67MHz max)
        
        msg = CHAN_1 | RANGE_CONFIG; //config channel 1 set Vref as 
        cs_ADC = 0;
        spi.write(msg);
        cs_ADC = 1;
        
        cs_ADC = 0;
        spi.write(EXTERNAL_CLOCK_MODE);
        cs_ADC = 1;
        
        msg = CHAN_2 | RANGE_CONFIG; //config channel 2
        cs_ADC = 0;
        spi.write(msg);
        cs_ADC = 1;
        
        cs_ADC = 0;
        spi.write(EXTERNAL_CLOCK_MODE);
        cs_ADC = 1;
            
        msg = CHAN_3 | RANGE_CONFIG; //config channel 3
        cs_ADC = 0;
        spi.write(msg);
        cs_ADC = 1;
        
        cs_ADC = 0;
        spi.write(EXTERNAL_CLOCK_MODE);
        cs_ADC = 1;
        
        msg = CHAN_4 | RANGE_CONFIG; //config channel 4
        cs_ADC = 0;
        spi.write(msg);
        cs_ADC = 1;
        
        cs_ADC = 0;
        spi.write(EXTERNAL_CLOCK_MODE);
        cs_ADC = 1;
        
    }
    
    
int SumDigits(int var)
{
    int intSumResult = 0;
    int intTempVar = var;
    while (intTempVar >0)
    {
        intSumResult += intTempVar%10;
        intTempVar = int(intTempVar/10);
        //intSumResult += int(var/100)%100;
    }
    
    return intSumResult;
}

int EvenParityBitGen(int var)
{
    unsigned int count = 0, i, b = 1;

    for(i = 0; i < 32; i++){
        if( var & (b << i) ){count++;}
    }

    if( (count % 2) ){return 0;}

    return 1;
}
    
void CloseGate()
{
    pinGate = 0;
    
}
    
Timer gateTimer;    
  

   void PositionControlPID()
   {
       while(1)
       {
       semPosCtrl.wait();
       
       Mntr = 1;//!led; 
       pulsesTest = wheel.getPulses();

       double testy = (double) abs(pulsesTest)/2000;
       pinSigOut.write(testy);
       
       //take all readings
        
        //sensor readings
        
        intPressureRead = (Read14BitADC(PRESSURE_CHAN, cs_ADC));//read pressure
        intPressureRead = intPressureRead-1333;
        intPressureRead = intPressureRead/11997 * 511;
        intFeedBack_pres = (intFeedBack_pres<<5) | SumDigits(intFeedBack_pres);
        intFeedBack_pres = (intFeedBack_pres <<1) &0x1;
        intFeedBack_pres = (intFeedBack_pres <<1) | EvenParityBitGen(intFeedBack_pres);
                
        intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
        dblPotPositionRead = (double) POT_2_MM*(intPotRead  - POT_OFFSET);
        
        
        //demand Readings

        
        //current reading
        analogReadingCurSens = (double) 0.3*pinCurSense.read()+0.7*analogReadingCurSens;
        
        //convert units and filter
        
         //get position and filter
        dblPos[0] = (double) pulsesTest*-0.0078125 + dblStartingPos;
        dblSensorDriftError = dblPotPositionRead - dblPos[0];
        
        if(dblSensorDriftError > 2.0)//if encoder reading is seriously wrong
        {
            //dblPos[0] = dblPotPositionRead;
        }
        
        //printf("%d, %f, %f\r\n",pulsesTest,dblPos[0],dblPotPositionRead);
        if(intPosFilOrder > 0)
        {
            for (ii = 1; ii<intPosFilOrder+1; ii++)
            {
                dblPos[ii] = (double) 0.7*dblPos[ii-1] + 0.3*dblPos[ii];
            }
        }
        else
        {
            dblPos[intPosFilOrder] = dblPos[0];
        }

        //get velocity and filter
        dblVel[0] = dblPos[intPosFilOrder] - dblLastPos;
        if(intVelFilOrder>0)
        {
            for (ii = 1; ii<intVelFilOrder+1; ii++)
            {
                dblVel[ii] = (double) 0.7*dblVel[ii-1] + 0.3*dblVel[ii];
            }
        }
        else
        {
            dblVel[intVelFilOrder] = dblVel[0];
        }
        
        
        //printf("%f\r\n",dblPosD[intDemPosFilOrder]);
        
        intFeedBack_pos = dblPos[intPosFilOrder]/MAX_ACTUATOR_LENGTH*511;
        
        if(intFeedBack_pos>511)
        {
            intFeedBack_pos = 511;
        }
            
        if(intFeedBack_pos<0)
        {
            intFeedBack_pos = 0;
        }
        
            
        intFeedBack_pos = (intFeedBack_pos<<5) | SumDigits(intFeedBack_pos);
        intFeedBack_pos = intFeedBack_pos <<1;
        intFeedBack_pos = (intFeedBack_pos <<1) | EvenParityBitGen(intFeedBack_pos);
        
        //intFeedBack = dblSensorDriftError*8191;
        
        
        
        
        
        ///////////////PATH GENERATION////////////////////////
        //work out next path point
        double dblPathDifference;
        dblPathDifference = dblTargetPos - dblPosD[intDemPosFilOrder];
        double dblLinearPath;
        double dblLastLinearPath;
        //check if target has been reached
        if (dblPathDifference > 1.05*MAX_SPEED_MMPS*dblSampleTime_s) {
            //is velocity positive or negative?
            double dblPathSign = dblPathDifference/fabs(dblPathDifference);
            
            dblLinearPath = dblLinearPath + dblPathSign*dblTargetVel*dblSampleTime_s;//next point in path
            
            //now smooth
            dblPosD[intDemPosFilOrder] = 0.2*dblLinearPath + 0.8*dblPosD[intDemPosFilOrder];
        } 
        else {
            //set velocity to zero
            dblTargetVel = 0.0;
        }
        
        //make sure path is safe
        if (dblPosD[intDemPosFilOrder] > dblMaxPos) {
            dblPosD[intDemPosFilOrder] = dblMaxPos;
        }
        if (dblPosD[intDemPosFilOrder] < 0.0) {
            dblPosD[intDemPosFilOrder] = 0.0;
        }
         
        
        dblVelD[0] = dblPosD[intDemPosFilOrder] - dblLastPosD;
        
        //run PID calculations
        //get errors
        dblError = dblPosD[intDemPosFilOrder] - dblPos[intPosFilOrder];
        dblErrorDot = dblVelD[intDemVelFilOrder] - dblVel[intVelFilOrder];
        //get integral
        //printf("%f\r\n",dblError);

        
        dblIntTerm = dblIntTerm + Ki*dblError;
        
        //limit integral term
        if (dblIntTerm > dblIntLimit)
        {
            dblIntTerm = dblIntLimit;
        }
        if (dblIntTerm < -1.0*dblIntLimit)
        {
            dblIntTerm = (double) -1.0*dblIntLimit;
        }
        
        if(fabs(dblError) <0.01)
        {
            dblError = 0.0;
            dblErrorDot = 0.0;
            
        }
            
        if (fabs(dblErrorDot) < 0.1)
        {
        dblErrorDot = 0.0;
        }
        
        //calculate output
        output[0] = Kp*dblError + dblIntTerm + Kd*dblErrorDot;
        
        //limit output
        if (output[0] > 0.95)
        {
            output[0] = 0.95;
        }
        if (output[0] < -0.95)
        {
            output[0] = -0.95;
        }
        
        if(intOutFilOrder>0)
        {
            for (ii = 1; ii < intOutFilOrder+1; ii++)
            {
                output[ii] = 0.7*output[ii-1] + 0.3*output[ii];
            }
        }
        else
        {
            output[intOutFilOrder] = output[0];
        }

        //limit current
        if (analogReadingCurSens> CurrentLimitSet)
        {
            currentBuck = CurrentLimitSet / analogReadingCurSens / currentBuckGain;
        }
        else
        {
            currentBuck = 1.0;
        }
                    
        output[intOutFilOrder] = currentBuck*output[intOutFilOrder];
        //end Current limit

        //find direction
        if(output[intOutFilOrder] >=0.0)
        {
            pinDirectionFwd = 1;
            pinDirectionRev = 0;
            dblControlBias = 0.0;
        }
        else
        {
            pinDirectionFwd = 0;
            pinDirectionRev = 1;
            dblControlBias = 0.0;
        }
        
        pinPwmOutput.write(abs(output[intOutFilOrder])+dblControlBias);
        
        //update all past variables
        dblLastPos = dblPos[intPosFilOrder];
        dblLastPosD = dblPosD[intDemPosFilOrder];
        
        Mntr = 0;
        
        //GateControl();
        gateTimer.reset();
        pinGate = 1;
        
        while((gateTimer.read_us() < 600) && pinGate == 1)
        {
            if(slave.receive())
            {
                //receive first msg
                slaveReceivePos = slave.read(); //reply with 0xFFFF
                
                slave.reply(intFeedBack_pos);//prepare position reply
                slaveReceiveVel = slave.read();//get next message
                              
                //intFeedBack = intPressureRead & 0xFFFF;
                slave.reply(intFeedBack_pres); //prepare pressure reply
                
                slave.read();//get next message, send reply
                
                slave.reply(0xFFFF); //prepare next dummy reply
                pinGate = 0;
                
                //deal with position
                //find parity & checkSum
                intPar_pos = EvenParityBitGen(slaveReceivePos>>1);
                intChkSum_pos = SumDigits(slaveReceivePos>>7);
                
                //check if parity, check Sum and mesage type matches
                if((intPar_pos == (slaveReceivePos&0x1))&&(intChkSum_pos == ((slaveReceivePos>>2)&0x1F))&&( ( (slaveReceivePos>>1) &0x1) ) == 0) {
                    slaveReceivePos = slaveReceivePos>>7;
                    dblTargetPos = (double) MAX_ACTUATOR_LENGTH*slaveReceivePos/511;
                
                    //limit demand to ensure safety
                    if(dblTargetPos > dblMaxPos)
                    {
                        dblTargetPos = dblMaxPos;
                    }
                    if(dblTargetPos < 0.0)
                    {
                        dblTargetPos = 0.0;
                    }
                }
                
                //deal with velocity
                //find parity & checkSum
                intPar_vel = EvenParityBitGen(slaveReceiveVel>>1);
                intChkSum_vel = SumDigits(slaveReceiveVel>>7);
                
                //check if parity, check Sum and mesage type matches
                if((intPar_vel == (slaveReceiveVel&0x1))&&(intChkSum_vel == ((slaveReceiveVel>>2)&0x1F))&&( ( (slaveReceiveVel>>1) &0x1) ) == 0) {
                    slaveReceiveVel = slaveReceiveVel>>7;
                    dblTargetVel = (double) MAX_SPEED_MMPS*slaveReceiveVel/511;
                
                    //limit demand to ensure safety
                    if(dblTargetVel > MAX_SPEED_MMPS)
                    {
                        dblTargetVel = MAX_SPEED_MMPS;
                    }
                    if(dblTargetVel < 0.0)
                    {
                        dblTargetVel = 0.0;
                    }
                }
                
                
                
                
//                dataFlag = slaveReceive>>13;
//                
//                if(dataFlag == 0)
//                {
//                    slaveReceive = slaveReceive>>7 & 0x1FF;
//                    //get demand and filter
//                    dblPosD[intDemPosFilOrder] = (double) dblMaxPos*slaveReceive/511;
//                
//                    //limit demand to ensure safety
//                    if(dblPosD[intDemPosFilOrder] > dblMaxPos)
//                    {
//                        dblPosD[intDemPosFilOrder] = dblMaxPos;
//                    }
//                    if(dblPosD[intDemPosFilOrder] < 0.0)
//                    {
//                        dblPosD[intDemPosFilOrder] = 0.0;
//                    }
//                }
                
            }
        }
        pinGate = 0;
        
       
       }
    }
   
   
   //configure all control parameters
   void ControlParameterConfig()
   {
       Kp = Kp/dblMotorVoltage;
       Kd = Kd/dblSampleTime_s/dblMotorVoltage;
       Ki = Ki*dblSampleTime_s/dblMotorVoltage;
    }

Ticker debugTicker;

void startPositionControl()
{
    semPosCtrl.release();
}

void startDebug()
{
    semDebug.release();
}

Ticker positionCtrlTicker;
   
int main() {
    cs_ADC = 1;
    
    pinPwmOutput.period_us(50);
    printf("\r\nYo Yo Yo, Low Level innit\r\n\n\n");
    wait(0.5);
    
    timer.start();
    gateTimer.start();
    //cs_ADC = 1;
    
    ControlParameterConfig();
//    for (ii = 0; ii< 10; ii++)
//    {
//        uintPotRead = Read14BitADC(3, cs_ADC);//read potentiometer
//        dblStartingPos = (double) POT_2_MM*uintPotRead  - dblPosBias;
//    }
//    
    slave.format(16,2);
    slave.frequency(10000000);
    dblPosD[intDemPosFilOrder] = 0.0;
    slaveReceivePos = 0.0;
    slaveReceiveVel = 0.0;
    wait(0.1);
    ADC_Config();
    wait(0.1);
    ADC_Config();
    wait(0.1);
    intPotRead = (16383-Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
    //intPotRead = (Read14BitADC(POSITION_CHAN, cs_ADC));//read potentiometer
    dblStartingPos = (double) POT_2_MM*(intPotRead  - POT_OFFSET);
    dblTargetPos = dblStartingPos;
    dblPosD[intDemPosFilOrder] = dblStartingPos;
    
    printf("\r\n%d, %f\r\n", intPotRead, dblStartingPos);
    //wait(0.05);
    
    
    //printf("\n\n\n");
    
    //dblStartingPos = (double) POT_2_MM*(uintPotRead  - POT_OFFSET);
    timerDutyCycle.start();
        
    //calculate/convert variables
    
    CurrentLimitSet = dblCurrentLimitAmps *0.14/3.3;
    PositionControlThread.start(PositionControlPID);
    DutyCycleThread.start(CalculateDutyCycle);
    positionCtrlTicker.attach(&startPositionControl, dblSampleTime_s);
    
    intFeedBack_pos = 0;
    intFeedBack_pres = 0;
    
    
    while(1) 
    {
        Thread::wait(osWaitForever);
    }
}