Biorobotics_group_2 / Mbed 2 deprecated System_Identification

Dynamics ident of the system

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed

Fork of Berekenen_motorhoek by Biorobotics_group_2

main.cpp@3:ac6f86c0db6e, 2016-10-13 (annotated)

Committer:
s1600907
Date:
Thu Oct 13 14:33:26 2016 +0000
Revision:
3:ac6f86c0db6e
Parent:
2:bf1466ac4e6f
Child:
4:bd1db680e5a1
void functies gebruikt die waardes overschrijven. Berekenen van de motorhoeken toegevoegd. (alles werkt)

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"
TimDouma 0:46a1642a10c8 3 #define SERIAL_BAUD 115200
TimDouma 1:709a462b8373 4 #include "iostream"
TimDouma 0:46a1642a10c8 5
TimDouma 0:46a1642a10c8 6 Serial pc(USBTX,USBRX);
TimDouma 0:46a1642a10c8 7 Ticker TickerAfstandBerekenen;
s1600907 3:ac6f86c0db6e 8 Ticker TickerMotorHoekBerekenen;
s1600907 3:ac6f86c0db6e 9
TimDouma 0:46a1642a10c8 10 // in te voeren x en y waardes (de positie waar we heen willen)
TimDouma 0:46a1642a10c8 11 volatile float x = -30.0;
TimDouma 0:46a1642a10c8 12 volatile float y = 200.0;
TimDouma 0:46a1642a10c8 13
TimDouma 0:46a1642a10c8 14 // waardes
TimDouma 0:46a1642a10c8 15 volatile const float a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden)
TimDouma 0:46a1642a10c8 16 volatile const float Bar = 200.0; // lengte van de armen
s1600907 3:ac6f86c0db6e 17 volatile const float pi = 3.14159265359;
TimDouma 0:46a1642a10c8 18
s1600907 3:ac6f86c0db6e 19 volatile float Dia1 = 0.0;
s1600907 3:ac6f86c0db6e 20 volatile float Dia2 = 0.0;
s1600907 3:ac6f86c0db6e 21 volatile float MotorHoek1 = 0.0;
s1600907 3:ac6f86c0db6e 22 volatile float MotorHoek2 = 0.0;
s1600907 3:ac6f86c0db6e 23
s1600907 3:ac6f86c0db6e 24 //NOG ERGENS ZORGEN DAT X EN Y NIET BUITEN HET BEREIKBARE GEBIED KOMEN
TimDouma 0:46a1642a10c8 25
TimDouma 0:46a1642a10c8 26 // functie om D1 en D2 te berekenen
s1600907 3:ac6f86c0db6e 27 void AfstandBerekenen (){
TimDouma 0:46a1642a10c8 28 float BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen
s1600907 3:ac6f86c0db6e 29 Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal
s1600907 3:ac6f86c0db6e 30
s1600907 3:ac6f86c0db6e 31 float BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm
s1600907 3:ac6f86c0db6e 32 Dia2 = pow(BV2,2)/(2*BV2);
s1600907 3:ac6f86c0db6e 33
TimDouma 0:46a1642a10c8 34 pc.baud(SERIAL_BAUD);
s1600907 3:ac6f86c0db6e 35 pc.printf("Dia1 = %f, Dia2 = %f\r\n", Dia1, Dia2);
TimDouma 0:46a1642a10c8 36 }
TimDouma 0:46a1642a10c8 37
s1600907 3:ac6f86c0db6e 38 // functie om de motorhoek te berekenen
s1600907 3:ac6f86c0db6e 39 void MotorHoekBerekenen (){
s1600907 3:ac6f86c0db6e 40 // eerst if loop doorlopen voor motor 1
s1600907 3:ac6f86c0db6e 41 if (x > -a) {
s1600907 3:ac6f86c0db6e 42 MotorHoek1 = pi - atan(y/(x+a)) - acos(Dia1/Bar);
s1600907 3:ac6f86c0db6e 43 }
s1600907 3:ac6f86c0db6e 44 else if (x > -a) {
s1600907 3:ac6f86c0db6e 45 MotorHoek1 = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
s1600907 3:ac6f86c0db6e 46 }
s1600907 3:ac6f86c0db6e 47 else { // als x=-a
s1600907 3:ac6f86c0db6e 48 MotorHoek1 = 0.5f*pi - acos(Dia1/Bar);
s1600907 3:ac6f86c0db6e 49 }
TimDouma 2:bf1466ac4e6f 50
s1600907 3:ac6f86c0db6e 51 if (x < a) {
s1600907 3:ac6f86c0db6e 52 MotorHoek2 = pi + atan(y/(x-a)) - acos(Dia2/Bar);
s1600907 3:ac6f86c0db6e 53 }
s1600907 3:ac6f86c0db6e 54 else if (x > a) {
s1600907 3:ac6f86c0db6e 55 MotorHoek2 = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
s1600907 3:ac6f86c0db6e 56 }
s1600907 3:ac6f86c0db6e 57 else { // als x=a
s1600907 3:ac6f86c0db6e 58 MotorHoek2 = 0.5f*pi - acos(Dia1/Bar);
s1600907 3:ac6f86c0db6e 59 }
s1600907 3:ac6f86c0db6e 60 pc.printf("MotorHoek1 = %f en MotorHoek2 = %f \r\n", MotorHoek1, MotorHoek2);
s1600907 3:ac6f86c0db6e 61 }
TimDouma 2:bf1466ac4e6f 62
s1600907 3:ac6f86c0db6e 63 int main() {
s1600907 3:ac6f86c0db6e 64 pc.baud(SERIAL_BAUD);
s1600907 3:ac6f86c0db6e 65 pc.printf("\r\n Nieuwe code uitproberen :) \r\n");
TimDouma 0:46a1642a10c8 66
s1600907 3:ac6f86c0db6e 67 TickerAfstandBerekenen.attach(&AfstandBerekenen,1); // ticker om de waardes van dia1 en dia 2 te berekenen
s1600907 3:ac6f86c0db6e 68 TickerMotorHoekBerekenen.attach(&MotorHoekBerekenen,1); // ticker om de motorhoek te berekenen
TimDouma 2:bf1466ac4e6f 69
TimDouma 0:46a1642a10c8 70 while (true) {
TimDouma 0:46a1642a10c8 71
TimDouma 0:46a1642a10c8 72 }
TimDouma 0:46a1642a10c8 73 }