Minh Nguyen / Algorithm

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Algorithm.cpp@0:cc55071995e8, 2021-02-09 (annotated)

Committer:
khaiminhvn
Date:
Tue Feb 09 23:53:33 2021 +0000
Revision:
0:cc55071995e8
Child:
1:66d79e3ee3d1
First draft

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaiminhvn 0:cc55071995e8 1 //INCLUDES
khaiminhvn 0:cc55071995e8 2 #include "Algorithm.h"
khaiminhvn 0:cc55071995e8 3
khaiminhvn 0:cc55071995e8 4 //R1Angle
khaiminhvn 0:cc55071995e8 5 float Algorithm::R1Angle(float th, float a)
khaiminhvn 0:cc55071995e8 6 {
khaiminhvn 0:cc55071995e8 7 return -tan(PI/2-2*a+th)*(REFLECTOR1_DIST+REFLECTOR1_LENGTH*cos(a))+
khaiminhvn 0:cc55071995e8 8 PANEL_HEIGHT-REFLECTOR1_HEIGHT-REFLECTOR1_LENGTH*sin(a);
khaiminhvn 0:cc55071995e8 9 }
khaiminhvn 0:cc55071995e8 10
khaiminhvn 0:cc55071995e8 11 //R2Angle
khaiminhvn 0:cc55071995e8 12 float Algorithm::R2Angle(float th, float a)
khaiminhvn 0:cc55071995e8 13 {
khaiminhvn 0:cc55071995e8 14 return tan(PI/2-2*a+th)*(-REFLECTOR2_DIST-REFLECTOR2_LENGTH*cos(a))+
khaiminhvn 0:cc55071995e8 15 PANEL_HEIGHT-REFLECTOR2_HEIGHT-REFLECTOR2_LENGTH*sin(a);
khaiminhvn 0:cc55071995e8 16 }
khaiminhvn 0:cc55071995e8 17
khaiminhvn 0:cc55071995e8 18 //calcAngle
khaiminhvn 0:cc55071995e8 19 float Algorithm::calcAngle(int n, float th)
khaiminhvn 0:cc55071995e8 20 {
khaiminhvn 0:cc55071995e8 21 float err,ap,a,an,af;
khaiminhvn 0:cc55071995e8 22 //Initial conditions
khaiminhvn 0:cc55071995e8 23 a = PI/4;
khaiminhvn 0:cc55071995e8 24 an = -PI/4;
khaiminhvn 0:cc55071995e8 25
khaiminhvn 0:cc55071995e8 26 th *= PI/180; //Convert to rad
khaiminhvn 0:cc55071995e8 27
khaiminhvn 0:cc55071995e8 28 switch(n)
khaiminhvn 0:cc55071995e8 29 {
khaiminhvn 0:cc55071995e8 30 case 1:
khaiminhvn 0:cc55071995e8 31 for(int i = 0;i < LOOP_LIMIT;i++)
khaiminhvn 0:cc55071995e8 32 {
khaiminhvn 0:cc55071995e8 33 ap = a - Algorithm::R1Angle(th,a)/((Algorithm::R1Angle(th,a)
khaiminhvn 0:cc55071995e8 34 -Algorithm::R1Angle(th,an))/(a-an));
khaiminhvn 0:cc55071995e8 35 an = a;
khaiminhvn 0:cc55071995e8 36 a = ap;
khaiminhvn 0:cc55071995e8 37 err = ((ap-an) > 0)?(ap-an):(an-ap);
khaiminhvn 0:cc55071995e8 38
khaiminhvn 0:cc55071995e8 39 if(err <= ERR_LIMIT)
khaiminhvn 0:cc55071995e8 40 {
khaiminhvn 0:cc55071995e8 41 break;
khaiminhvn 0:cc55071995e8 42 }
khaiminhvn 0:cc55071995e8 43 af = ((ap-an)/2)*180/PI; //Convert to degrees
khaiminhvn 0:cc55071995e8 44 af = (af <= REFLECTOR1_LOW)? REFLECTOR1_LOW:(af >= REFLECTOR1_HIGH)?
khaiminhvn 0:cc55071995e8 45 REFLECTOR1_HIGH : af;
khaiminhvn 0:cc55071995e8 46 }
khaiminhvn 0:cc55071995e8 47
khaiminhvn 0:cc55071995e8 48 case 2:
khaiminhvn 0:cc55071995e8 49 for(int i = 0;i < LOOP_LIMIT;i++)
khaiminhvn 0:cc55071995e8 50 {
khaiminhvn 0:cc55071995e8 51 ap = a - Algorithm::R2Angle(th,a)/((Algorithm::R2Angle(th,a)
khaiminhvn 0:cc55071995e8 52 -Algorithm::R2Angle(th,an))/(a-an));
khaiminhvn 0:cc55071995e8 53 an = a;
khaiminhvn 0:cc55071995e8 54 a = ap;
khaiminhvn 0:cc55071995e8 55 err = ((ap-an) > 0)?(ap-an):(an-ap);
khaiminhvn 0:cc55071995e8 56
khaiminhvn 0:cc55071995e8 57 if(err <= ERR_LIMIT)
khaiminhvn 0:cc55071995e8 58 {
khaiminhvn 0:cc55071995e8 59 break;
khaiminhvn 0:cc55071995e8 60 }
khaiminhvn 0:cc55071995e8 61 af = ((ap-an)/2)*180/PI; //Convert to degrees
khaiminhvn 0:cc55071995e8 62 af = (af <= REFLECTOR2_LOW)? REFLECTOR2_LOW:(af >= REFLECTOR2_HIGH)?
khaiminhvn 0:cc55071995e8 63 REFLECTOR2_HIGH : af;
khaiminhvn 0:cc55071995e8 64 }
khaiminhvn 0:cc55071995e8 65 }
khaiminhvn 0:cc55071995e8 66
khaiminhvn 0:cc55071995e8 67
khaiminhvn 0:cc55071995e8 68 return af;
khaiminhvn 0:cc55071995e8 69 }