Jitse Giesen / Mbed 2 deprecated Koppeling

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@2:263a53d72bb7, 2017-10-23 (annotated)

Committer:
Jitse_Giesen
Date:
Mon Oct 23 10:13:26 2017 +0000
Revision:
2:263a53d72bb7
Parent:
1:224bbe370e19
Child:
3:08bf605844eb

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


Jitse_Giesen

0:3e98322ac0e2


     1

#include "mbed.h"





Jitse_Giesen

0:3e98322ac0e2


     2

#include "math.h"





Jitse_Giesen

0:3e98322ac0e2


     3







Jitse_Giesen

0:3e98322ac0e2


     4

DigitalOut led(LED_RED);





Jitse_Giesen

0:3e98322ac0e2


     5

DigitalOut ledd(LED_GREEN);





Jitse_Giesen

2:263a53d72bb7


     6

DigitalOut leddd(LED_BLUE);





Jitse_Giesen

0:3e98322ac0e2


     7

InterruptIn button(SW2);





Jitse_Giesen

0:3e98322ac0e2


     8

Timer t;





Jitse_Giesen

0:3e98322ac0e2


     9

Serial pc(USBTX, USBRX);





Jitse_Giesen

0:3e98322ac0e2


    10

int Xin=0;





Jitse_Giesen

0:3e98322ac0e2


    11

float huidigetijdX;





Jitse_Giesen

0:3e98322ac0e2


    12

void ledtX(){





Jitse_Giesen

0:3e98322ac0e2


    13

    t.reset();





Jitse_Giesen

0:3e98322ac0e2


    14

    Xin++;





Jitse_Giesen

0:3e98322ac0e2


    15

    pc.printf("Xin is %i\n",Xin);





Jitse_Giesen

0:3e98322ac0e2


    16

    ledd=0;





Jitse_Giesen

0:3e98322ac0e2


    17

    led=1;





Jitse_Giesen

0:3e98322ac0e2


    18

    wait(0.5);





Jitse_Giesen

0:3e98322ac0e2


    19

    ledd=1;





Jitse_Giesen

0:3e98322ac0e2


    20

    led=0;





Jitse_Giesen

0:3e98322ac0e2


    21

}  





Jitse_Giesen

0:3e98322ac0e2


    22







Jitse_Giesen

0:3e98322ac0e2


    23

int tellerX(){





Jitse_Giesen

0:3e98322ac0e2


    24

    ledd=1; 





Jitse_Giesen

2:263a53d72bb7


    25

    leddd=1;





Jitse_Giesen

0:3e98322ac0e2


    26

    pc.baud(115200);





Jitse_Giesen

0:3e98322ac0e2


    27

    while(true){





Jitse_Giesen

0:3e98322ac0e2


    28

    button.fall(ledtX);      





Jitse_Giesen

0:3e98322ac0e2


    29

    /*if (EMG>threshold){





Jitse_Giesen

0:3e98322ac0e2


    30

        Piek();                 // dit is wat je uiteindelijk wil dat er staat





Jitse_Giesen

0:3e98322ac0e2


    31

    }*/





Jitse_Giesen

0:3e98322ac0e2


    32

    t.start();





Jitse_Giesen

0:3e98322ac0e2


    33

    huidigetijdX=t.read();





Jitse_Giesen

2:263a53d72bb7


    34

    if (huidigetijdX>1){





Jitse_Giesen

0:3e98322ac0e2


    35

        led=1;          // ga door naar het volgende programma





Jitse_Giesen

2:263a53d72bb7


    36

        return 0;





Jitse_Giesen

0:3e98322ac0e2


    37

        }





Jitse_Giesen

0:3e98322ac0e2


    38

    }  





Jitse_Giesen

0:3e98322ac0e2


    39

}   





Jitse_Giesen

0:3e98322ac0e2


    40

int Yin=0;





Jitse_Giesen

0:3e98322ac0e2


    41

float huidigetijdY;





Jitse_Giesen

0:3e98322ac0e2


    42

void ledtY(){





Jitse_Giesen

0:3e98322ac0e2


    43

    t.reset();





Jitse_Giesen

0:3e98322ac0e2


    44

    Yin++;





Jitse_Giesen

0:3e98322ac0e2


    45

    pc.printf("Yin is %i\n",Yin);





Jitse_Giesen

0:3e98322ac0e2


    46

    ledd=0;





Jitse_Giesen

2:263a53d72bb7


    47

    leddd=1;





Jitse_Giesen

0:3e98322ac0e2


    48

    wait(0.5);





Jitse_Giesen

0:3e98322ac0e2


    49

    ledd=1;





Jitse_Giesen

2:263a53d72bb7


    50

    leddd=0;





Jitse_Giesen

0:3e98322ac0e2


    51

}  





