Tekenrobot met Dwengo

bewerken

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

bewerken

 

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
  • Houd 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

bewerken
  • 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

bewerken

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.

Stap 4: Robotarm laten bewegen met Dwengo

bewerken

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

bewerken
Test seriële communicatie tussen Dwengo en Processing
bewerken

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
bewerken

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

Informatie afkomstig van https://nl.wikibooks.org Wikibooks NL.
Wikibooks NL is onderdeel van de wikimediafoundation.