File content as of revision 33:88fbf14d8aaf:

#include "mbed.h" 
#include "MODSERIAL.h"
#include "QEI.h"
#include "Math.h"
#include "ttmath.h"
#include "FastPWM.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);



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

float steps1 = 0;
float steps2 = 0;
float steps3 = 0;
int check;
int quit;
int limit_pos = 8400;
float steps;
int g = 0;

bool check1;
bool check2;
bool check3;
int counts1 = 0;
int counts2 = 0;
int counts3 = 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=-172;
float y0=0;
float x0=0;

float theta1;
float theta2;
float theta3;
float oldtheta1=0;
float oldtheta2=0;
float oldtheta3=0;

// Constant values for PI
float Kp;
float Kd;

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 u_d1 = 0;
float u_d2 = 0;
float u_d3 = 0;

float lasterror1 = 0;
float lasterror2 = 0;
float lasterror3 = 0;

float derivative1 = 0;
float derivative2 = 0;
float derivative3 = 0;

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

bool timer = false;
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;
                    }
                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 alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
                float beta = atan((z1-z2)/(y1-y2));
                    if(beta<0)  {
                    beta = beta + pi;
                    }
                theta2 = (beta - alpha);
    }
}

void delta_calctheta3 ()   {
    float xreff = x0*cospi-y0*sinpi;
    float yreff = y0*cospi+y0*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 alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
                float beta = atan((z1-z2)/(y1-y2));
                    if(beta<0)  {
                    beta = beta + pi;
                    }
                theta3 = (beta - alpha);
    }
}


  

Ticker      motor_timer; 
void motor(){ 

    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    counts3 = Encoder3.getPulses();
    
//    z0 = -172.0 + 10* sin(time_sin); 

float r = 20*(1-1/(1+(time_sin)));
    x0 = r*sin(6.28 * time_sin);
    y0 = r*cos(6.28 * 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 * 25;
    
    u_k1 = Kp * error1;
    u_k2 = Kp * error2;
    u_k3 = Kp * 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)  {    
    motor_timer.attach(&motor, 0.002);

    pc.baud(115200);  
    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);

    while(true){
        
        
        }
//END MAIN
}