File content as of revision 2:3f849fd62ebb:

#include "mbed.h"
#include "QEI.h"
#include <cmath>

#define SERIAL_BAUD 115200

Serial pc(USBTX,USBRX);

DigitalOut dirpin(D4);
PwmOut pwmpin(D5);
AnalogIn pot_1(A1);
DigitalOut dirpin_2(D7);
PwmOut pwmpin_2(D6);
AnalogIn pot_2(A2);

QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //X4 encoding
QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //X4 encoding

// motor1 is motor voor rotatie
// motor2 is motor voor translatie 

double tijd = 0.00006; // sec
double delta_t = 17*tijd;
const double pi = 3.14159265358979323846; // constant pi
volatile double positie_x;
volatile double positie_y;
volatile double delta_y; // verandering in y positie, nodig door x-verandering
volatile double delta_x; // verandering in x positie, nodig door y-verandering
volatile double dc2_x; // gewenste counts2 verandering in x-rii
volatile double dc1_x; // gewenste counts1 verandering in x-rii
volatile double dc2_y; // gewenste counts2 verandering in y-rii
volatile double dc1_y; // gewenste counts1 verandering in y-rii
volatile double A; // matrix a is de positie van de pen tov het midden
volatile double counts_per_round = 8400;
volatile double gamma = (2*pi)/(25*counts_per_round); // gamma is de afstand afgelegd per count door het grote tandwiel
volatile double Lpc_x; // positie van de pen tov het midden
volatile double Lpc_y; // positie van de pen tov het midden
volatile double theta; // hoekiets
const double r = 185; // (mm) straal binnencirkel
const double alpha = (2*pi)/counts_per_round; // afstand afgelegd door pennenbakje per count 
const double l_beta = 32.5; // afstand (mm) dat de pen altijd bij de toren vandaan is door fysieke limitaties
volatile double counts1;
volatile double counts2;
volatile double speed_desired_motor2; // gewenste snelheid van motor2
volatile double speed_desired_motor1; // gewenste snelheid van motor1
volatile double max_speed_motor2;

int main()
{   
    pc.baud(115200);
    pc.printf("start\r\n"); // om te kijken of het programma uberhaupt start
    pwmpin.period_us(60);
    pc.printf("tijd = %f \r\n", tijd);
    pc.printf("delta_t = %f \r\n",delta_t);
    
    while(true){
        
    // Potmeter 1
    float out_1 = pot_1 * 2.0f;
    float out_2 = out_1 - 1.0f;
    pc.printf("out_2 = %f ", out_2); // + of - x-richting
       
    // Potmeter 2   
    float out_3 = pot_2 * 2.0f;
    float out_4 = out_3 - 1.0f;
    pc.printf("out_4 = %f ", out_4); // + of - y-richting
    
    // Encoder
    counts1 = Encoder1.getPulses();
    counts2 = Encoder2.getPulses();
    pc.printf("counts1=%i counts2=%i ", counts1,counts2);
    
    // verwerking potmeter output tot richtingen motor
    positie_x = out_2*20.0*delta_t; // de max is 20 mm/s
    positie_y = out_4*20.0*delta_t;
    theta = gamma*counts2;
    Lpc_x = (r-(alpha*counts1+l_beta))*sin(theta);
    Lpc_y = (r-(alpha*counts1+l_beta))*cos(theta);
    A = sqrt(pow(Lpc_x,2)+pow(Lpc_y,2));
    
    // x-richting 
    dc2_x = (asin(positie_x/A)/gamma)-counts2;
    delta_y = A*cos(gamma*dc2_x+theta);
    dc1_x = (r-(delta_y/cos(theta))-l_beta)/alpha;
        
    // y-richting 
    dc2_y = (acos(delta_y/A)/gamma)-counts2;
    delta_x = A*sin(gamma*dc1_y+theta);
    dc1_y = (r-(delta_x/cos(theta))-l_beta)/alpha;
    
    // van delta counts naar pwm
    speed_desired_motor1 = ((dc1_x+dc1_y)*alpha)/delta_t; // voor max 2cm/s translatie -> range pwm: -1/pi - 1/pi
    speed_desired_motor2 = ((dc2_x+dc2_y)*gamma)/delta_t; // voor max rotatie -> range pwm: -2/4,65 - 2/4,65
    max_speed_motor2 = 46.5; // moet nog berekening in
    
    pc.printf("speed1=%f speed2=%f ", speed_desired_motor1,speed_desired_motor2);
    
    double pwm1 = speed_desired_motor1/(pi); // 20.0 mm/s
    double pwm2 = (speed_desired_motor2*2)/(max_speed_motor2);
    
    pc.printf("pwm1=%f pwm2=%f\r\n",pwm1,pwm2);    
        
    // zeg richtingen aan motor
    dirpin.write(pwm2 < 0);  
    pwmpin = fabs (pwm2);
    dirpin_2.write(pwm1 < 0);
    pwmpin_2 = fabs (pwm1); 
 
    wait(0.01); // aantal keer dat een signaal wordt gecheckt en doorgestuurd    
    } 
}