Diff: Algorithm.cpp

Revision:

0:cc55071995e8

Child:

1:66d79e3ee3d1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Algorithm.cpp	Tue Feb 09 23:53:33 2021 +0000
@@ -0,0 +1,69 @@
+//INCLUDES
+#include "Algorithm.h"
+
+//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);
+}
+
+//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);
+}
+
+//calcAngle
+float Algorithm::calcAngle(int n, float th)
+{
+    float err,ap,a,an,af;
+    //Initial conditions
+    a = PI/4;
+    an = -PI/4;
+    
+    th *= PI/180; //Convert to rad
+    
+    switch(n)
+    {
+        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));
+            an = a;
+            a = ap;
+            err = ((ap-an) > 0)?(ap-an):(an-ap);
+            
+            if(err <= ERR_LIMIT)
+            {
+                break;
+            }
+            af = ((ap-an)/2)*180/PI; //Convert to degrees
+            af = (af <= REFLECTOR1_LOW)? REFLECTOR1_LOW:(af >= REFLECTOR1_HIGH)?
+                REFLECTOR1_HIGH : af;
+        }
+        
+        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));
+            an = a;
+            a = ap;
+            err = ((ap-an) > 0)?(ap-an):(an-ap);
+            
+            if(err <= ERR_LIMIT)
+            {
+                break;
+            }
+            af = ((ap-an)/2)*180/PI; //Convert to degrees
+            af = (af <= REFLECTOR2_LOW)? REFLECTOR2_LOW:(af >= REFLECTOR2_HIGH)?
+                REFLECTOR2_HIGH : af;
+        }
+    }
+    
+    
+    return af; 
+}
\ No newline at end of file