Diff: Algorithm.cpp

Revision:

1:66d79e3ee3d1

Parent:

0:cc55071995e8

Child:

2:d2ac643a08f6

--- a/Algorithm.cpp	Tue Feb 09 23:53:33 2021 +0000
+++ b/Algorithm.cpp	Tue Feb 16 19:23:52 2021 +0000
@@ -18,7 +18,7 @@
 //calcAngle
 float Algorithm::calcAngle(int n, float th)
 {
-    float err,ap,a,an,af;
+    float err,ap,a,an,af,afa;
     //Initial conditions
     a = PI/4;
     an = -PI/4;
@@ -30,8 +30,8 @@
         case 1:
         for(int i = 0;i < LOOP_LIMIT;i++)
         {
-            ap = a - Algorithm::R1Angle(th,a)/((Algorithm::R1Angle(th,a)
-                -Algorithm::R1Angle(th,an))/(a-an));
+            ap = a - Algorithm::R1Angle(th,a)*(a-an)/(Algorithm::R1Angle(th,a)
+                -Algorithm::R1Angle(th,an));
             an = a;
             a = ap;
             err = ((ap-an) > 0)?(ap-an):(an-ap);
@@ -40,10 +40,11 @@
             {
                 break;
             }
-            af = ((ap-an)/2)*180/PI; //Convert to degrees
+            af = ((ap+an)/2)*180/PI; //Convert to degrees
             af = (af <= REFLECTOR1_LOW)? REFLECTOR1_LOW:(af >= REFLECTOR1_HIGH)?
                 REFLECTOR1_HIGH : af;
         }
+        break;
         
         case 2:
         for(int i = 0;i < LOOP_LIMIT;i++)
@@ -58,10 +59,11 @@
             {
                 break;
             }
-            af = ((ap-an)/2)*180/PI; //Convert to degrees
+            af = ((ap+an)/2)*180/PI; //Convert to degrees
             af = (af <= REFLECTOR2_LOW)? REFLECTOR2_LOW:(af >= REFLECTOR2_HIGH)?
                 REFLECTOR2_HIGH : af;
         }
+        break;
     }