Dwengo/tekenrobot: verschil tussen versies

Verwijderde inhoud Toegevoegde inhoud
Regel 132:
// even wachten tot servo's gedraaid zijn
//eventueel waardes om LCD-scherm ter controle
}
</source>
<source lang =c>
Met onderstaande code kan je de arm laten bewegen in Dwengo
 
#include <dwengoBoard.h>
#include <dwengoConfig.h>
#include <dwengoDelay.h>
#include <dwengoLCD.h>
#include <dwengoServo.h>
#include <dwengoUsart.h>
#include <dwengoMotor.h>
#include <math.h>
 
//unsigned char newsticker(ram char *msg, unsigned char position);
 
// verplaatsen tekenarm met 2 servo's
void draai(double x,double y)
{
double alfa,beta,h,h2,hoek,getal,k=13,l=11,diagonaal;
clearLCD();
diagonaal = sqrt(x*x+y*y); //stelling van pythagoras
getal = (k*k-l*l-diagonaal*diagonaal)/(-2*l*diagonaal);//cosinusregel om alfa te berekenen
alfa=acos (getal); //bereken hoek alfa
beta = (diagonaal * diagonaal - k*k - l*l)/(-2*k*l); //cosinusregel om beta te berekenen
beta = acos(beta); // hoek berekend in radialen
h=atan (y/x); //bereken hoek h
printIntToLCD(x, 0, 10);
printIntToLCD(y, 1, 10);
 
h2=3.14/2-(alfa+h);//bereken hoek van arm 1: 90° - (alfa + h)
printIntToLCD(h2/3.14*180, 0, 0); // omzetten naar graden en opschrijven
hoek= (h2/3.14)* 255;//Hoek omzetten van radialen naar iets dat de servo begrijpt
//hoek= ((3.14/2)/3.14)* 255;//test 90°
hoek = 255 - hoek; //
printIntToLCD(beta/3.14*180, 1, 0);//controle hoek 2
beta = beta /3.14 * 255; //radialen naar graden
 
//beta = (2* 3.14 / 3) /3.14 * 255;//test 90°
beta = 205 - beta;
if (hoek < 255 && hoek > 0 && beta <255 && beta >0){
setPosition(1, hoek); // servo1 aansturen met
setPosition(2, beta); // servo2 aansturen
delay_ms(500); // korte tijd om servo's te laten bewegen
}
}
 
void main(void) {
char c[2];
double i,j;
initBoard();
initLCD();
initServo();
initMotor();
clearLCD();
setPosition(2, 220); //beginpositie arm 2 evenwijdig aan arm 1, dicht
setPosition(1,255); //beginpositie arm 1 evenwijdig aan blad
delay_ms(1000); // even tijd om beginpositie aan te nemen.
while(TRUE){
draai(14,10);
draai(14,12);
draai(16,12);
draai(16,10);
}
}
</source>
Informatie afkomstig van https://nl.wikibooks.org Wikibooks NL.
Wikibooks NL is onderdeel van de wikimediafoundation.