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@2:d2ac643a08f6, 2021-03-01 (annotated)

Committer:
khaiminhvn
Date:
Mon Mar 01 15:32:29 2021 +0000
Revision:
2:d2ac643a08f6
Parent:
1:66d79e3ee3d1 
Fixed angle calculation algorithm

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 2:d2ac643a08f6 6 {
khaiminhvn 2:d2ac643a08f6 7 return -tan(PI/2-2*a+th)*(REFLECTOR1_DIST+REFLECTOR1_LENGTH/2*cos(a))+
khaiminhvn 2:d2ac643a08f6 8 PANEL_HEIGHT-REFLECTOR1_HEIGHT+REFLECTOR1_LENGTH/2*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 2:d2ac643a08f6 14 return tan(PI/2-2*a-th)*(-REFLECTOR2_DIST-REFLECTOR2_LENGTH/2*cos(a))+
khaiminhvn 2:d2ac643a08f6 15 PANEL_HEIGHT-REFLECTOR2_HEIGHT+REFLECTOR2_LENGTH/2*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 2:d2ac643a08f6 21 float err,ap,a,an,af;
khaiminhvn 0:cc55071995e8 22 //Initial conditions
khaiminhvn 2:d2ac643a08f6 23 a = 0.1;
khaiminhvn 2:d2ac643a08f6 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 2:d2ac643a08f6 33 ap = a - Algorithm::R1Angle(th,a)*(a-an)/(Algorithm::R1Angle(th,a)-Algorithm::R1Angle(th,an));
khaiminhvn 0:cc55071995e8 34 an = a;
khaiminhvn 0:cc55071995e8 35 a = ap;
khaiminhvn 0:cc55071995e8 36 err = ((ap-an) > 0)?(ap-an):(an-ap);
khaiminhvn 0:cc55071995e8 37
khaiminhvn 0:cc55071995e8 38 if(err <= ERR_LIMIT)
khaiminhvn 0:cc55071995e8 39 {
khaiminhvn 0:cc55071995e8 40 break;
khaiminhvn 0:cc55071995e8 41 }
khaiminhvn 0:cc55071995e8 42 }
khaiminhvn 2:d2ac643a08f6 43 af = ((ap+an)/2)*180/PI; //Convert to degrees
khaiminhvn 2:d2ac643a08f6 44 af = (af <= REFLECTOR1_LOW)? REFLECTOR1_LOW:(af >= REFLECTOR1_HIGH)? REFLECTOR1_HIGH : af;
khaiminhvn 1:66d79e3ee3d1 45 break;
khaiminhvn 0:cc55071995e8 46
khaiminhvn 0:cc55071995e8 47 case 2:
khaiminhvn 0:cc55071995e8 48 for(int i = 0;i < LOOP_LIMIT;i++)
khaiminhvn 0:cc55071995e8 49 {
khaiminhvn 2:d2ac643a08f6 50 ap = a - Algorithm::R2Angle(th,a)*(a-an)/(Algorithm::R2Angle(th,a)-Algorithm::R2Angle(th,an));
khaiminhvn 0:cc55071995e8 51 an = a;
khaiminhvn 0:cc55071995e8 52 a = ap;
khaiminhvn 0:cc55071995e8 53 err = ((ap-an) > 0)?(ap-an):(an-ap);
khaiminhvn 0:cc55071995e8 54
khaiminhvn 0:cc55071995e8 55 if(err <= ERR_LIMIT)
khaiminhvn 0:cc55071995e8 56 {
khaiminhvn 0:cc55071995e8 57 break;
khaiminhvn 0:cc55071995e8 58 }
khaiminhvn 0:cc55071995e8 59 }
khaiminhvn 2:d2ac643a08f6 60 af = ((ap+an)/2)*180/PI; //Convert to degrees
khaiminhvn 2:d2ac643a08f6 61 af = (af <= REFLECTOR2_LOW)? REFLECTOR2_LOW:(af >= REFLECTOR2_HIGH)? REFLECTOR2_HIGH : af;
khaiminhvn 1:66d79e3ee3d1 62 break;
khaiminhvn 0:cc55071995e8 63 }
khaiminhvn 0:cc55071995e8 64
khaiminhvn 0:cc55071995e8 65
khaiminhvn 0:cc55071995e8 66 return af;
khaiminhvn 0:cc55071995e8 67 }