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) {