File content as of revision 36:22d1bcb82061:

#include "mbed.h" 
#include "MODSERIAL.h"
#include "QEI.h"
#include "Math.h"
#include "ttmath.h"
#include "FastPWM.h"
#include "BiQuad.h"

MODSERIAL pc(USBTX, USBRX);
//Serial term (USBTX, USBRX);
FastPWM motor1_pwm(PTC2);
DigitalOut motor1_dir(PTC3);
FastPWM motor2_pwm(PTA2);
DigitalOut motor2_dir(PTB23);
FastPWM motor3_pwm(PTC4);
DigitalOut motor3_dir(PTC12);

AnalogIn potmeter1(A1);

//Define objects
AnalogIn    emg0( A0 ); // 1e
AnalogIn    emg2( A2 ); // 2e
AnalogIn    emg4( A4 ); // 3e

float A;
float B;
Ticker      sample_timer;
DigitalOut  led(LED1);

const int leng_filt = 60;
const int box_length = 50;
const int box_lengthC = 150;
const int box_checkC = 50;
const float grenswaardeA0 = 0.016;
const float grenswaardeB0 = 0.012;
const float grenswaardeC = 0.012;
float Ay1;
float Ay2;
float A_array[leng_filt] = {0};
float A_ar[leng_filt] = {0};
float A_ar2[leng_filt] = {0};
float A_ar3[box_length] = {0};
int boxcheckC = 0;
int boxcheckA = 0;
int boxcheckB = 0;

int boxcheckCC = 0;
int boxcheckAA = 0;
int boxcheckBB = 0;

float By1;
float By2;
float B_array[leng_filt] = {0};
float B_ar[leng_filt] = {0};
float B_ar2[leng_filt] = {0};
float B_ar3[box_length] = {0};

float Cy1;
float Cy2;
float C_array[leng_filt] = {0};
float C_ar[leng_filt] = {0};
float C_ar2[leng_filt] = {0};
float C_ar3[box_lengthC] = {0};

float result = 0;
float Asum = 0;
const int Fs = 2000; //Sample Frequency
const double b0 = 0.292893;
const double b1 = 0.585786;
const double b2 = 0.292893;
const double a0 = 1.000000;
const double a1 = -0.00000;
const double a2 = 0.171573;
   
