Diff: main.cpp

Revision:

4:05c5da1c8b08

Parent:

3:c524afedf863

Child:

5:b4a0301ec8cc

--- a/main.cpp	Mon Oct 24 09:00:05 2016 +0000
+++ b/main.cpp	Mon Oct 24 10:15:56 2016 +0000
@@ -121,20 +121,32 @@
 }
 
 ///////////////////// functies voor motorhoekberekenen //////////////////////////
-// vorige x berekenen
+// vorige x opslaan
 double Calc_Prev_x () {
     double Prev_x = x;
         //pc.printf("prev x = %f\r\n", Prev_x);
     return Prev_x;
 }
 
-// vorige y berekenen
+// vorige y opslaan
 double Calc_Prev_y () {
     double Prev_y = y;
         //pc.printf("prev y = %f\r\n", Prev_y);
     return Prev_y;
 }
 
+// vorige Theta1Gear opslaan
+double Calc_Prev_Theta1_Gear () {
+    double Prev_Theta1_Gear = Theta1Gear;
+    return Prev_Theta1_Gear;
+}
+
+//vorige Theta2Gear opslaan
+double Calc_Prev_Theta2_Gear () {
+    double Prev_Theta2_Gear = Theta2Gear;
+    return Prev_Theta2_Gear;
+}
+
 void FunctieFlipper();      // declarate function, function discribed below
 // berekenen van de nieuwe x en y waardes
 bool CalcXY (bool BicepsLeft, bool BicepsRight, bool Leg, bool Stepper_State) {
@@ -201,11 +213,9 @@
 }
 
 // calculate Theta1
-void CalcTheta1 (double Dia1, double Prev_x, double Prev_y) {
+void CalcTheta1 (double Dia1) {
     double a = 50.0; 
     double Bar = 200.0;    // lengte van de armen 
-    double Prev_Theta1_Gear = Theta1Gear; 
-    double MaxThetaGear = 100.0;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
     
     // Hoek berekenen van het grote tandwiel (gear)
     if (x > -a) {
@@ -219,25 +229,17 @@
     }
     Theta1Gear = Theta1Gear*180.0/pi;       // veranderen van radialen naar graden
              
-    // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
-    if (Theta1Gear >= MaxThetaGear) {    // is de maximale hoek in radialen
-        Theta1Gear = Prev_Theta1_Gear;
-        x = Prev_x;
-        y = Prev_y;
-    }
-    
     // omrekenen van grote tandwiel naar motortandwiel
     Theta1 = Theta1Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12. 
         
-    pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta1, Theta1Gear, Prev_Theta1_Gear);
+    pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta1, Theta1Gear);
 }
 
-void CalcTheta2 (double Dia2, double Prev_x, double Prev_y) {
+void CalcTheta2 (double Dia2) {
     double a = 50.0; 
     double Bar = 200.0;    // lengte van de armen 
     double Prev_Theta2_Gear = Theta2Gear; 
-    double MaxThetaGear = 100.0;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
-    
+        
     // Hoek berekenen van het grote tandwiel (gear)
     if (x < a) {
         Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
@@ -250,28 +252,39 @@
     }
     Theta2Gear = Theta2Gear*180/pi;         // veranderen van radialen naar graden
     
-        // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes)
-    if (Theta2Gear >= MaxThetaGear) {
+    // omrekenen van grote tandwiel naar motortandwiel
+    Theta2 = Theta2Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
+
+    pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta2, Theta2Gear);
+}
+
+// als een van de hoeken te groot wordt, zet dan alles terug naar de vorige positie
+void AngleLimits (double Prev_Theta1_Gear, double Prev_Theta2_Gear, double Prev_x, double Prev_y) {
+    double MaxThetaGear = 100.0;   // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
+    
+    if (Theta1Gear >= MaxThetaGear || Theta2Gear >= MaxThetaGear) {
+        Theta1Gear = Prev_Theta1_Gear;
         Theta2Gear = Prev_Theta2_Gear;
         x = Prev_x;
         y = Prev_y;
+        
+        Theta1 = Theta1Gear*42.0/12.0;
+        Theta2 = Theta2Gear*42.0/12.0; 
     }
-    
-    // omrekenen van grote tandwiel naar motortandwiel
-    Theta2 = Theta2Gear*42.0/12.0;  // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
-
-    pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta2, Theta2Gear, Prev_Theta2_Gear);
 }
 
 void CalculationsForTheta () {
     sample();
     double Prev_x = Calc_Prev_x ();
     double Prev_y = Calc_Prev_y ();
+    double Calc_Prev_Theta1_Gear ();
+    double Calc_Prev_Theta2_Gear ();
     bool Stepper_State = CalcXY (BicepsLeft, BicepsRight, Leg, Stepper_State);
     double Dia1 = CalcDia1 (x, y);
     double Dia2 = CalcDia2 (x, y);
-    CalcTheta1 (Dia1, Prev_x, Prev_y);
-    CalcTheta2 (Dia2, Prev_x, Prev_y); 
+    CalcTheta1 (Dia1);
+    CalcTheta2 (Dia2); 
+    AngleLimits (Theta1Gear, Theta2Gear, Prev_x, Prev_y);
 
     scope.send();
 }