Diagonaal berekenen lukt, meerdere outputs nog niet
Dependencies: mbed FastPWM HIDScope MODSERIAL QEI
main.cpp
- Committer:
- s1600907
- Date:
- 2016-10-14
- Revision:
- 4:bd1db680e5a1
- Parent:
- 3:ac6f86c0db6e
- Child:
- 5:a9da7bc89b24
File content as of revision 4:bd1db680e5a1:
#include "mbed.h" #include "math.h" #include "iostream" #define SERIAL_BAUD 115200 Serial pc(USBTX,USBRX); Ticker TickerVeranderenXY; Ticker TickerAfstandBerekenen; Ticker TickerMotorHoekBerekenen; Ticker TickerBerekenenBewegingsHoek; Ticker TickerFlipperBewegen; // in te voeren x en y waardes (MOETEN UITEINDELIJK DE POSITIE IN CALIBRATIE ZIJN) volatile float x = -200.0; volatile float y = 200.0; // waardes volatile const float a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden) volatile const float Bar = 200.0; // lengte van de armen volatile const float pi = 3.14159265359; volatile float VorigeHoek1 = 0.5f*pi; // begin hoek van 90 graden volatile float VorigeHoek2 = 0.5f*pi; volatile float step = 1; // EEN GOK VOOR HET STAPJE EN MOET DUS NOG AANGEPAST volatile float Dia1 = 0.0; volatile float Dia2 = 0.0; volatile float MotorHoek1 = 0.0; volatile float MotorHoek2 = 0.0; volatile float BewegingsHoek1 = 0.0; volatile float BewegingsHoek2 = 0.0; volatile bool Richting1 = true; // cw = true, ccw = false volatile bool Richting2 = true; volatile bool FlipperAan = false; // als true dan flipper aanzetten // waardes die komen van Charlotte: true is aangespannen, false is niet aangespannen volatile bool BicepsLinks = true; volatile bool BicepsRechts = true; volatile bool Been = true; // het berekenen van de nieuwe x en y en zorgen dat deze niet buiten het bereik komen void VeranderenXY () { // de beweging if (BicepsLinks==true && BicepsRechts==true && Been==true) { FlipperAan = true; // flipper aan } else if (BicepsLinks==true && BicepsRechts==false && Been==false) { x = x - step; // naar links } else if (BicepsLinks==false && BicepsRechts==true && Been==false) { x = x + step; // naar rechts } else if (BicepsLinks==true && BicepsRechts==true && Been==false) { y = y + step; // naar voren } else if (BicepsLinks==false && BicepsRechts==true && Been==true) { y = y - step; // naar achter } // Grenswaardes LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT if (x > 200) { x = 200; } else if (x < -200) { x = -200; } if (y > 300) { y = 300; } else if (y < 50) { y = 50; // GOKJE, UITPROBEREN } pc.printf("x = %f, y = %f\r\n", x, y); } // functie om D1 en D2 te berekenen void AfstandBerekenen (){ float BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal float BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm Dia2 = pow(BV2,2)/(2*BV2); //pc.printf("Dia1 = %f, Dia2 = %f\r\n", Dia1, Dia2); } // functie om de motorhoek te berekenen void MotorHoekBerekenen (){ // eerst if loop doorlopen voor motor 1 if (x > -a) { MotorHoek1 = pi - atan(y/(x+a)) - acos(Dia1/Bar); } else if (x > -a) { MotorHoek1 = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar); } else { // als x=-a MotorHoek1 = 0.5f*pi - acos(Dia1/Bar); } // twee if loop doorlopen voor motor 2 if (x < a) { MotorHoek2 = pi + atan(y/(x-a)) - acos(Dia2/Bar); } else if (x > a) { MotorHoek2 = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar); } else { // als x=a MotorHoek2 = 0.5f*pi - acos(Dia1/Bar); } //pc.printf("MotorHoek1 = %f en MotorHoek2 = %f \r\n", MotorHoek1, MotorHoek2); } // het verschil uitrekenen tussen waar we zijn en waar we heen willen void BerekenenBewegingsHoek () { // Hoek 1: grootte beweging en richting BewegingsHoek1 = VorigeHoek1 - MotorHoek1; VorigeHoek1 = MotorHoek1; if (BewegingsHoek1 > 0) { Richting1 = false; // ccw } else { Richting1 = true; // cw } // Hoek 2: grootte beweging en richting BewegingsHoek2 = VorigeHoek2 - MotorHoek2; VorigeHoek2 = MotorHoek2; if (BewegingsHoek2 > 0) { Richting2 = true; // cw } else { Richting2 = false; // ccw } //pc.printf("Beweging 1 = %f, Beweging 2 = %f\r\n", BewegingsHoek1, BewegingsHoek2); } void FlipperBewegen () { if (FlipperAan) { // als FlipperAan is true dan de beweging gaan maken pc.printf("De flipper moet bewegen\r\n"); } else { // als FlipperUit dan doe niks } } int main() { pc.baud(SERIAL_BAUD); pc.printf("\r\n Nieuwe code uitproberen :) \r\n"); TickerVeranderenXY.attach(&VeranderenXY,1); // ticker voor het veranderen van de x en y waardes TickerAfstandBerekenen.attach(&AfstandBerekenen,1); // ticker om de waardes van dia1 en dia 2 te berekenen TickerMotorHoekBerekenen.attach(&MotorHoekBerekenen,1); // ticker om de motorhoek te berekenen TickerBerekenenBewegingsHoek.attach(&BerekenenBewegingsHoek,1); // ticker om grote en richting vd beweging te berekenen TickerFlipperBewegen.attach(&FlipperBewegen,1); // ticker om de flipper te laten bewegen while (true) { } }