Biorobotics_group_2 / Mbed 2 deprecated Berekenen_motorhoek

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?

UserRevisionLine numberNew 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 }