File content as of revision 0:2aa29a0824df:

#include "mbed.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "HIDScope.h"

DigitalOut gpo(D0);
DigitalOut led(LED_RED);
AnalogIn pot1(A0);
AnalogIn pot2(A1);
AnalogIn pot3(A2);
QEI encoder1(D10, D11, NC, 32);
QEI encoder2(D12, D13, NC, 32);
FastPWM Motor1PWM(D6);
DigitalOut Motor1Direction(D7);
FastPWM Motor2PWM(D5);            //!!!!! Juiste poorten aangeven
DigitalOut Motor2Direction(D4);   //!!!!! Juiste poort aangeven

MODSERIAL pc(USBTX, USBRX);

//HIDSCOPE
HIDScope    scope(4);
Ticker      scopeTimer;


///Variables

double q1Error;
double q2Error;
double PrevErrorq1[100];
double PrevErrorq2[100];
int n = 100;            // Zelfde waarde als PrevErrorarray
double q1Ref = 1.0;     //VOOR TESTEN
double q2Ref;
//Double xPos;
//Double yPos;
double xRef;
double yRef;
double q1Pos;
double q2Pos;
double PIDerrorq1;
double PIDerrorq2;
double IntegralErrorq1 = 0.0;
double IntegralErrorq2 = 0.0;
double DerivativeErrorq1 = 0.0;
double DerivativeErrorq2 = 0.0;
double GainP1 = 2.0;
double GainI1 = 1.2;
double GainD1 = 0.0;
double GainP2 = 2.0;
double GainI2 = 2.0;
double GainD2 = 2.0;
double TickerPeriod = 1.0/400.0;

Ticker Kikker;
int count = 0;
int countstep = 0;



void UpdateRefPosition()
{
  //  Update, (based on EMG or pots) the reference position xRef, yRef
    //q1Ref = pot1.read()*2*3.1416;
    //q2Ref = pot2.read()*2*3.1416;
}


void InverseKinematics()
{
  //  Convert, using the inverse kinematics relations, xRef and yRef to q1Ref and q2Ref
// (So, update the values of q1Ref and q2Ref)
}


void ReadCurrentPosition()  //Update the coordinates of the end-effector q1Pos, q2Pos
{    
    q1Pos = encoder1.getPulses()/32/131.25*2*3.1416;        //Position motor 1 in rad
    q2Pos = encoder2.getPulses()/32/131.25*2*3.1416;        //Position motor 2 in rad 
}   

void UpdateError()    //Update the q1Error and q2Error values based on q1Ref, q2Ref, q1Pos, q2Pos
{
    q1Error = q1Ref - q1Pos;
    q2Error = q2Ref - q2Pos;

    //Update PrevErrorq1 and PrevErrorq2
    
    for (int i = 0; i <= n-2 ; i++)
    {
        PrevErrorq1[i] = PrevErrorq1[i+1];
        PrevErrorq2[i] = PrevErrorq2[i+1];
    }
    
    PrevErrorq1[n-1] = q1Error;
    PrevErrorq2[n-1] = q2Error;    
}

void PIDControl(){
    // Update PIDerrorq1 and PIDerrorq2 based on the gains and the values q1Error and q2Error
    
    
    // PID control motor 1
        //P-Control
        double P_error1 = GainP1 * q1Error;
     
        //I-Control
        IntegralErrorq1 = IntegralErrorq1 + q1Error * TickerPeriod;
        double I_error1 = GainI1 * IntegralErrorq1;
     
        //D-Control
        //First smoothen the error signal
        double MovAvq1_1 = 0.0;
        double MovAvq1_2 = 0.0;
        for (int i=0; i<=n-2; i++){   // Creates two mov. av. with one element shifted
            MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1));
            MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1));
        }
        DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod;    
        double D_error1 = GainD1 * DerivativeErrorq1;
        // Derivative error sum over all time?   
     
    PIDerrorq1 = P_error1 + I_error1 + D_error1;
    
    // PID control motor 2
        //P-Control
        double P_error2 = GainP2 * q2Error;
     
        //I-Control
        IntegralErrorq2 = IntegralErrorq2 + q2Error * TickerPeriod;
        double I_error2 = GainI2 * IntegralErrorq2;
     
        //D-Control
        //First smoothen the error signal
        double MovAvq2_1 = 0.0;
        double MovAvq2_2 = 0.0;
        for (int i=0; i<=n-2; i++){   // Creates two mov. av. with one element shifted
            MovAvq2_1 = MovAvq2_1 + PrevErrorq2[i]/((double)(n-1));
            MovAvq2_2 = MovAvq2_2 + PrevErrorq2[i+1]/((double)(n-1));
        }
        DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod;    
        double D_error2 = GainD2 * DerivativeErrorq2;
        // Derivative error sum over all time?   
     
    PIDerrorq2 = P_error2 + I_error2 + D_error2;
}

    
void MotorControl()
{
        //Motor 1 
        //Keep signal within bounds
        if (PIDerrorq1 > 1.0){
            PIDerrorq1 = 1.0;
            }
        else if (PIDerrorq1 < -1.0){
            PIDerrorq1 = -1.0;
            }     
        //Direction    
        if (PIDerrorq1 <= 0){
            Motor1Direction = 0;
            Motor1PWM.write(-PIDerrorq1); //write Duty cycle 
        }        
        if (PIDerrorq1 >= 0){
            Motor1Direction = 1;
            Motor1PWM.write(PIDerrorq1); //write Duty cycle 
        }    
        
        //Motor 2 
        //Keep signal within bounds
        if (PIDerrorq2 > 1.0){
            PIDerrorq2 = 1.0;
            }
        else if (PIDerrorq2 < -1.0){
            PIDerrorq2 = -1.0;
            }     
        //Direction           
          
        if (PIDerrorq2 <= 0){
            Motor2Direction = 0;
            Motor2PWM.write(-PIDerrorq2); //write Duty cycle 
        }        
        if (PIDerrorq2 >= 0){
            Motor2Direction = 1;
            Motor2PWM.write(PIDerrorq2); //write Duty cycle 
        }    
}

void NormalOperatingModus()
{
     UpdateRefPosition();
     InverseKinematics();
     ReadCurrentPosition();
     UpdateError();
     PIDControl();
     MotorControl();
     
     scope.set(0, q1Pos);
     scope.set(1, q1Ref);
     
     GainP1 = pot3.read() * 10;
     GainI1 = pot2.read() * 10;
     GainD1 = pot1.read() ;
          
     countstep++;
     count++;
     if (count == 400) // print 1x per seconde waardes.
    {
        pc.printf("GainP1 = %1f, GainI1 = %1f, GainD1 = %1f, q1Pos = %lf, q1Ref = %1f \n\r", GainP1, GainI1, GainD1, q1Pos, q1Ref);
        count = 0;   
    }
     if (countstep == 4000)
     {
         q1Ref = !q1Ref;
         countstep = 0;
     }    
         
    
}
 
 
int main()
{
        pc.baud(115200);
        //Initialize array errors to 0
        for (int i = 0; i <= 9; i++){
             PrevErrorq2[i] = 0;
             PrevErrorq2[i] = 0;
             }
             
        int frequency_pwm = 16700; //16.7 kHz PWM
        Motor1PWM.period(1.0/frequency_pwm); // T = 1/f
        Motor2PWM.period(1.0/frequency_pwm); // T = 1/f     
        
        Kikker.attach(NormalOperatingModus, TickerPeriod);
        scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
        
        while(true);
        {}        
}