File content as of revision 30:390cab7cd6e6:

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

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 x00;
float y00;
float z00;

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;

bool timer = false;

Ticker      motor_timer; 
void motor()
{ 
    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    counts3 = Encoder3.getPulses();

//    angle1 = counts1/(8400.0)*2.0*pie; 
//    angle2 = counts2/(8400.0)*2.0*pie; 
//    angle3 = counts3/(8400.0)*2.0*pie; 

//    sinewave = sin(time_sin);

    lasterror1 = error1;
    lasterror2 = error2;
    lasterror3 = error3;
    
    error1 = counts1 - steps1;
    error2 = counts2 - steps2;
    error3 = counts3 - steps3;
    
    derivative1 = error1 - lasterror1;
    derivative2 = error2 - lasterror2;
    derivative3 = error3 - lasterror3;
    
    Kp = 50;
    Kd = potmeter1 * 100;
    
    
    // Proportional part
    u_k1 = Kp * error1;
    u_k2 = Kp * error2;
    u_k3 = Kp * error3;
    
    u_d1 = Kd * derivative1;
    u_d2 = Kd * derivative2;
    u_d3 = Kd * derivative3;

    newmotor1 = u_k1 + u_d1;
    newmotor2 = u_k2 + u_d2;
    newmotor3 = u_k3 + u_d3;

    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;
        }
    
    if (timer == true)  {
    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);
    }
    
    else  {
    motor1_pwm.write(0);
    motor2_pwm.write(0);
    motor3_pwm.write(0);
        }
}
      
     
float delta_calcangleyz(float x00, float y00, float z00)   {
    float y2 = y00 + le;
    float y1 = f;
    float z1 = 0.0;
    float z2 = z00;
    float rje2 = re*re - x00*x00;
    float rje = sqrt(rje2);
    float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00);
    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 x00, float y00, float z00) {
    theta1 = theta2 = theta3 = 0;
    x00=x0;
    y00=y0;
    z00=z0;
    theta1 = delta_calcangleyz(x00, y00, z00);
       
        if (check == 0)     {
            x00=x0*cospi+y0*sinpi;
            y00=y0*cospi-x0*sinpi;
            z00=z0;
            theta2 = delta_calcangleyz(x00, y00, z00);
            x00=x0*cospi-y0*sinpi;
            y00=y0*cospi+y0*sinpi;
            z00=z0;
            theta3 = delta_calcangleyz(x00, y00, z00);
            }
            
    return theta1, theta2, theta3;
}
            
//double error;

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

// Omschrijving
float movefunctioninit ()   {   
    
    theta1 = delta_calcinverse(x00,y00,z00);
    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;    
                
    steps1 = steps1 + anglestep(theta1);
    steps2 = steps2 + anglestep(theta2);
    steps3 = steps3 + anglestep(theta3);
    pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
    pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3);

    

//    pc.printf("\n\r error1: %f", error1);


    // Set the direction of the motors.   
 //   if (theta1 < 0) {
//        motor1_dir.write(1); 
//    }
//        else {
//            motor1_dir.write(0);
//        }
//    
//    if (theta2 < 0) {
//        motor2_dir.write(0); 
//    }
//        else {
//            motor2_dir.write(1);
//        }
//    
//    if (theta3 < 0) {
//        motor3_dir.write(0); 
//    }
//        else {
//            motor3_dir.write(1);
//        }
//    
//    int frequency_pwm = 10000; //10 kHz PWM
//    motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
//    motor1_pwm.write(0.57); // write Duty Cycle  
//    
//    motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
//    motor2_pwm.write(0.57); // write Duty Cycle  
//    
//    motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
//    motor3_pwm.write(0.57); // write Duty Cycle  
//    
//    check1 = true;
//    check2 = true;
//    check3 = true; 
//
//    Encoder1.reset();
//    Encoder2.reset();
//    Encoder3.reset(); 
//    
}

