Biorobotics_group_2 / Mbed 2 deprecated Berekenen_motorhoek

Diagonaal berekenen lukt, meerdere outputs nog niet

Dependencies:   mbed FastPWM HIDScope MODSERIAL QEI

main.cpp@5:a9da7bc89b24, 2016-10-17 (annotated)

Committer:
s1600907
Date:
Mon Oct 17 14:25:12 2016 +0000
Revision:
5:a9da7bc89b24
Parent:
4:bd1db680e5a1
Child:
6:ee243782bf51
Alle mogelijke bewegingen staan erin. Deze moeten nog aangevuld worden met Sjoerd zijn onderdelen om de beweging ook te realiseren;

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 5:a9da7bc89b24 7 InterruptIn Button1(PTC6); // DIT WORDT EEN ANDERE BUTTON
s1600907 5:a9da7bc89b24 8
s1600907 5:a9da7bc89b24 9 // tickers
s1600907 5:a9da7bc89b24 10 Ticker TickerCaseBepalen;
s1600907 4:bd1db680e5a1 11 Ticker TickerVeranderenXY;
TimDouma 0:46a1642a10c8 12 Ticker TickerAfstandBerekenen;
s1600907 3:ac6f86c0db6e 13 Ticker TickerMotorHoekBerekenen;
s1600907 4:bd1db680e5a1 14 Ticker TickerBerekenenBewegingsHoek;
s1600907 4:bd1db680e5a1 15 Ticker TickerFlipperBewegen;
s1600907 3:ac6f86c0db6e 16
s1600907 5:a9da7bc89b24 17 // in te voeren x en y waardes
s1600907 5:a9da7bc89b24 18 volatile float x = 0.0;
s1600907 5:a9da7bc89b24 19 volatile float y = 305.5;
s1600907 5:a9da7bc89b24 20 volatile float vorigex = 0.0;
s1600907 5:a9da7bc89b24 21 volatile float vorigey = 0.0;
TimDouma 0:46a1642a10c8 22
TimDouma 0:46a1642a10c8 23 // waardes
s1600907 5:a9da7bc89b24 24 volatile const float a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden) KEERTJE NAMETEN
s1600907 5:a9da7bc89b24 25 volatile const float Bar = 200.0; // lengte van de armen KEERTJE NAMETEN
s1600907 3:ac6f86c0db6e 26 volatile const float pi = 3.14159265359;
s1600907 5:a9da7bc89b24 27 volatile float VorigeHoek1 = 1.0/3.0*pi; // begin hoek van 90 graden
s1600907 5:a9da7bc89b24 28 volatile float VorigeHoek2 = 1.0/3.0*pi;
s1600907 5:a9da7bc89b24 29 volatile float Step = 1; // EEN GOK VOOR HET STAPJE EN MOET DUS NOG AANGEPAST
s1600907 5:a9da7bc89b24 30 volatile float MaxHoek = pi - 30*pi/180; // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel)
TimDouma 0:46a1642a10c8 31
s1600907 3:ac6f86c0db6e 32 volatile float Dia1 = 0.0;
s1600907 3:ac6f86c0db6e 33 volatile float Dia2 = 0.0;
s1600907 3:ac6f86c0db6e 34 volatile float MotorHoek1 = 0.0;
s1600907 3:ac6f86c0db6e 35 volatile float MotorHoek2 = 0.0;
s1600907 4:bd1db680e5a1 36 volatile float BewegingsHoek1 = 0.0;
s1600907 4:bd1db680e5a1 37 volatile float BewegingsHoek2 = 0.0;
s1600907 4:bd1db680e5a1 38 volatile bool Richting1 = true; // cw = true, ccw = false
s1600907 4:bd1db680e5a1 39 volatile bool Richting2 = true;
s1600907 4:bd1db680e5a1 40 volatile bool FlipperAan = false; // als true dan flipper aanzetten
s1600907 5:a9da7bc89b24 41 bool BezigMetCalibreren = true; // we beginnen met calibreren
s1600907 5:a9da7bc89b24 42 bool BeginnenMetFlippen = false; // om de flipper aan te zetten
s1600907 5:a9da7bc89b24 43 bool BezigMetFlippen = false; // om verandering in x en y tegen te houden tijdens het flippen. DIT WORDT VERANDERD IN SJOERDS DEEL (NAAM CONTROLEREN)
s1600907 3:ac6f86c0db6e 44
s1600907 5:a9da7bc89b24 45 // waardes die komen van Charlotte: true is aangespannen, false is niet aangespannen (NAAM CONTROLEREN)
s1600907 5:a9da7bc89b24 46 volatile bool BicepsLinks = false;
s1600907 4:bd1db680e5a1 47 volatile bool BicepsRechts = true;
s1600907 5:a9da7bc89b24 48 volatile bool Been = false;
s1600907 5:a9da7bc89b24 49
s1600907 5:a9da7bc89b24 50 // nodig voor het switchstatement
s1600907 5:a9da7bc89b24 51 enum states {Flipper, Links, Rechts, Voor, Achter, GeenSignaal};
s1600907 5:a9da7bc89b24 52 states MyState = Links;
s1600907 5:a9da7bc89b24 53
s1600907 5:a9da7bc89b24 54 // het bepalen van welke case we hebben
s1600907 5:a9da7bc89b24 55 void CaseBepalen () {
s1600907 5:a9da7bc89b24 56 if (BicepsLinks==true && BicepsRechts==true && Been==true && BezigMetFlippen == false) {
s1600907 5:a9da7bc89b24 57 MyState = Flipper; // flipper bewegen
s1600907 5:a9da7bc89b24 58 }
s1600907 5:a9da7bc89b24 59 else if (BicepsLinks==true && BicepsRechts==false && Been==false && BezigMetFlippen == false) {
s1600907 5:a9da7bc89b24 60 MyState = Links; // naar links bewegen
s1600907 5:a9da7bc89b24 61 }
s1600907 5:a9da7bc89b24 62 else if (BicepsLinks==false && BicepsRechts==true && Been==false && BezigMetFlippen == false) {
s1600907 5:a9da7bc89b24 63 MyState = Rechts; // naar rechts bewegen
s1600907 5:a9da7bc89b24 64 }
s1600907 5:a9da7bc89b24 65 else if (BicepsLinks==true && BicepsRechts==true && Been==false && BezigMetFlippen == false) {
s1600907 5:a9da7bc89b24 66 MyState = Voor; // naar voren bewegen
s1600907 5:a9da7bc89b24 67 }
s1600907 5:a9da7bc89b24 68 else if (BicepsLinks==false && BicepsRechts==true && Been==true && BezigMetFlippen == false) {
s1600907 5:a9da7bc89b24 69 MyState = Achter; // naar achter bewegen
s1600907 5:a9da7bc89b24 70 }
s1600907 5:a9da7bc89b24 71 else if (BicepsLinks==false && BicepsRechts==false && Been==false || BezigMetFlippen==true) {
s1600907 5:a9da7bc89b24 72 MyState = GeenSignaal;
s1600907 5:a9da7bc89b24 73 }
s1600907 5:a9da7bc89b24 74 }
s1600907 4:bd1db680e5a1 75
s1600907 4:bd1db680e5a1 76 // het berekenen van de nieuwe x en y en zorgen dat deze niet buiten het bereik komen
s1600907 4:bd1db680e5a1 77 void VeranderenXY () {
s1600907 5:a9da7bc89b24 78 // bepalen hoe de x en y waardes gaan veranderen
s1600907 5:a9da7bc89b24 79 switch(MyState) {
s1600907 5:a9da7bc89b24 80 case Flipper :
s1600907 5:a9da7bc89b24 81 BeginnenMetFlippen = true; // we zijn aan het flipper dus geen andere dingen doen
s1600907 5:a9da7bc89b24 82 break;
s1600907 5:a9da7bc89b24 83 case Links :
s1600907 5:a9da7bc89b24 84 vorigex = x; // ervoor zorgen dat als de motor te ver draait de x niet verder telt
s1600907 5:a9da7bc89b24 85 x = x - Step;
s1600907 5:a9da7bc89b24 86 break;
s1600907 5:a9da7bc89b24 87 case Rechts :
s1600907 5:a9da7bc89b24 88 vorigex = x;
s1600907 5:a9da7bc89b24 89 x = x + Step;
s1600907 5:a9da7bc89b24 90 break;
s1600907 5:a9da7bc89b24 91 case Voor :
s1600907 5:a9da7bc89b24 92 vorigey = y; // ervoor zorgen dat als de motor te ver draait de y niet verder telt
s1600907 5:a9da7bc89b24 93 y = y + Step;
s1600907 5:a9da7bc89b24 94 break;
s1600907 5:a9da7bc89b24 95 case Achter :
s1600907 5:a9da7bc89b24 96 vorigey = y;
s1600907 5:a9da7bc89b24 97 y = y - Step;
s1600907 5:a9da7bc89b24 98 break;
s1600907 5:a9da7bc89b24 99 case GeenSignaal :
s1600907 5:a9da7bc89b24 100 break;
s1600907 5:a9da7bc89b24 101 default :
s1600907 5:a9da7bc89b24 102 pc.printf("Er is geen verandering in x en y\r\n");
s1600907 4:bd1db680e5a1 103 }
s1600907 5:a9da7bc89b24 104
s1600907 4:bd1db680e5a1 105 // Grenswaardes LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
s1600907 4:bd1db680e5a1 106 if (x > 200) {
s1600907 4:bd1db680e5a1 107 x = 200;
s1600907 4:bd1db680e5a1 108 }
s1600907 4:bd1db680e5a1 109 else if (x < -200) {
s1600907 4:bd1db680e5a1 110 x = -200;
s1600907 4:bd1db680e5a1 111 }
s1600907 5:a9da7bc89b24 112 if (y > 306) {
s1600907 5:a9da7bc89b24 113 y = 306;
s1600907 4:bd1db680e5a1 114 }
s1600907 4:bd1db680e5a1 115 else if (y < 50) {
s1600907 4:bd1db680e5a1 116 y = 50; // GOKJE, UITPROBEREN
s1600907 4:bd1db680e5a1 117 }
s1600907 4:bd1db680e5a1 118 pc.printf("x = %f, y = %f\r\n", x, y);
s1600907 4:bd1db680e5a1 119 }
TimDouma 0:46a1642a10c8 120
TimDouma 0:46a1642a10c8 121 // functie om D1 en D2 te berekenen
s1600907 3:ac6f86c0db6e 122 void AfstandBerekenen (){
TimDouma 0:46a1642a10c8 123 float BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen
s1600907 3:ac6f86c0db6e 124 Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal
s1600907 3:ac6f86c0db6e 125
s1600907 3:ac6f86c0db6e 126 float BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm
s1600907 3:ac6f86c0db6e 127 Dia2 = pow(BV2,2)/(2*BV2);
s1600907 3:ac6f86c0db6e 128
s1600907 4:bd1db680e5a1 129 //pc.printf("Dia1 = %f, Dia2 = %f\r\n", Dia1, Dia2);
TimDouma 0:46a1642a10c8 130 }
TimDouma 0:46a1642a10c8 131
s1600907 3:ac6f86c0db6e 132 // functie om de motorhoek te berekenen
s1600907 3:ac6f86c0db6e 133 void MotorHoekBerekenen (){
s1600907 3:ac6f86c0db6e 134 // eerst if loop doorlopen voor motor 1
s1600907 3:ac6f86c0db6e 135 if (x > -a) {
s1600907 3:ac6f86c0db6e 136 MotorHoek1 = pi - atan(y/(x+a)) - acos(Dia1/Bar);
s1600907 3:ac6f86c0db6e 137 }
s1600907 3:ac6f86c0db6e 138 else if (x > -a) {
s1600907 3:ac6f86c0db6e 139 MotorHoek1 = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
s1600907 3:ac6f86c0db6e 140 }
s1600907 3:ac6f86c0db6e 141 else { // als x=-a
s1600907 3:ac6f86c0db6e 142 MotorHoek1 = 0.5f*pi - acos(Dia1/Bar);
s1600907 3:ac6f86c0db6e 143 }
s1600907 4:bd1db680e5a1 144 // twee if loop doorlopen voor motor 2
s1600907 3:ac6f86c0db6e 145 if (x < a) {
s1600907 3:ac6f86c0db6e 146 MotorHoek2 = pi + atan(y/(x-a)) - acos(Dia2/Bar);
s1600907 3:ac6f86c0db6e 147 }
s1600907 3:ac6f86c0db6e 148 else if (x > a) {
s1600907 3:ac6f86c0db6e 149 MotorHoek2 = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
s1600907 3:ac6f86c0db6e 150 }
s1600907 3:ac6f86c0db6e 151 else { // als x=a
s1600907 3:ac6f86c0db6e 152 MotorHoek2 = 0.5f*pi - acos(Dia1/Bar);
s1600907 3:ac6f86c0db6e 153 }
s1600907 5:a9da7bc89b24 154
s1600907 5:a9da7bc89b24 155 // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
s1600907 5:a9da7bc89b24 156 if (MotorHoek1 >= MaxHoek) { // is de maximale hoek in radialen
s1600907 5:a9da7bc89b24 157 MotorHoek1 = VorigeHoek1;
s1600907 5:a9da7bc89b24 158 x = vorigex;
s1600907 5:a9da7bc89b24 159 y = vorigey;
s1600907 5:a9da7bc89b24 160 }
s1600907 5:a9da7bc89b24 161 if (MotorHoek2 >= MaxHoek) {
s1600907 5:a9da7bc89b24 162 MotorHoek2 = VorigeHoek2;
s1600907 5:a9da7bc89b24 163 x = vorigex;
s1600907 5:a9da7bc89b24 164 y = vorigey;
s1600907 5:a9da7bc89b24 165 }
s1600907 4:bd1db680e5a1 166 //pc.printf("MotorHoek1 = %f en MotorHoek2 = %f \r\n", MotorHoek1, MotorHoek2);
s1600907 3:ac6f86c0db6e 167 }
TimDouma 2:bf1466ac4e6f 168
s1600907 5:a9da7bc89b24 169 // het verschil uitrekenen tussen waar we zijn en waar we heen willen OVERLAP MET SJOERD
s1600907 4:bd1db680e5a1 170 void BerekenenBewegingsHoek () {
s1600907 4:bd1db680e5a1 171 // Hoek 1: grootte beweging en richting
s1600907 4:bd1db680e5a1 172 BewegingsHoek1 = VorigeHoek1 - MotorHoek1;
s1600907 4:bd1db680e5a1 173 VorigeHoek1 = MotorHoek1;
s1600907 4:bd1db680e5a1 174 if (BewegingsHoek1 > 0) {
s1600907 4:bd1db680e5a1 175 Richting1 = false; // ccw
s1600907 4:bd1db680e5a1 176 }
s1600907 4:bd1db680e5a1 177 else {
s1600907 4:bd1db680e5a1 178 Richting1 = true; // cw
s1600907 4:bd1db680e5a1 179 }
s1600907 4:bd1db680e5a1 180
s1600907 4:bd1db680e5a1 181 // Hoek 2: grootte beweging en richting
s1600907 4:bd1db680e5a1 182 BewegingsHoek2 = VorigeHoek2 - MotorHoek2;
s1600907 4:bd1db680e5a1 183 VorigeHoek2 = MotorHoek2;
s1600907 4:bd1db680e5a1 184 if (BewegingsHoek2 > 0) {
s1600907 4:bd1db680e5a1 185 Richting2 = true; // cw
s1600907 4:bd1db680e5a1 186 }
s1600907 4:bd1db680e5a1 187 else {
s1600907 4:bd1db680e5a1 188 Richting2 = false; // ccw
s1600907 4:bd1db680e5a1 189 }
s1600907 4:bd1db680e5a1 190 //pc.printf("Beweging 1 = %f, Beweging 2 = %f\r\n", BewegingsHoek1, BewegingsHoek2);
s1600907 4:bd1db680e5a1 191 }
s1600907 4:bd1db680e5a1 192
s1600907 5:a9da7bc89b24 193 // als BeginnenMetFlippen aan staat voeren we deze functie uit. Zolang deze bezig is is BezigMetFlippen true
s1600907 4:bd1db680e5a1 194 void FlipperBewegen () {
s1600907 5:a9da7bc89b24 195 if (BeginnenMetFlippen == true) {
s1600907 5:a9da7bc89b24 196 // SJOERDS DEEL
s1600907 4:bd1db680e5a1 197 }
s1600907 5:a9da7bc89b24 198 else { // als BezigMetFlippen == false dan doe niks
s1600907 4:bd1db680e5a1 199 }
s1600907 4:bd1db680e5a1 200 }
s1600907 5:a9da7bc89b24 201
s1600907 5:a9da7bc89b24 202 // als de button ingedrukt wordt dan stoppen we met calibreren
s1600907 5:a9da7bc89b24 203 void ButtonIndrukken () {
s1600907 5:a9da7bc89b24 204 BezigMetCalibreren = false;
s1600907 5:a9da7bc89b24 205 }
s1600907 4:bd1db680e5a1 206
s1600907 3:ac6f86c0db6e 207 int main() {
s1600907 3:ac6f86c0db6e 208 pc.baud(SERIAL_BAUD);
s1600907 3:ac6f86c0db6e 209 pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
TimDouma 0:46a1642a10c8 210
s1600907 5:a9da7bc89b24 211 // calibreren gebeurt hier, zodat de tickers nog niet lopen en dit dus geen rekenkracht kost en je niet gebonden bent aan tijden
s1600907 5:a9da7bc89b24 212 // we gaan door met calibreren tot we de button indrukken
s1600907 5:a9da7bc89b24 213 while (BezigMetCalibreren == true) {
s1600907 5:a9da7bc89b24 214 // potmeter dingen aanpassen
s1600907 5:a9da7bc89b24 215
s1600907 5:a9da7bc89b24 216 pc.printf("calibreren is bezig\r\n");
s1600907 5:a9da7bc89b24 217 Button1.fall(&ButtonIndrukken);
s1600907 5:a9da7bc89b24 218 }
s1600907 5:a9da7bc89b24 219
s1600907 5:a9da7bc89b24 220 TickerCaseBepalen.attach(&CaseBepalen, 1); // ticker om te bepalen welke case we gaan gebruiken
s1600907 4:bd1db680e5a1 221 TickerVeranderenXY.attach(&VeranderenXY,1); // ticker voor het veranderen van de x en y waardes
s1600907 4:bd1db680e5a1 222 TickerAfstandBerekenen.attach(&AfstandBerekenen,1); // ticker om de waardes van dia1 en dia 2 te berekenen
s1600907 4:bd1db680e5a1 223 TickerMotorHoekBerekenen.attach(&MotorHoekBerekenen,1); // ticker om de motorhoek te berekenen
s1600907 4:bd1db680e5a1 224 TickerBerekenenBewegingsHoek.attach(&BerekenenBewegingsHoek,1); // ticker om grote en richting vd beweging te berekenen
s1600907 4:bd1db680e5a1 225 TickerFlipperBewegen.attach(&FlipperBewegen,1); // ticker om de flipper te laten bewegen
TimDouma 2:bf1466ac4e6f 226
TimDouma 0:46a1642a10c8 227 while (true) {
TimDouma 0:46a1642a10c8 228
TimDouma 0:46a1642a10c8 229 }
TimDouma 0:46a1642a10c8 230 }