Diff: Algorithm.cpp

Revision:

2:d2ac643a08f6

Parent:

1:66d79e3ee3d1

diff -r 66d79e3ee3d1 -r d2ac643a08f6 Algorithm.cpp
--- a/Algorithm.cpp	Tue Feb 16 19:23:52 2021 +0000
+++ b/Algorithm.cpp	Mon Mar 01 15:32:29 2021 +0000
@@ -3,25 +3,25 @@
 
 //R1Angle
 float Algorithm::R1Angle(float th, float a)
-{
-    return -tan(PI/2-2*a+th)*(REFLECTOR1_DIST+REFLECTOR1_LENGTH*cos(a))+
-    PANEL_HEIGHT-REFLECTOR1_HEIGHT-REFLECTOR1_LENGTH*sin(a);
+{   
+    return -tan(PI/2-2*a+th)*(REFLECTOR1_DIST+REFLECTOR1_LENGTH/2*cos(a))+
+    PANEL_HEIGHT-REFLECTOR1_HEIGHT+REFLECTOR1_LENGTH/2*sin(a);
 }
 
 //R2Angle
 float Algorithm::R2Angle(float th, float a)
 {
-    return tan(PI/2-2*a+th)*(-REFLECTOR2_DIST-REFLECTOR2_LENGTH*cos(a))+
-    PANEL_HEIGHT-REFLECTOR2_HEIGHT-REFLECTOR2_LENGTH*sin(a);
+    return tan(PI/2-2*a-th)*(-REFLECTOR2_DIST-REFLECTOR2_LENGTH/2*cos(a))+
+    PANEL_HEIGHT-REFLECTOR2_HEIGHT+REFLECTOR2_LENGTH/2*sin(a);
 }
 
 //calcAngle
 float Algorithm::calcAngle(int n, float th)
 {
-    float err,ap,a,an,af,afa;
+    float err,ap,a,an,af;
     //Initial conditions
-    a = PI/4;
-    an = -PI/4;
+    a = 0.1;
+    an = PI/4;
     
     th *= PI/180; //Convert to rad
     
@@ -30,8 +30,7 @@
         case 1:
         for(int i = 0;i < LOOP_LIMIT;i++)
         {
-            ap = a - Algorithm::R1Angle(th,a)*(a-an)/(Algorithm::R1Angle(th,a)
-                -Algorithm::R1Angle(th,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,17 +39,15 @@
             {
                 break;
             }
-            af = ((ap+an)/2)*180/PI; //Convert to degrees
-            af = (af <= REFLECTOR1_LOW)? REFLECTOR1_LOW:(af >= REFLECTOR1_HIGH)?
-                REFLECTOR1_HIGH : af;
         }
+        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++)
         {
-            ap = a - Algorithm::R2Angle(th,a)/((Algorithm::R2Angle(th,a)
-                -Algorithm::R2Angle(th,an))/(a-an));
+            ap = a - Algorithm::R2Angle(th,a)*(a-an)/(Algorithm::R2Angle(th,a)-Algorithm::R2Angle(th,an));
             an = a;
             a = ap;
             err = ((ap-an) > 0)?(ap-an):(an-ap);
@@ -59,10 +56,9 @@
             {
                 break;
             }
-            af = ((ap+an)/2)*180/PI; //Convert to degrees
-            af = (af <= REFLECTOR2_LOW)? REFLECTOR2_LOW:(af >= REFLECTOR2_HIGH)?
-                REFLECTOR2_HIGH : af;
         }
+        af = ((ap+an)/2)*180/PI; //Convert to degrees
+        af = (af <= REFLECTOR2_LOW)? REFLECTOR2_LOW:(af >= REFLECTOR2_HIGH)? REFLECTOR2_HIGH : af;
         break;
     }