RR 2d Robot

ik heb arduino en 2 servo motoren. Met deze deze 2 servomotoren moet ik een robot bouwen die voor mij een cikel en een vierkant kan gaan tekenen.

ik heb de robot zelf al gebouwd alleen het programmeren daarvan lukt mij niet wie kan mij helpen? hieronder wat extra gegevens van de robot. Zie link (filmpje) voor wat het uiteindelijk moet worden.

https://www.youtube.com/watch?v=YvGLOOrEIME

de afstand tussen de servo's is 150mm de korte arm is 110 mm de lange arm is 220 mm

Het is eigenlijk simpel rekenen en telkens een stapje zetten. Zet 4 punten neer (om de hoeken te bepalen. maak alle armen even lang (dat rekent een stuk makkelijker., dan krijg je namelijk steeds een parallelogram.

Zet nu denkbeeldig een cirkel in punt 1 en ook 1 cirkel op servo 1 (natuurlijk met de afstanden van de armen. bereken nu het snijpunt van beide cirkels, en je weet in welke stand de armen moeten staan. vervolgens ga je in 100 stappen van positie1 (punt1) naar punt2 dan weer naar punt3 en vervolgens naar 4 en dan ook nog terug naar 1 natuurlijk.

voor de cirkel reken je uit wat het middelpunt is (4 punten x en y bij elkaar tellen en delen door 4. vervolgens ga je naar punt1 en reken je een cirkel uit (of met sinus/cosinus) of met kwadraten, en die laat je dan in 200 stappen doorlopen.

Ik heb de oplossing ook liggen, maar die geef ik niet want dit is erg mooi om zelf uit te zoeken.