Jitse_Giesen

0:3e98322ac0e2


    52







Jitse_Giesen

0:3e98322ac0e2


    53

int tellerY(){





Jitse_Giesen

0:3e98322ac0e2


    54

    t.reset();





Jitse_Giesen

2:263a53d72bb7


    55

    ledd=1;





Jitse_Giesen

2:263a53d72bb7


    56

    leddd=0; 





Jitse_Giesen

0:3e98322ac0e2


    57

    pc.baud(115200);





Jitse_Giesen

0:3e98322ac0e2


    58

    while(true){





Jitse_Giesen

0:3e98322ac0e2


    59

    button.fall(ledtY);      





Jitse_Giesen

0:3e98322ac0e2


    60

    /*if (EMG>threshold){





Jitse_Giesen

0:3e98322ac0e2


    61

        Piek();                 // dit is wat je uiteindelijk wil dat er staat





Jitse_Giesen

0:3e98322ac0e2


    62

    }*/





Jitse_Giesen

0:3e98322ac0e2


    63

    t.start();





Jitse_Giesen

0:3e98322ac0e2


    64

    huidigetijdY=t.read();





Jitse_Giesen

2:263a53d72bb7


    65

    if (huidigetijdY>1){





Jitse_Giesen

2:263a53d72bb7


    66

        leddd=1; 





Jitse_Giesen

2:263a53d72bb7


    67

        button.fall(0);   





Jitse_Giesen

0:3e98322ac0e2


    68

        return 0;      // ga door naar het volgende programma





Jitse_Giesen

0:3e98322ac0e2


    69

        }





Jitse_Giesen

0:3e98322ac0e2


    70

    }





Jitse_Giesen

0:3e98322ac0e2


    71

}





Jitse_Giesen

2:263a53d72bb7


    72

int B;





Jitse_Giesen

0:3e98322ac0e2


    73

float Pox = 0;





Jitse_Giesen

0:3e98322ac0e2


    74

float Poy = 0;





Jitse_Giesen

0:3e98322ac0e2


    75

float Pbx = 0;





Jitse_Giesen

0:3e98322ac0e2


    76

float Pby = 887;





Jitse_Giesen

0:3e98322ac0e2


    77

float Prx = 768;





Jitse_Giesen

0:3e98322ac0e2


    78

float Pry = 443;





Jitse_Giesen

0:3e98322ac0e2


    79

float Pex=121;





Jitse_Giesen

0:3e98322ac0e2


    80

float Pey=308;





Jitse_Giesen

0:3e98322ac0e2


    81

float diamtrklosje=20;





Jitse_Giesen

0:3e98322ac0e2


    82

float pi=3.14159265359;





Jitse_Giesen

2:263a53d72bb7


    83

float omtrekklosje=diamtrklosje*pi;





Jitse_Giesen

0:3e98322ac0e2


    84

float Lou;





Jitse_Giesen

0:3e98322ac0e2


    85

float Lbu;





Jitse_Giesen

0:3e98322ac0e2


    86

float Lru;





Jitse_Giesen

2:263a53d72bb7


    87

float dLod;





Jitse_Giesen

2:263a53d72bb7


    88

float dLbd;





Jitse_Giesen

2:263a53d72bb7


    89

float dLrd;





Jitse_Giesen

0:3e98322ac0e2


    90

float roto;





Jitse_Giesen

0:3e98322ac0e2


    91

float rotb;





Jitse_Giesen

0:3e98322ac0e2


    92

float rotr;





Jitse_Giesen

2:263a53d72bb7


    93

float rotzo;





Jitse_Giesen

2:263a53d72bb7


    94

float rotzb;





Jitse_Giesen

2:263a53d72bb7


    95

float rotzr;





Jitse_Giesen

0:3e98322ac0e2


    96

float counto;





Jitse_Giesen

0:3e98322ac0e2


    97

float countb;





Jitse_Giesen

2:263a53d72bb7


    98

float countr;





Jitse_Giesen

2:263a53d72bb7


    99

float countzo;





Jitse_Giesen

2:263a53d72bb7


   100

float countzb;





Jitse_Giesen

2:263a53d72bb7


   101

float countzr; 





Jitse_Giesen

0:3e98322ac0e2


   102

float Psx;





Jitse_Giesen

0:3e98322ac0e2


   103

float Psy;