// Omschrijving
float movestop() {  
wait(0.6);
timer = false;
} 
//       
//    while (check1 || check2 || check3)    {
//
//        int frequency_pwm = 10000; //10 kHz PWM
//        motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
//        motor1_pwm.write(newmotor1); // write Duty Cycle
//        
//        motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
//        motor2_pwm.write(newmotor2); // write Duty Cycle
//        
//        motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
//        motor3_pwm.write(newmotor3); // write Duty Cycle
//        
//        if (error1==0)  {
//            check1=false;
//            }
//        if (error2==0)  {
//            check2=false;
//            }
//        if (error3==0)  {
//            check3=false;
//            }
//
////       if(abs(counts1)>=abs(steps1)) {
////            pc.printf("\n\r 1 is false");
////            motor1_pwm.write(0);
////            check1=false;  
////            pc.printf("\n\rcounts1 %i", counts1);
////            }
////            
////        if (abs(counts2)>=abs(steps2))   {
////            pc.printf("\n\r 2 is false");
////            motor2_pwm.write(0);
////            check2=false;
////            pc.printf("\n\rcounts2 %i", counts2);
////            }
////            
////        if (abs(counts3)>=abs(steps3))  {
////            pc.printf("\n\r 3 is false");
////            motor3_pwm.write(0);
////            check3=false;
////            pc.printf("\n\rcounts3 %i", counts3);
////            }
//    }
//            wait(1.0);
//        pc.printf("\n\rdone moving");
//}


///////////////////
// 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(1.0/(double)frequency_pwm); // T=1/f
//    motor1_pwm.write(newmotor1); // write Duty Cycle
//        
//    motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
//    motor2_pwm.write(newmotor2); // write Duty Cycle
//        
//    motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
//    motor3_pwm.write(newmotor3); // write Duty Cycle

    while(true){
    pc.printf("\n\r----------------------------------------------------------");
    pc.printf("\n\r----------------------------------------------------------");

        
        delta_calcinverse(x0,y0,z0);
    
        oldtheta1 = theta1;
        oldtheta2 = theta2;
        oldtheta3 = theta3; 
                            
        char cc = pc.getc();
        
        timer = true;

        
        if (cc=='d') {
            x0=x0+5.0f;
            if (x0>=-75 && x0<=75){
                movefunctioninit ();
                movestop();
            }
            else {
                timer = false;
                x0=x0-5.0f;
                
            }
        }
        
        if (cc=='a') {
            x0=x0-5.0f;
            if (x0>=-75 && x0<=75){
                movefunctioninit ();
                movestop();
            }
            else {
                timer = false;
                x0=x0+5.0f;
            }
        }
        
        if (cc=='w'){
            y0=y0+5.0f;
            if (y0>=-75 && y0<=75){
                movefunctioninit ();
                movestop();
            }
            else {
                timer = false;
                y0=y0-5.0f;
            }
        }
        
        if (cc=='s'){
            y0=y0-5.0f;
            if (y0>=-75 && y0<=75)   {
                movefunctioninit ();
                movestop();
                }
            else {
                timer = false;
                y0=y0+5.0f;
            }
        }
        
        if (cc=='u') 
        {
            z0=z0+5.0f;
            if (z0>=-210 && z0<=-130) {
                movefunctioninit ();
                movestop();
//                for (float i=0.0; i<5; i++)  {
//                pc.printf("\n\r dit is error1 %f, error2 %f, error3 %f", error1, error2, error3);
//                wait(1.0/10.0);
//                }
            }
            else{
                timer = false;
                z0=z0-5.0f;
            }
        }
        
        if (cc=='j') 
        {
            z0=z0-5.0f;
            if (z0>=-210 && z0<=-130){
                movefunctioninit ();
                movestop();
            }
            else {
                timer = false;
                z0=z0+5.0f;
            }
        }
    // end while                                           
    }
     
// end main
}