File content as of revision 0:83a0d1ae0768:

#include "mbed.h"
#include "QEI.h"
#include "math.h"

#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

const double period = 60.0*pow(10.0,-6.0); // sec
const double delta_t = 17.0*period;
const double pi = 3.14159265358979323846; // constant pi
volatile double positie_x;
volatile double positie_y;
volatile double delta_y; // verandering in y positie
volatile double dc2; // gewenste counts2 verandering
volatile double dc1; // gewenste counts1 verandering
volatile double A; // matrix a is de positie van de pen tov het midden
volatile double counts_per_round = 8400.0;
volatile double gamma = (2.0*pi)/(25.0*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.0; // (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);
    
    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*delta_t;
    positie_y = out_4*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));
    
    dc2 = (asin(positie_x/A)/gamma)-counts2;
    delta_y = A*cos(gamma*dc2+theta);
    dc1 = ((r-delta_y)/cos(theta)-l_beta)/alpha;
    
    // van delta counts naar pwm
    speed_desired_motor1 = (dc1*alpha)/delta_t; // voor max 2cm/s translatie -> range pwm: -1/pi - 1/pi
    speed_desired_motor2 = (dc2*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=%d speed2=%d ", speed_desired_motor1,speed_desired_motor2);
    
    double pwm1 = speed_desired_motor1/(20.0*pi); // 20.0 mm/s
    double pwm2 = (speed_desired_motor2*2.0)/(20.0*max_speed_motor2);
    
    pc.printf("pwm1=%d pwm2=%d\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    
    } 
}