Jitse_Giesen

0:3e98322ac0e2


   104

float Vex;





Jitse_Giesen

0:3e98322ac0e2


   105

float Vey;





Jitse_Giesen

0:3e98322ac0e2


   106

float Kz=1; // nadersnelheid instellen





Jitse_Giesen

0:3e98322ac0e2


   107

float modVe;





Jitse_Giesen

0:3e98322ac0e2


   108

float Vmax=20;





Jitse_Giesen

0:3e98322ac0e2


   109

float Pstx;





Jitse_Giesen

0:3e98322ac0e2


   110

float Psty;





Jitse_Giesen

0:3e98322ac0e2


   111

float T=0.02;//seconds





Jitse_Giesen

0:3e98322ac0e2


   112







Jitse_Giesen

0:3e98322ac0e2


   113

Ticker loop;





Jitse_Giesen

0:3e98322ac0e2


   114

float touwlengtes(){





Jitse_Giesen

0:3e98322ac0e2


   115

    Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));     // rekent touwlengtes uit die nodig zijn voor de nieuwe positie aan de hand van de ingegeven coördinaten en de posities van de palen





Jitse_Giesen

0:3e98322ac0e2


   116

    Lbu=sqrt(pow((Pstx-Pox),2)+pow((Psty-Pby),2));





Jitse_Giesen

0:3e98322ac0e2


   117

    Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));





Jitse_Giesen

0:3e98322ac0e2


   118

    return 0;





Jitse_Giesen

0:3e98322ac0e2


   119

}





Jitse_Giesen

0:3e98322ac0e2


   120

float turns(){





Jitse_Giesen

2:263a53d72bb7


   121

    





Jitse_Giesen

0:3e98322ac0e2


   122

    roto=Lou/omtrekklosje; 





Jitse_Giesen

0:3e98322ac0e2


   123

    rotb=Lbu/omtrekklosje; 





Jitse_Giesen

0:3e98322ac0e2


   124

    rotr=Lru/omtrekklosje; 





Jitse_Giesen

0:3e98322ac0e2


   125

    counto=roto*4200;                               // omzetten van rotaties naar counts





Jitse_Giesen

0:3e98322ac0e2


   126

    countb=rotb*4200;





Jitse_Giesen

0:3e98322ac0e2


   127

    countr=rotr*4200;





Jitse_Giesen

0:3e98322ac0e2


   128

    return 0;





Jitse_Giesen

0:3e98322ac0e2


   129

}





Jitse_Giesen

0:3e98322ac0e2


   130







Jitse_Giesen

0:3e98322ac0e2


   131

    





Jitse_Giesen

0:3e98322ac0e2


   132

float Pst(){





Jitse_Giesen

0:3e98322ac0e2


   133

    Pstx=Pex+Vex*T;





Jitse_Giesen

0:3e98322ac0e2


   134

    Psty=Pey+Vey*T;





Jitse_Giesen

0:3e98322ac0e2


   135

    touwlengtes();





Jitse_Giesen

0:3e98322ac0e2


   136

    Pex=Pstx;                   //Dit is nog een aanname. Moet zijn: read the encoder and put the real position as the Pe for the next step





Jitse_Giesen

0:3e98322ac0e2


   137

    Pey=Psty;





Jitse_Giesen

0:3e98322ac0e2


   138

    //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);





Jitse_Giesen

0:3e98322ac0e2


   139

    //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);





Jitse_Giesen

0:3e98322ac0e2


   140

    turns();





Jitse_Giesen

0:3e98322ac0e2


   141

    //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); 





Jitse_Giesen

0:3e98322ac0e2


   142

    pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);





Jitse_Giesen

0:3e98322ac0e2


   143

    /*float R;





Jitse_Giesen

0:3e98322ac0e2


   144

    R=Vex/Vey;                  // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constand is en de end efector dus een rechte lijn maakt





Jitse_Giesen

0:3e98322ac0e2


   145

    pc.printf("\n\r R=%f",R);*/





Jitse_Giesen

0:3e98322ac0e2


   146

    return 0;





Jitse_Giesen

0:3e98322ac0e2


   147

} 





Jitse_Giesen

2:263a53d72bb7


   148

 





Jitse_Giesen

0:3e98322ac0e2


   149

float Ps(){





Jitse_Giesen

0:3e98322ac0e2


   150

    Psx=(Xin)*30+121;





Jitse_Giesen

0:3e98322ac0e2


   151

    Psy=(Yin)*30+308;     





Jitse_Giesen

0:3e98322ac0e2


   152

    pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);





