Tekenrobot met Dwengo
In deze tutorial leer je stap voor stap om een tekenrobot te maken die reageert op muisbewegingen.
De robot wordt gemaakt met 2 servomotoren die je aansluit op een Dwengo-bordje.
De verbinding met Dwengo gebeurt met Processing.
Dwengo
We maken gebruik van het Dwengo - experimenteerbord. Meer info hierover vind je op de uitstekende website Dit projectje lijkt me niet ideaal om met Dwengo te starten. Hiervoor vind je wel uitstekende tutorials op de website. Als inleiding kan ik zeker ook dit eboekje aanraden.
Processing
Processing is een uitbreiding van Java, speciaal ontwikkeld om te programmeren in een visuele context.
De software is open-source en te installeren op Linux, Windows of Mac. Ga naar hun website voor meer informatie.
Als je onder Linux werkt download je de juiste versie en je pakt ze uit in een map naar keuze. Bijv. /home/processing
Het opstarten doe je door naar de juiste map gaan en onderstaande regel uit te voeren.
sudo ./processing
sudo is belangrijk omdat je zo zeker de rechten hebt voor de seriële verbinding.
Materiaal
De tekenarm wordt gemaakt in Lego. Twee eenvoudige servomotoren zorgen voor de beweging. De verbinding tussen PC en Dwengo gebeurt met een seriële-naar-USB kabel.
Vereiste voorkennis
Dwengo wordt geprogrammeerd in C, en Processing is Java, dus een minimum aan programmeerervaring is vereist. De achterliggende wiskunde (Pythagoras en cosinusregel) worden gezien in het derde en vierde jaar secundair onderwijs. Van leerlingen van het 5e jaar mag je dus veronderstellen dat ze voldoende wiskundige achtergrond hebben.
- Stap 1: Wiskunde
- Stap 2: Pseudocode
- Stap 3: Een robotarm maken met Lego
- Stap 4: De robotarm in Dwengo
- Stap 5: Beweeg de robotarm door muisbeweging
- Stap 6: Robotarm beweegt naar coördinaten doorgegeven door muis
Stap 1: Wiskunde
Hierboven zie je een schematische weergave van de tekenarm. De twee rode lijnstukken a en c zijn de twee assen. De bedoeling is dat je de x en y-coördinaat van punt C omzet naar een beweging van 2 servomotoren.
De eerste servomotor beweegt lijnstuk a en zal dus gelijk zijn aan (alfa + gamma) De tweede servomotor beweegt lijnstuk c en zal dus gelijk zijn aan (beta)
gamma
De hoek gamma bereken je door de tangens van deze hoek.
- tan (gamma) = y / x tangens in een rechthoekige driehoek is overstaande rechthoekszijde delen door aanliggende rechthoekszijde)
- Hou er rekening mee dat in 'C' geen graden, maar radialen gebruikt worden voor de hoeken.
alfa
Voor alfa heb je de cosinusregel nodig die je moet omvormen naar alfa. In deze cosinusregel heb je ook de lengte van b nodig. Die bereken je door Pythagoras toe te passen.
beta
Voor beta heb je de cosinusregel nodig die je moet omvormen naar beta. In deze cosinusregel heb je ook de lengte van b nodig. Die bereken je door Pythagoras toe te passen.
Stap 2: Pseudo-code
- begin lus in Processing
- Lees x en y coördinaat van muis op scherm.
- Stuur x en y coördinaat via seriële verbinding naar Dwengo
- Einde lus Processing
- Begin lus Dwengo
- Ontvang (x,y)
- Bereken alfa, beta en gamma
- Zet (alfa + gamma) om in waarde voor je servomotor (0-255)
- Zet (beta) om in waarde voor je servomotor (0-255)
- Beweeg servo 1 en servo 2 naar deze positie
- Wacht korte tijd om servo tijd te gunnen te bewegen
- Einde lus Dwengo
Stap 3: De tekenarm
Maak in Lego een arm die op 2 plaatsen scharniert. Probeer je arm zo te maken dat de servo's niet teveel belast worden. Je kan later ook een motor toevoegen die je pen op en neer beweegt. Dwengo ondersteunt standaard 2 servomotoren, dus als je een derde motor nodig hebt zal dit een gewone motor zijn.
2 servomotoren en wat Lego maken een eenvoudige robotarm.Het bordje is Dwengo.
2 servomotoren en wat Lego maken een eenvoudige robotarm. Het bordje is Dwengo.
Stap 4: Robotarm laten bewegen met Dwengo
Nu zal je een functie moeten schrijven die de x-en y-coördinaat omzet naar 2 bewegingen van een servo-motor.
void draai (long x, long y) // twee parameters x en y worden meegegeven
{
clearLCD();
// je variabelen declareren. De arm (a en c) geef je hier de juiste lengte in cm.
// bereken b door de stelling van Pythagoras
// cosinusregel om de cosinus van alfa te berekenen.
// acos om hoek alfa in radialen te weten
// cosinusregel om de cosinus van beta te berekenen.
// acos om hoek beta in radialen te weten
// gamma berekenen uit atan (y/x)
// hoek1 = berekenen hoek van arm 1, afhankelijk van de beginpositie van de arm.
// Als de arm bijvoorbeeld evenwijdig met y-as start dan is hoek1 = 3.14 - (alfa + gamma)
// hoek1 omzetten naar iets dat de servomotor begrijpt. (bijvoorbeeld 180° is 255, 0° is O, ... regel van drie)
// hoek1 = 225 - hoek1 (eventuele omzetting om beginpositie servo te corrigeren)
// hoek2= beta (verder zelfde procedure als hoek1)
// Als hoek1 en hoek2 een waarde hebben die de servo's kunnen aannemen dan ga naar volgende regels
// positie servo1
// positie servo2
// even wachten tot servo's gedraaid zijn
//eventueel waardes om LCD-scherm ter controle
}
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 iets dat de servo begrijp
//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){ //oneindige tekenlus
draai(14,10); // ga naar coördinaat 14,10. Deze code moet je vervangen door informatie die je van Processing krijgt.
draai(14,12);
draai(16,12);
draai(16,10);
}
}
Stap 5: Robotarm bewegen met muis
Test seriële communicatie tussen Dwengo en Processing
De bedoeling is dat je een muisbeweging doorgeeft aan je Dwengobordje, dat de relatieve beweging kort op het scherm laat zien
De code hieronder is 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>
void main(void)
{
char c[2];
initBoard();
initLCD();
while(TRUE) { //oneindige lus
initUsart(); //verbinding initialiseren
while (!DataRdyUSART()); //wachten tot er informatie klaarstaat
getsUSART(c,2); //een string van 2 bytes inlezen
clearLCD(); //scherm leegmaken
printIntToLCD(c[0],0,0); //eerste byte als integer weergeven op scherm
printIntToLCD(c[1],1,0); //tweede byte als integer weergeven op scherm
}
} //terugkeren naar begin en opnieuw beginnen
Gebruik onderstaande Processingcode om in samenwerking met Dwengo de seriële verbinding te testen.
Deze code maakt een seriële verbinding met Dwengo en stuurt de x -en y-coördinaat van de muis door.
import processing.serial.*;
Serial myPort;
int valx,valy;
void setup()
{
size(200, 200);
String portName = Serial.list()[1];
myPort = new Serial(this, portName, 9600);
valx=mouseX;
valy=mouseY;
}
void draw() {
valx=valx-mouseX;
valy=valy-mouseY;
myPort.write(valx); //doorgeven relatieve verandering x-coördinaat
myPort.write(valy); //relatieve verandering y-coördinaat
println("x is"+valx);
println("y is"+valy);
valx=mouseX;
valy=mouseY;
}
Doorgeven relatieve muisbeweging waardoor de arm beweegt
Je gebruikt de Processing code van hierboven en de Dwengocode van hieronder.
#include <dwengoBoard.h>
#include <dwengoConfig.h>
#include <dwengoDelay.h>
#include <dwengoLCD.h>
#include <dwengoServo.h>
#include <dwengoUsart.h>
#include <dwengoMotor.h>
#include <math.h>
void main(void)
{
char c[2];
int een,twee;
initBoard();
initLCD();
initServo();
setPosition(2, 220);
setPosition(1,255);
een = 255;
twee = 255;
while(TRUE) { //oneindige lus
initUsart(); //verbinding initialiseren
while (!DataRdyUSART()); //wachten tot er informatie klaarstaat
getsUSART(c,2); //een string van 2 bytes inlezen
clearLCD(); //scherm leegmaken
een = een + c[0]; //relatieve beweging verandert waarde voor servov 1
twee = twee + c[1]; //relatieve beweging verandert waardr voor servo 2
if (een>255) // als een een maximumwaarde bereikt wordt er niks meer bijgeteld. Dit is om te vermijden dat de servo van 0 herbegint
{een = 255;} // hierdoor krijg je anders een wilde beweging van de arm
if (een<0) //zelfde maar voor minimumwaarde
{een = 0;}
if (twee>255)
{twee = 255;}
if (twee<0)
{twee = 0;}
setPosition(1, een); // servo1 aansturen
setPosition(2, twee); // servo2 aansturen
printIntToLCD(c[0],0,0); //eerste byte als integer weergeven op scherm
printIntToLCD(c[1],1,0); //tweede byte als integer weergeven op scherm
}
delay_ms(500); //korte vertraging om arm kans te geven om te bewegen.
} //terugkeren naar begin en opnieuw beginnen
Een eerste gevecht tussen twee standaard Dwengo robots