void sample()
{    
    BiQuad lowpassA(b0,b1, b2, a0, a1, a2);
    // Signaal 1 (A)
    float A = emg0.read();
    float Amean = 0;
    float Ay2 = 0;
    
    for (int j=leng_filt-1; j>=1; j--)
    {    A_ar[j] = A_ar[j-1];           }

    A_ar[0] = A;
    
    for(int i=0; i<=leng_filt-1; i++)
    {    Amean += A_ar[i]*1/leng_filt;  }
    
    Ay1 = A - Amean;
    Ay1 = fabs(Ay1);
    Ay1 = lowpassA.step(Ay1);              // First signal, after Butterworth  
        
    for (int j=leng_filt-1; j>=1; j--)
    {   A_ar2[j] = A_ar2[j-1];          }
    
    A_ar2[0] = Ay1;
    
    for(int i=0; i<=leng_filt-1; i++)
    {   Ay2 += A_ar2[i]*1/leng_filt;     }
    
    float Ay3;
    if(Ay2>grenswaardeA0)
    {   Ay3 = 1; }
    
    //if(Ay2<=grenswaardeA0)
    //{   if(Ay2>grenswaardeA1)
    //    {   Ay3 = 0.5; }   }
    
    if(Ay2<=grenswaardeA0)
    {   Ay3 = 0; }
    
    for (int j=box_length-1; j>=1; j--)
    {   A_ar3[j] = A_ar3[j-1];         }
    
    A_ar3[0] = Ay3;
    boxcheckA = 0;
    
    for (int j=0; j<=box_length-1; j++)
    {   if(A_ar3[j] == 1)
            { boxcheckA = 1;
            boxcheckAA = 1;}           }

    // Signaal 2 (B)    
    BiQuad lowpassB(b0,b1, b2, a0, a1, a2);
    float B = emg2.read();
    float Bmean = 0;
    float By2 = 0;
    
    for (int j=leng_filt-1; j>=1; j--)
    {    B_ar[j] = B_ar[j-1];           }

    B_ar[0] = B;
    
    for(int i=0; i<=leng_filt-1; i++)
    {    Bmean += B_ar[i]*1/leng_filt;  }
    
    By1 = B - Bmean;
    By1 = fabs(By1);
    By1 = lowpassB.step(By1);              // First signal, after Butterworth  
        
    for (int j=leng_filt-1; j>=1; j--)
    {   B_ar2[j] = B_ar2[j-1];          }
    
    B_ar2[0] = By1;
    
    for(int i=0; i<=leng_filt-1; i++)
    {   By2 += B_ar2[i]*1/leng_filt;     }
    
    float By3;
    if(By2>grenswaardeB0)
    {   By3 = 1; }
    
    if(By2<=grenswaardeB0)
    {   By3 = 0; }
    
    for (int j=box_length-1; j>=1; j--)
    {   B_ar3[j] = B_ar3[j-1];         }
    
    B_ar3[0] = By3;
    boxcheckB = 0;
    
    for (int j=0; j<=box_length-1; j++)
    {   if(B_ar3[j] == 1)
            { boxcheckB = 1;
            boxcheckBB = 1;}           }  
    
    // Signaal 3 (C)
    BiQuad lowpassC(b0,b1, b2, a0, a1, a2);
    float C = emg4.read();
    float Cmean = 0;
    float Cy2 = 0;
    
    for (int j=leng_filt-1; j>=1; j--)
    {    C_ar[j] = C_ar[j-1];           }

    C_ar[0] = C;
    
    for(int i=0; i<=leng_filt-1; i++)
    {    Cmean += C_ar[i]*1/leng_filt;  }
    
    Cy1 = C - Cmean;
    Cy1 = fabs(Cy1);
    Cy1 = lowpassC.step(Cy1);              // First signal, after Butterworth  
    
    for (int j=leng_filt-1; j>=1; j--)
    {   C_ar2[j] = C_ar2[j-1];          }
    
    C_ar2[0] = Cy1;
    
    for(int i=0; i<=leng_filt-1; i++)
    {   Cy2 += C_ar2[i]*1/leng_filt;    }
    
    float Cy3;
    if(Cy2>grenswaardeC)
    {   Cy3 = 1; }
    
    if(Cy2<=grenswaardeC)
    {   Cy3 = 0; }
    
    for (int j=box_lengthC-1; j>=1; j--)
    {   C_ar3[j] = C_ar3[j-1];         }
    
    C_ar3[0] = Cy3;
    boxcheckC = 0;
    int C_sum = 0;
    for (int j=0; j<=box_length-1; j++)
    {   C_sum += C_ar3[j];
        if(C_sum >= box_checkC)
        {   boxcheckC = 1;
        boxcheckCC=1;} 
        }
    
    led = !led;
}


QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);


int quit;
int limit_pos = 8400;
float steps;
int g = 0;

int counts1 = 0;
int counts2 = 0;
int counts3 = 0;

float savedX = 0;
float savedY = 0;
float savedZ = 0;
int sign = 0;


const float le = 15.0;
const float f = 37.5;
const float re = 174.0;
const float rf = 50.0;
const float pi = 3.14159265358979323846;
const float cospi = -0.5;
const float sinpi = 0.8660254;
float y2;
float y1;
float z1;
float z2;
float rje2;
float rje;
float r2;
float r;

float z0=-158;
float y0=0;
float x0=0;

float theta1;
float theta2;
float theta3;


// Constant values for PI
float Kp;

float Ts = 0.0025;
float error1 = 0;
float error2 = 0;
float error3 = 0;

float newmotor1 = 1;
float newmotor2 = 1;
float newmotor3 = 1;

float u_k1 = 0;
float u_k2 = 0;
float u_k3 = 0;

