File content as of revision 19:9c8ab7922191:

#include "mbed.h" 
#include "MODSERIAL.h"
#include "QEI1.h"
#include "QEI2.h"
#include "QEI.h"
#include "Math.h"
#include "ttmath.h"

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

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

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

float delta_calcangleyz(float x0, float y0, float z0)   {
    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;
                    }
                float theta1 = (beta - alpha)*180.0/pi;
                return theta1;
        }
}

float delta_calcinverse(float x0, float y0, float z0) {
theta1 = theta2 = theta3 = 0;
theta1 = delta_calcangleyz(x0, y0, z0);
   
    if (check == 0)     {
        theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0);
        theta3 = delta_calcangleyz(x0*cos(2/3*pi) - y0*sin(2/3*pi), y0*cos(2/3*pi)+x0*sin(2/3*pi), z0);
        }
        
return theta1, theta2, theta3;
}
            
Ticker pulse;
void pulseget()     {
    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    counts3 = Encoder.getPulses();
    }

float anglestep(float angle)    {
    float steps;
    steps = angle / 360 * 8400;
    return steps;
    }

// DE MAIN FUNCTIE
int main(void)
{    
    pc.baud(115200);
        
    char cc = pc.getc();

    while(true)     {
    counts1 = 0;
    counts2 = 0;
    counts3 = 0;      

    delta_calcinverse(x0,y0,z0);

    oldtheta1 = theta1;
    oldtheta2 = theta2;
    oldtheta3 = theta3;        
           
char cc = pc.getc();

                        
if (cc=='a') {
            check1=true;
            check2=true;
            check3=true;
                                   
            z0=z0+2.0f;
             
            theta1 = delta_calcinverse(x0,y0,z0);
            
            theta1 = theta1 - oldtheta1;
            theta2 = theta2 - oldtheta2;
            theta3 = theta3 - oldtheta3;    
                        
            float steps1 = anglestep(theta1);
            float steps2 = anglestep(theta2);
            float steps3 = anglestep(theta3);
            
            motor1_dir.write(1); // positief
            motor2_dir.write(0); // positief
            motor3_dir.write(1); // positief
            
            if (steps1>0) {
            motor1_dir.write(0); // positief
            }
                if (steps1>0) {
                motor2_dir.write(1); // positief
                }
                    if (steps1>0) {
                    motor3_dir.write(0); // positief
                    }
            
            int frequency_pwm = 10000; //10 kHz PWM
            motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
            motor1_pwm.write(0.7); // write Duty Cycle  
            
            motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
            motor2_pwm.write(0.7); // write Duty Cycle  
            
            motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
            motor3_pwm.write(0.7); // write Duty Cycle  

            wait(1.0/100);

            
            while (check1 || check2 || check3)    {
            counts1 = Encoder1.getPulses();
            counts2 = Encoder2.getPulses();
            counts3 = Encoder.getPulses();
            
            wait(1.0/1000);
             //pc.printf("while");
             if(abs(counts1)>=abs(steps1)) {
                  motor1_pwm.write(0);
                  check1=false;
                  counts1=0;
                }
                if (abs(counts2)>=abs(steps2))   {
                    motor2_pwm.write(0);
                    check2=false;
                    counts2=0;
                    }
                        if (abs(counts3)>=abs(steps3))  {
                            motor3_pwm.write(0);
                            check3=false;
                            counts3=0;
                            }
        }  
}
              
if (cc=='d') {
            
            check1=true;
            check2=true;
            check3=true;
                                   
            x0=x0-2.0f;
             
            theta1 = delta_calcinverse(x0,y0,z0);
                        pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);       
                        pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);       

            theta1 = theta1 - oldtheta1;
            theta2 = theta2 - oldtheta2;
            theta3 = theta3 - oldtheta3;    
                        
            float steps1 = anglestep(theta1);
            float steps2 = anglestep(theta2);
            float steps3 = anglestep(theta3);
            
            //pc.printf("\n\r (%f, %f, %f)", steps1, steps2, steps3);       

            
            motor1_dir.write(0); // positief
            motor2_dir.write(1); // positief
            motor3_dir.write(0); // positief
            
            if (steps1<0) {
            motor1_dir.write(1); // positief
            }
                if (steps1<0) {
                motor2_dir.write(0); // positief
                }
                    if (steps1<0) {
                    motor3_dir.write(1); // positief
                    }
            
            int frequency_pwm = 10000; //10 kHz PWM
            motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
            motor1_pwm.write(0.7); // write Duty Cycle  
            
            motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
            motor2_pwm.write(0.7); // write Duty Cycle  
            
            motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
            motor3_pwm.write(0.7); // write Duty Cycle   
                        
            while (check1 || check2 || check3)    {
            counts1 = Encoder1.getPulses();
            counts2 = Encoder2.getPulses();
            counts3 = Encoder.getPulses();
            
            wait(1.0/500);
             //pc.printf("while");
             if(abs(counts1)>=abs(steps1)) {
                  motor1_pwm.write(0);
                  check1=false;
                  counts1=0;
                  wait(1.0/100);
                }
                if (abs(counts2)>=abs(steps2))   {
                    motor2_pwm.write(0);
                    check2=false;
                    counts2=0;
                    wait(1.0/100);
                    }
                        if (abs(counts3)>=abs(steps3))  {
                            motor3_pwm.write(0);
                            check3=false;
                            //pc.printf("\n\rsteps %f, counts %f", steps3, counts3);
                            counts3=0;
                            wait(1.0/100);
                            }
        }

}                
               
            
            wait(1.0/100);
            }
}