File content as of revision 4:a97c2e8a80d8:

#include "mbed.h"
#include "math.h"
#include "HIDScope.h"

// Define the HIDScope and Ticker object
HIDScope    scope(2);
Ticker      scopeTimer;
Ticker      ikTimer;


// Constantes
const double L1 = 0.35;
const double L2 = 0.30;
const double T  = 3;

//Variables
double q1 = 0;
double q2 = 90;
double vx = 0.05;
double vy = 0;

//Hoe werken de variabls?
// q1 en q2 komen uit de Encoder dus die worden geleverd
// This is the function for the Inverse Kinematics We start with a inverse Jacobian so we can determine q_dot (rotation speed of the motors)

double IK() {
    
    double invj[2][2] =
    {
        {sin(q1 + q2)/(L1*cos(q1 + q2)*sin(q1) - L1*sin(q1 + q2)*cos(q1)), -cos(q1 + q2)/(L1*cos(q1 + q2)*sin(q1) - L1*sin(q1 + q2)*cos(q1))},
        {-(L2*sin(q1 + q2) + L1*sin(q1))/(L1*L2*cos(q1 + q2)*sin(q1) - L1*L2*sin(q1 + q2)*cos(q1)), (L2*cos(q1 + q2) + L1*cos(q1))/(L1*L2*cos(q1 + q2)*sin(q1) - L1*L2*sin(q1 + q2)*cos(q1))}
    };
    
    double V_q1 = invj[1][1]*vx + invj[1][2]*vy;
    double V_q2 = invj[2][1]*vx + invj[2][2]*vy;
    
    // Numerical Integral to make it position controlled
    double q1_new = q1 + V_q1*T;
    q1 = q1_new;
    double q2_new = q2 + V_q2*T;
    q2 = q2_new;
    
    return(q1, q2);
    
}

// Write a function where the data gets send to HIDScope
void scopeSend()
{
    scope.set(0,q1);
    scope.set(1,q2);
    scope.send();
}

    
int main()
{
    // Attach the data read and send function at 0.1 s
    ikTimer.attach_us(&IK, 0.1);
    // Attach the data read and send function at 100 Hz
    scopeTimer.attach_us(&scopeSend, 2e3);   
    
    while(1) { }
}