Diagonaal berekenen lukt, meerdere outputs nog niet
Dependencies: mbed FastPWM HIDScope MODSERIAL QEI
main.cpp@4:bd1db680e5a1, 2016-10-14 (annotated)
- Committer:
- s1600907
- Date:
- Fri Oct 14 10:22:22 2016 +0000
- Revision:
- 4:bd1db680e5a1
- Parent:
- 3:ac6f86c0db6e
- Child:
- 5:a9da7bc89b24
geheel van signaal spieren naar berekening in beweging xy. Flipper zit er nog niet in
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TimDouma | 0:46a1642a10c8 | 1 | #include "mbed.h" |
TimDouma | 0:46a1642a10c8 | 2 | #include "math.h" |
s1600907 | 4:bd1db680e5a1 | 3 | #include "iostream" |
TimDouma | 0:46a1642a10c8 | 4 | #define SERIAL_BAUD 115200 |
TimDouma | 0:46a1642a10c8 | 5 | |
TimDouma | 0:46a1642a10c8 | 6 | Serial pc(USBTX,USBRX); |
s1600907 | 4:bd1db680e5a1 | 7 | Ticker TickerVeranderenXY; |
TimDouma | 0:46a1642a10c8 | 8 | Ticker TickerAfstandBerekenen; |
s1600907 | 3:ac6f86c0db6e | 9 | Ticker TickerMotorHoekBerekenen; |
s1600907 | 4:bd1db680e5a1 | 10 | Ticker TickerBerekenenBewegingsHoek; |
s1600907 | 4:bd1db680e5a1 | 11 | Ticker TickerFlipperBewegen; |
s1600907 | 3:ac6f86c0db6e | 12 | |
s1600907 | 4:bd1db680e5a1 | 13 | // in te voeren x en y waardes (MOETEN UITEINDELIJK DE POSITIE IN CALIBRATIE ZIJN) |
s1600907 | 4:bd1db680e5a1 | 14 | volatile float x = -200.0; |
TimDouma | 0:46a1642a10c8 | 15 | volatile float y = 200.0; |
TimDouma | 0:46a1642a10c8 | 16 | |
TimDouma | 0:46a1642a10c8 | 17 | // waardes |
TimDouma | 0:46a1642a10c8 | 18 | volatile const float a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden) |
TimDouma | 0:46a1642a10c8 | 19 | volatile const float Bar = 200.0; // lengte van de armen |
s1600907 | 3:ac6f86c0db6e | 20 | volatile const float pi = 3.14159265359; |
s1600907 | 4:bd1db680e5a1 | 21 | volatile float VorigeHoek1 = 0.5f*pi; // begin hoek van 90 graden |
s1600907 | 4:bd1db680e5a1 | 22 | volatile float VorigeHoek2 = 0.5f*pi; |
s1600907 | 4:bd1db680e5a1 | 23 | volatile float step = 1; // EEN GOK VOOR HET STAPJE EN MOET DUS NOG AANGEPAST |
TimDouma | 0:46a1642a10c8 | 24 | |
s1600907 | 3:ac6f86c0db6e | 25 | volatile float Dia1 = 0.0; |
s1600907 | 3:ac6f86c0db6e | 26 | volatile float Dia2 = 0.0; |
s1600907 | 3:ac6f86c0db6e | 27 | volatile float MotorHoek1 = 0.0; |
s1600907 | 3:ac6f86c0db6e | 28 | volatile float MotorHoek2 = 0.0; |
s1600907 | 4:bd1db680e5a1 | 29 | volatile float BewegingsHoek1 = 0.0; |
s1600907 | 4:bd1db680e5a1 | 30 | volatile float BewegingsHoek2 = 0.0; |
s1600907 | 4:bd1db680e5a1 | 31 | volatile bool Richting1 = true; // cw = true, ccw = false |
s1600907 | 4:bd1db680e5a1 | 32 | volatile bool Richting2 = true; |
s1600907 | 4:bd1db680e5a1 | 33 | volatile bool FlipperAan = false; // als true dan flipper aanzetten |
s1600907 | 3:ac6f86c0db6e | 34 | |
s1600907 | 4:bd1db680e5a1 | 35 | // waardes die komen van Charlotte: true is aangespannen, false is niet aangespannen |
s1600907 | 4:bd1db680e5a1 | 36 | volatile bool BicepsLinks = true; |
s1600907 | 4:bd1db680e5a1 | 37 | volatile bool BicepsRechts = true; |
s1600907 | 4:bd1db680e5a1 | 38 | volatile bool Been = true; |
s1600907 | 4:bd1db680e5a1 | 39 | |
s1600907 | 4:bd1db680e5a1 | 40 | // het berekenen van de nieuwe x en y en zorgen dat deze niet buiten het bereik komen |
s1600907 | 4:bd1db680e5a1 | 41 | void VeranderenXY () { |
s1600907 | 4:bd1db680e5a1 | 42 | // de beweging |
s1600907 | 4:bd1db680e5a1 | 43 | if (BicepsLinks==true && BicepsRechts==true && Been==true) { |
s1600907 | 4:bd1db680e5a1 | 44 | FlipperAan = true; // flipper aan |
s1600907 | 4:bd1db680e5a1 | 45 | } |
s1600907 | 4:bd1db680e5a1 | 46 | else if (BicepsLinks==true && BicepsRechts==false && Been==false) { |
s1600907 | 4:bd1db680e5a1 | 47 | x = x - step; // naar links |
s1600907 | 4:bd1db680e5a1 | 48 | } |
s1600907 | 4:bd1db680e5a1 | 49 | else if (BicepsLinks==false && BicepsRechts==true && Been==false) { |
s1600907 | 4:bd1db680e5a1 | 50 | x = x + step; // naar rechts |
s1600907 | 4:bd1db680e5a1 | 51 | } |
s1600907 | 4:bd1db680e5a1 | 52 | else if (BicepsLinks==true && BicepsRechts==true && Been==false) { |
s1600907 | 4:bd1db680e5a1 | 53 | y = y + step; // naar voren |
s1600907 | 4:bd1db680e5a1 | 54 | } |
s1600907 | 4:bd1db680e5a1 | 55 | else if (BicepsLinks==false && BicepsRechts==true && Been==true) { |
s1600907 | 4:bd1db680e5a1 | 56 | y = y - step; // naar achter |
s1600907 | 4:bd1db680e5a1 | 57 | } |
s1600907 | 4:bd1db680e5a1 | 58 | |
s1600907 | 4:bd1db680e5a1 | 59 | // Grenswaardes LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT |
s1600907 | 4:bd1db680e5a1 | 60 | if (x > 200) { |
s1600907 | 4:bd1db680e5a1 | 61 | x = 200; |
s1600907 | 4:bd1db680e5a1 | 62 | } |
s1600907 | 4:bd1db680e5a1 | 63 | else if (x < -200) { |
s1600907 | 4:bd1db680e5a1 | 64 | x = -200; |
s1600907 | 4:bd1db680e5a1 | 65 | } |
s1600907 | 4:bd1db680e5a1 | 66 | if (y > 300) { |
s1600907 | 4:bd1db680e5a1 | 67 | y = 300; |
s1600907 | 4:bd1db680e5a1 | 68 | } |
s1600907 | 4:bd1db680e5a1 | 69 | else if (y < 50) { |
s1600907 | 4:bd1db680e5a1 | 70 | y = 50; // GOKJE, UITPROBEREN |
s1600907 | 4:bd1db680e5a1 | 71 | } |
s1600907 | 4:bd1db680e5a1 | 72 | pc.printf("x = %f, y = %f\r\n", x, y); |
s1600907 | 4:bd1db680e5a1 | 73 | } |
TimDouma | 0:46a1642a10c8 | 74 | |
TimDouma | 0:46a1642a10c8 | 75 | // functie om D1 en D2 te berekenen |
s1600907 | 3:ac6f86c0db6e | 76 | void AfstandBerekenen (){ |
TimDouma | 0:46a1642a10c8 | 77 | float BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen |
s1600907 | 3:ac6f86c0db6e | 78 | Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal |
s1600907 | 3:ac6f86c0db6e | 79 | |
s1600907 | 3:ac6f86c0db6e | 80 | float BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm |
s1600907 | 3:ac6f86c0db6e | 81 | Dia2 = pow(BV2,2)/(2*BV2); |
s1600907 | 3:ac6f86c0db6e | 82 | |
s1600907 | 4:bd1db680e5a1 | 83 | //pc.printf("Dia1 = %f, Dia2 = %f\r\n", Dia1, Dia2); |
TimDouma | 0:46a1642a10c8 | 84 | } |
TimDouma | 0:46a1642a10c8 | 85 | |
s1600907 | 3:ac6f86c0db6e | 86 | // functie om de motorhoek te berekenen |
s1600907 | 3:ac6f86c0db6e | 87 | void MotorHoekBerekenen (){ |
s1600907 | 3:ac6f86c0db6e | 88 | // eerst if loop doorlopen voor motor 1 |
s1600907 | 3:ac6f86c0db6e | 89 | if (x > -a) { |
s1600907 | 3:ac6f86c0db6e | 90 | MotorHoek1 = pi - atan(y/(x+a)) - acos(Dia1/Bar); |
s1600907 | 3:ac6f86c0db6e | 91 | } |
s1600907 | 3:ac6f86c0db6e | 92 | else if (x > -a) { |
s1600907 | 3:ac6f86c0db6e | 93 | MotorHoek1 = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar); |
s1600907 | 3:ac6f86c0db6e | 94 | } |
s1600907 | 3:ac6f86c0db6e | 95 | else { // als x=-a |
s1600907 | 3:ac6f86c0db6e | 96 | MotorHoek1 = 0.5f*pi - acos(Dia1/Bar); |
s1600907 | 3:ac6f86c0db6e | 97 | } |
s1600907 | 4:bd1db680e5a1 | 98 | // twee if loop doorlopen voor motor 2 |
s1600907 | 3:ac6f86c0db6e | 99 | if (x < a) { |
s1600907 | 3:ac6f86c0db6e | 100 | MotorHoek2 = pi + atan(y/(x-a)) - acos(Dia2/Bar); |
s1600907 | 3:ac6f86c0db6e | 101 | } |
s1600907 | 3:ac6f86c0db6e | 102 | else if (x > a) { |
s1600907 | 3:ac6f86c0db6e | 103 | MotorHoek2 = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar); |
s1600907 | 3:ac6f86c0db6e | 104 | } |
s1600907 | 3:ac6f86c0db6e | 105 | else { // als x=a |
s1600907 | 3:ac6f86c0db6e | 106 | MotorHoek2 = 0.5f*pi - acos(Dia1/Bar); |
s1600907 | 3:ac6f86c0db6e | 107 | } |
s1600907 | 4:bd1db680e5a1 | 108 | //pc.printf("MotorHoek1 = %f en MotorHoek2 = %f \r\n", MotorHoek1, MotorHoek2); |
s1600907 | 3:ac6f86c0db6e | 109 | } |
TimDouma | 2:bf1466ac4e6f | 110 | |
s1600907 | 4:bd1db680e5a1 | 111 | // het verschil uitrekenen tussen waar we zijn en waar we heen willen |
s1600907 | 4:bd1db680e5a1 | 112 | void BerekenenBewegingsHoek () { |
s1600907 | 4:bd1db680e5a1 | 113 | // Hoek 1: grootte beweging en richting |
s1600907 | 4:bd1db680e5a1 | 114 | BewegingsHoek1 = VorigeHoek1 - MotorHoek1; |
s1600907 | 4:bd1db680e5a1 | 115 | VorigeHoek1 = MotorHoek1; |
s1600907 | 4:bd1db680e5a1 | 116 | if (BewegingsHoek1 > 0) { |
s1600907 | 4:bd1db680e5a1 | 117 | Richting1 = false; // ccw |
s1600907 | 4:bd1db680e5a1 | 118 | } |
s1600907 | 4:bd1db680e5a1 | 119 | else { |
s1600907 | 4:bd1db680e5a1 | 120 | Richting1 = true; // cw |
s1600907 | 4:bd1db680e5a1 | 121 | } |
s1600907 | 4:bd1db680e5a1 | 122 | |
s1600907 | 4:bd1db680e5a1 | 123 | // Hoek 2: grootte beweging en richting |
s1600907 | 4:bd1db680e5a1 | 124 | BewegingsHoek2 = VorigeHoek2 - MotorHoek2; |
s1600907 | 4:bd1db680e5a1 | 125 | VorigeHoek2 = MotorHoek2; |
s1600907 | 4:bd1db680e5a1 | 126 | if (BewegingsHoek2 > 0) { |
s1600907 | 4:bd1db680e5a1 | 127 | Richting2 = true; // cw |
s1600907 | 4:bd1db680e5a1 | 128 | } |
s1600907 | 4:bd1db680e5a1 | 129 | else { |
s1600907 | 4:bd1db680e5a1 | 130 | Richting2 = false; // ccw |
s1600907 | 4:bd1db680e5a1 | 131 | } |
s1600907 | 4:bd1db680e5a1 | 132 | //pc.printf("Beweging 1 = %f, Beweging 2 = %f\r\n", BewegingsHoek1, BewegingsHoek2); |
s1600907 | 4:bd1db680e5a1 | 133 | } |
s1600907 | 4:bd1db680e5a1 | 134 | |
s1600907 | 4:bd1db680e5a1 | 135 | void FlipperBewegen () { |
s1600907 | 4:bd1db680e5a1 | 136 | if (FlipperAan) { // als FlipperAan is true dan de beweging gaan maken |
s1600907 | 4:bd1db680e5a1 | 137 | pc.printf("De flipper moet bewegen\r\n"); |
s1600907 | 4:bd1db680e5a1 | 138 | } |
s1600907 | 4:bd1db680e5a1 | 139 | else { // als FlipperUit dan doe niks |
s1600907 | 4:bd1db680e5a1 | 140 | } |
s1600907 | 4:bd1db680e5a1 | 141 | } |
s1600907 | 4:bd1db680e5a1 | 142 | |
s1600907 | 3:ac6f86c0db6e | 143 | int main() { |
s1600907 | 3:ac6f86c0db6e | 144 | pc.baud(SERIAL_BAUD); |
s1600907 | 3:ac6f86c0db6e | 145 | pc.printf("\r\n Nieuwe code uitproberen :) \r\n"); |
TimDouma | 0:46a1642a10c8 | 146 | |
s1600907 | 4:bd1db680e5a1 | 147 | TickerVeranderenXY.attach(&VeranderenXY,1); // ticker voor het veranderen van de x en y waardes |
s1600907 | 4:bd1db680e5a1 | 148 | TickerAfstandBerekenen.attach(&AfstandBerekenen,1); // ticker om de waardes van dia1 en dia 2 te berekenen |
s1600907 | 4:bd1db680e5a1 | 149 | TickerMotorHoekBerekenen.attach(&MotorHoekBerekenen,1); // ticker om de motorhoek te berekenen |
s1600907 | 4:bd1db680e5a1 | 150 | TickerBerekenenBewegingsHoek.attach(&BerekenenBewegingsHoek,1); // ticker om grote en richting vd beweging te berekenen |
s1600907 | 4:bd1db680e5a1 | 151 | TickerFlipperBewegen.attach(&FlipperBewegen,1); // ticker om de flipper te laten bewegen |
TimDouma | 2:bf1466ac4e6f | 152 | |
TimDouma | 0:46a1642a10c8 | 153 | while (true) { |
TimDouma | 0:46a1642a10c8 | 154 | |
TimDouma | 0:46a1642a10c8 | 155 | } |
TimDouma | 0:46a1642a10c8 | 156 | } |