File content as of revision 2:935a0c78bc68:

#include "mbed.h"
#include "MODSERIAL.h"
MODSERIAL pc(USBTX,USBRX);
Ticker Treecko;                 //We make a awesome ticker for our control system
double Ox;
double Oy;
double DetJaq;
double Jaqr1k1;
double Jaqr1k2;
double Jaqr2k1;
double Jaqr2k2;
double vx;
double vy;
double q1_dif;
double q2_dif;
double q1=1;
double q2=1;
double T=0.01; 
double p;
double q1set;
double q2set;
DigitalIn Knopje1 (PTA4);       //tijdelijk
DigitalIn Knopje2 (PTC6);       //tijdelijk


void IKHAATROBOTS ()
{
bool knop1 = Knopje1;
bool knop2 = Knopje2;

    if  (knop1 == false){
        vx += 0.01;}
    else if  (knop1 == true){
        vx = 0;}
    else
    {}
    if (knop2 == false){ 
        vy += 0.01;}
    else if (knop2 ==true){
        vy = 0;}
    else
    {}
    
p = q1+q2;
Ox = -(0.38*cos(q1+q2) + 0.3*cos(q1));
Oy = -0.38*sin(q1+q2) - 0.3*sin(q1);
DetJaq = Oy*0.38*cos(q1+q2) + 0.38*sin(q1+q2)*Ox;
Jaqr1k1 = (0.38*cos(q1+q2)*vx)/DetJaq;
Jaqr1k2 = (0.38*sin(q1+q2)*vy)/DetJaq;
Jaqr2k1 = Ox*vx/DetJaq;
Jaqr2k2 = Oy*vy/DetJaq;
q1_dif = Jaqr1k1 + Jaqr1k2;
q2_dif = Jaqr2k1 + Jaqr2k2;
q1 += q1_dif*T;
q2 += q2_dif*T;
}

int main()
{
    Treecko.attach(&IKHAATROBOTS, T);
    pc.baud(115200);
    while (true){
    pc.printf("vx = %f, vy = %f, q1 = %f, q2 = %f \n\r",vx, vy, q1, q2);}
}