float angle1 = 0;
float angle2 = 0;
float angle3 = 0;

float time_sin = 0;

     
void delta_calctheta1 ()   {
    float y2 = y0 + le;
    float y1 = f;
    float z1 = 0.0;
    float z2 = z0;
    float rje2 = re*re - x0*x0;
    float rje = sqrt(rje2);
    float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0);
    float r = sqrt(r2);
     
        if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r))  {
            int check = 1;
            pc.printf("\n\rPunt bestaat niet");
            }
        else    {
                float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
                float beta = atan((z1+z2)/(y1-y2));
                    if(beta<=0)  {
                    beta = beta + pi;
                        if (beta>=alpha)    {
                            theta1 = beta-alpha;
                            }
                        else {
                            theta1 = -alpha+beta;
                            }
                            
                    }
                    if(beta>0)  {
                        if (beta<=alpha)    {
                            theta1 = -alpha+beta;
                            }
                        else {
                            theta1 = beta-alpha;
                            }
                        }
    }
}

void delta_calctheta2 ()   {
    float xref = x0*cospi+y0*sinpi;
    float yref = y0*cospi-x0*sinpi;
    float zref = z0;
    float y2 = yref + le;
    float y1 = f;
    float z1 = 0.0;
    float z2 = zref;
    float rje2 = re*re - xref*xref;
    float rje = sqrt(rje2);
    float r2 = (y1-y2)*(y1-y2) + (z1-zref)*(z1-zref);
    float r = sqrt(r2);
     
        if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r))  {
            int check = 1;
            pc.printf("\n\rPunt bestaat niet");
            }
         else    {
                float alpha2 = acos((r2 + rf*rf -rje2)/(2*rf*r));
                float beta2 = atan((z1+z2)/(y1-y2));
                    if(beta2<=0)  {
                    beta2 = beta2 + pi;
                        if (beta2>=alpha2)    {
                            theta2 = beta2-alpha2;
                            }
                        else {
                            theta2 = -alpha2+beta2;
                            }
                            
                    }
                    if(beta2>0)  {
                        if (beta2<=alpha2)    {
                            theta2 = -alpha2+beta2;
                            }
                        else {
                            theta2 = beta2-alpha2;
                            }
                        }
    }
}

void delta_calctheta3 ()   {
    float xreff = x0*cospi-y0*sinpi;
    float yreff = y0*cospi+x0*sinpi;
    float zreff = z0;
    float y2 = yreff + le;
    float y1 = f;
    float z1 = 0.0;
    float z2 = zreff;
    float rje2 = re*re - xreff*xreff;
    float rje = sqrt(rje2);
    float r2 = (y1-y2)*(y1-y2) + (z1-zreff)*(z1-zreff);
    float r = sqrt(r2);
     
        if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r))  {
            int check = 1;
            pc.printf("\n\rPunt bestaat niet");
            }
         else    {
                float alpha3 = acos((r2 + rf*rf -rje2)/(2*rf*r));
                float beta3 = atan((z1+z2)/(y1-y2));
                    if(beta3<=0)  {
                    beta3 = beta3 + pi;
                        if (beta3>=alpha3)    {
                            theta3 = beta3-alpha3;
                            }
                        else {
                            theta3 = -alpha3+beta3;
                            }
                            
                    }
                    if(beta3>0)  {
                        if (beta3<=alpha3)    {
                            theta3 = -alpha3+beta3;
                            }
                        else {
                            theta3 = beta3-alpha3;
                            }
                        }
    }
}


  