Jitse_Giesen

0:3e98322ac0e2


   153

    return 0;





Jitse_Giesen

0:3e98322ac0e2


   154

}





Jitse_Giesen

0:3e98322ac0e2


   155

void Ve(){





Jitse_Giesen

0:3e98322ac0e2


   156

    Vex=Kz*(Psx-Pex);





Jitse_Giesen

0:3e98322ac0e2


   157

    Vey=Kz*(Psy-Pey);





Jitse_Giesen

0:3e98322ac0e2


   158

    modVe=sqrt(pow(Vex,2)+pow(Vey,2));





Jitse_Giesen

0:3e98322ac0e2


   159

    if(modVe>Vmax){





Jitse_Giesen

0:3e98322ac0e2


   160

        Vex=(Vex/modVe)*Vmax;





Jitse_Giesen

0:3e98322ac0e2


   161

        Vey=(Vey/modVe)*Vmax;





Jitse_Giesen

0:3e98322ac0e2


   162

    }





Jitse_Giesen

0:3e98322ac0e2


   163

    Pst();





Jitse_Giesen

0:3e98322ac0e2


   164

    pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);





Jitse_Giesen

0:3e98322ac0e2


   165

    if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){





Jitse_Giesen

2:263a53d72bb7


   166

        B=5;





Jitse_Giesen

2:263a53d72bb7


   167

        loop.detach();





Jitse_Giesen

2:263a53d72bb7


   168

        }





Jitse_Giesen

0:3e98322ac0e2


   169

}





Jitse_Giesen

0:3e98322ac0e2


   170







Jitse_Giesen

0:3e98322ac0e2


   171

int calculator(){





Jitse_Giesen

0:3e98322ac0e2


   172

    pc.baud(115200);





Jitse_Giesen

0:3e98322ac0e2


   173

    Ps();





Jitse_Giesen

0:3e98322ac0e2


   174

    loop.attach(&Ve,0.02);





Jitse_Giesen

0:3e98322ac0e2


   175

    return 0;





Jitse_Giesen

0:3e98322ac0e2


   176

}





Jitse_Giesen

0:3e98322ac0e2


   177







Jitse_Giesen

2:263a53d72bb7


   178

void zakker(){





Jitse_Giesen

2:263a53d72bb7


   179

    while(1){





Jitse_Giesen

2:263a53d72bb7


   180

        wait(1);





Jitse_Giesen

2:263a53d72bb7


   181

    if(B==5){                       // hierdoor wacht dit programma totdat de beweging klaar is





Jitse_Giesen

2:263a53d72bb7


   182

    





Jitse_Giesen

2:263a53d72bb7


   183

    dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken





Jitse_Giesen

2:263a53d72bb7


   184

    dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;





Jitse_Giesen

2:263a53d72bb7


   185

    dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;





Jitse_Giesen

2:263a53d72bb7


   186

    rotzo=dLod/omtrekklosje;





Jitse_Giesen

2:263a53d72bb7


   187

    rotzb=dLbd/omtrekklosje;





Jitse_Giesen

2:263a53d72bb7


   188

    rotzr=dLrd/omtrekklosje;





Jitse_Giesen

2:263a53d72bb7


   189

    countzo=rotzo*4200;





Jitse_Giesen

2:263a53d72bb7


   190

    countzb=rotzb*4200;





Jitse_Giesen

2:263a53d72bb7


   191

    countzr=rotzr*4200;





Jitse_Giesen

2:263a53d72bb7


   192

    





Jitse_Giesen

2:263a53d72bb7


   193

    pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr);       // hier moet komen te staan hoe het zakken gaat





Jitse_Giesen

2:263a53d72bb7


   194

    }





Jitse_Giesen

2:263a53d72bb7


   195

    }





Jitse_Giesen

0:3e98322ac0e2


   196

}





Jitse_Giesen

0:3e98322ac0e2


   197







Jitse_Giesen

0:3e98322ac0e2


   198

int main()





Jitse_Giesen

0:3e98322ac0e2


   199

{





Jitse_Giesen

0:3e98322ac0e2


   200

    tellerX();





Jitse_Giesen

0:3e98322ac0e2


   201

    tellerY();





Jitse_Giesen

2:263a53d72bb7


   202

    





Jitse_Giesen

0:3e98322ac0e2


   203

    calculator();





Jitse_Giesen

2:263a53d72bb7


   204

    zakker();





Jitse_Giesen

0:3e98322ac0e2


   205

    return 0;





Jitse_Giesen

0:3e98322ac0e2


   206

}