Kirsten van Dorenvanck / Mbed 2 deprecated Kinematica

Kinematica robot M9 Gr 13. Geeft de x en y waarden op hidscope weer in 2 scopes. De waarden zijn berekend met het aantal pulses van de encoders van beide motoren.

Dependencies:   HIDScope QEI mbed

main.cpp

Committer:
Thornlady
Date:
2015-10-16
Revision:
1:3af988298ce8
Parent:
0:9d0c764b4ae6
Child:
2:7993edc4dba6

File content as of revision 1:3af988298ce8:

# include "mbed.h"
# include "QEI.h"
# include "HIDScope.h"

// Encoder definieren
QEI Encoder1(D12,D13,NC,32); // Je gebruikt hierbij X2 encoding
QEI Encoder2(D14,D15,NC,32); // Je gebruikt hierbij x2 encoding

// Aantal pulses/deg
const double P = 4200/360;
const double L1 = 220.4;
const double L2 = 130;
const double R = 2.166; //ratio van de tandwielen op de arm


double C1;
double C2;
double Q1;
double Q2;
double X;
double Y;

// HIDScope definieren
HIDScope scope (2);
Ticker Kinematicatimer;

// Code voor de X- en Y-waarden
void Kinematica ()
{
    C1 = Encoder1.getPulses();  //Aantal pulsen gemeten motor 1
    C2 = Encoder2.getPulses();  //Aantal pulsen gemeten motor 2
    Q1 = C1/P;  // Hoek van motor 1
    Q2 = R*C2/P;  // Hoek van motor 2, met ratio van de tandwielen bovenop
    X = cos (Q1) * L1 + cos (Q1+Q2) * L2; //X waarde van einde arm 2    RATIO VAN DE TANDWIELEN NOG NIET ERBIJ 
    Y = sin (Q1) * L1 + sin (Q1+Q2) * L2; //Y waarde van einde arm 2    RATIO VAN DE TANDWIELEN NOG NIET ERBIJ!!!


    scope.set (0, X);
    scope.set (1, Y);
    scope.send ();
}

int main ()
{
    Kinematicatimer.attach_us(&Kinematica, 1e4);
    while(true) {}
}