Ticker      motor_timer; 
void motor(){ 

    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    counts3 = Encoder3.getPulses();
    
//z0 = 158 - 60* sin(3*time_sin); 

//    float r = 20*(1-1/(1+(time_sin)));
//    x0 = r*sin(3*time_sin);
//    y0 = r*cos(3*time_sin);

     

    delta_calctheta1 ();  
    delta_calctheta2 ();
    delta_calctheta3 ();
    
    angle1 = counts1/(8400.0)*2.0*pi; 
    angle2 = counts2/(8400.0)*2.0*pi; 
    angle3 = counts3/(8400.0)*2.0*pi; 
    
    error1 = theta1 - angle1;
    error2 = theta2 - angle2;
    error3 = theta3 - angle3;
    
    Kp = potmeter1 * 10;
    
    u_k1 = 10 * error1;
    u_k2 = 10 * error2;
    u_k3 = 10 * error3;

    newmotor1 = u_k1;
    newmotor2 = u_k2;
    newmotor3 = u_k3;

    if (newmotor1>1.0f){
        newmotor1 = 1.0f;
        }
    if (newmotor1<-1.0f){
        newmotor1 = -1.0f;
        }
    
    if (newmotor2>1.0f){
        newmotor2 =1.0f;
        }
    if (newmotor2<-1.0f){
        newmotor2 = -1.0f;
        }
        
    if (newmotor3>1.0f){
        newmotor3 =1.0f;
        }
    if (newmotor3<-1.0f){
        newmotor3 = -1.0f;
        }
    

    motor1_pwm.write(fabs(newmotor1));
    motor1_dir.write(newmotor1<0);
    
    motor2_pwm.write(fabs(newmotor2));
    motor2_dir.write(newmotor2>0);
    
    motor3_pwm.write(fabs(newmotor3));
    motor3_dir.write(newmotor3<0);
    
    time_sin += 0.002;
}
      
          

///////////////////
// MAIN FUNCTION //
///////////////////

int main(void)  {   
    
    delta_calctheta1 ();  
    delta_calctheta2 ();
    delta_calctheta3 ();
    
        motor_timer.attach(&motor, 0.002);
//            sample_timer.attach(&sample, 0.005);

       char cc = pc.getc();


    int frequency_pwm = 10000; //10 kHz PWM
    motor1_pwm.period_us(6);
    motor2_pwm.period_us(6);
    motor3_pwm.period_us(6);
    

    pc.baud(115200);  

    while(true){
//        
//        
//        while (boxcheckCC == 0)  {
//        if (boxcheckAA == 1 && z0>=-179 && z0<=-158){
//            z0=z0+1.0f;
//            wait(1.0/2000);
//            boxcheckAA = 0;
//            }
//        if (boxcheckBB == 1 && z0>=-178 && z0<=-157){
//            z0=z0-1.0f;
//            wait(1.0/2000);
//            boxcheckBB = 0;
//            }
//        }
//        
//        wait(1.5);
//        boxcheckCC = 0;
//        savedZ = z0;
//        
//        sign = 1;
//        
//        wait(5.0);
//        
//        sign = 0;
//        boxcheckCC = 0;
//        
//        
//        float diffZ = -158 - z0;
//
//        
//        for (int i=0; i<=diffZ; i++)    {
//         if (diffZ>0)   {
//         z0 = z0+1;
//         }
//         if (diffZ<0)   {
//         z0 = z0-1;     
//         }
//         wait(1.0/20);
//         }
//         
//break;        

            
        
//        pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
//        pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
//        wait(0.1);

       
       char cc = pc.getc();
        if (cc=='d' && x0>=-76 && x0<=75) {
            x0=x0+1.0f ;
            savedX=x0;
            }

        if (cc=='a' && x0>=-75 && x0<=76) {
            
            x0=x0-1.0f;
            savedX=x0;
            }

        if (cc=='w' && y0>=-76 && y0<=75){
            y0=y0+1.0f;
            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
            savedY=y0;

            }

        if (cc=='s' && y0>=-75 && y0<=76){
            y0=y0-1.0f;
            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
            pc  .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
            savedY=y0;
            }

        if (cc=='u' && z0>=-211 && z0<=-130){
            pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
            z0=z0+1.0f;
            savedZ=z0;
            }

        if (cc=='j' && z0>=-210 && z0<=-129){
                        pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
            pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
            pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
            z0=z0-1.0f;
            savedZ=z0;
            }
 
        }
//END MAIN
}