Dynamics ident of the system
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed
Fork of Berekenen_motorhoek by
Diff: main.cpp
- Revision:
- 3:ac6f86c0db6e
- Parent:
- 2:bf1466ac4e6f
- Child:
- 4:bd1db680e5a1
--- a/main.cpp Thu Oct 13 12:34:30 2016 +0000 +++ b/main.cpp Thu Oct 13 14:33:26 2016 +0000 @@ -5,6 +5,8 @@ Serial pc(USBTX,USBRX); Ticker TickerAfstandBerekenen; +Ticker TickerMotorHoekBerekenen; + // in te voeren x en y waardes (de positie waar we heen willen) volatile float x = -30.0; volatile float y = 200.0; @@ -12,50 +14,58 @@ // 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 float D1 = 9.0; +volatile const float pi = 3.14159265359; +volatile float Dia1 = 0.0; +volatile float Dia2 = 0.0; +volatile float MotorHoek1 = 0.0; +volatile float MotorHoek2 = 0.0; + +//NOG ERGENS ZORGEN DAT X EN Y NIET BUITEN HET BEREIKBARE GEBIED KOMEN // functie om D1 en D2 te berekenen -float AfstandBerekenenLinks () -{ +void AfstandBerekenen (){ float BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen - float D1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal + 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.baud(SERIAL_BAUD); - pc.printf("D1 = %f\r\n", D1); - - return D1 ; + pc.printf("Dia1 = %f, Dia2 = %f\r\n", Dia1, Dia2); } -float AfstandBerekenenRechts() -{ - float BV2 = sqrt(pow((x-a),2) + pow(y,2));// diagonaal (afstand van armas tot locatie) berekenen - float D2 = pow(BV2,2)/(2*BV2); // berekenen van de afstand oorsprong tot diagonaal - pc.baud(SERIAL_BAUD); - pc.printf("D2 = %f\r\n", D2); - - return D2 ; -} -void Blij() -{ - pc.baud(SERIAL_BAUD); - pc.printf("het werkt wel met een void functie\r\n"); -} +// 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); + } + 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); +} -/* -void MotorhoekBerekenen (){ - if (x > -a){ - float MotorHoek1 = 180 - atan(y/(x+a)) - acos( -*/ -int main() -{ - float D1 = AfstandBerekenenLinks(); - float D2 = AfstandBerekenenRechts(); - - TickerAfstandBerekenen.attach(&AfstandBerekenenLinks,1); +int main() { + pc.baud(SERIAL_BAUD); + pc.printf("\r\n Nieuwe code uitproberen :) \r\n"); - pc.baud(SERIAL_BAUD); - pc.printf("D1 = %f en D2 = %f\r\n", D1, D2); + 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 while (true) {