-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy patharrayOfLDR.cpp
95 lines (65 loc) · 2.3 KB
/
arrayOfLDR.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
/*arrayOfLDR.cpp*/
#include "Arduino.h"
#include "arrayOfLDR.h"
arrayOfLDR::arrayOfLDR(int ld1,int ld2,int ld3,int ld4,int ld5,int ld6)
{
pinMode(ld1,INPUT);
pinMode(ld2,INPUT);
pinMode(ld3,INPUT);
pinMode(ld4,INPUT);
pinMode(ld5,INPUT);
pinMode(ld6,INPUT);
_ld1=ld1;
_ld2=ld2;
_ld3=ld3;
_ld4=ld4;
_ld5=ld5;
_ld6=ld6;
}
float arrayOfLDR::read_sensor_values()
{
int ldr1=digitalRead(_ld1);
int ldr2=digitalRead(_ld2);
int ldr3=digitalRead(_ld3);
int ldr4=digitalRead(_ld4);
int ldr5=digitalRead(_ld5);
int ldr6=digitalRead(_ld6);
int junction=ldr1+ldr2+ldr3+ldr4+ldr5+ldr6;
float ret_error = 10;
if(junction > 3) return 20;
else if(ldr1==0 && ldr2==0 && ldr3==1 && ldr4==1 && ldr5==0 && ldr6==0)
ret_error = 0;
else if(ldr1==0 && ldr2==0 && ldr3==1 && ldr4==0 && ldr5==0 && ldr6==0)
ret_error = 1;
else if(ldr1==0 && ldr2==1 && ldr3==1 && ldr4==0 && ldr5==0 && ldr6==0)
ret_error = 2;
else if(ldr1==0 && ldr2==1 && ldr3==0 && ldr4==0 && ldr5==0 && ldr6==0 )
ret_error = 3;
else if(ldr1==1 && ldr2==1 && ldr3==0 && ldr4==0 && ldr5==0 && ldr6==0)
ret_error = 4;
else if(ldr1==1 && ldr2==0 && ldr3==0 && ldr4==0 && ldr5==0 && ldr6==0)
ret_error = 5;
else if(ldr1==0 && ldr2==0 && ldr3==0 && ldr4==1 && ldr5==0 && ldr6==0)
ret_error = -1;
else if(ldr1==0 && ldr2==0 && ldr3==0 && ldr4==1 && ldr5==1 && ldr6==0)
ret_error = -2;
else if(ldr1==0 && ldr2==0 && ldr3==0 && ldr4==0 && ldr5==1 && ldr6==0)
ret_error = -3;
else if(ldr1==0 && ldr2==0 && ldr3==0 && ldr4==0 && ldr5==1 && ldr6==1)
ret_error = -4;
else if(ldr1==0 && ldr2==0 && ldr3==0 && ldr4==0 && ldr5==0 && ldr6==1)
ret_error = -5;
else if(ldr1==0 && ldr2==0 && ldr3==0 && ldr4==0 && ldr5==0 && ldr6==0)
ret_error = 0.5;
else ret_error = 10;
return ret_error;
}
double arrayOfLDR::calculate_pid(int error,int previous_error,int previous_sum)
{
int p=error;
int i=error+previous_sum;
int d=error-previous_error;
double kp=7.5,kd=2.5,ki=0;
double correction_value=(kp)*p+(ki)*i+(kd)*d;
return correction_value;
}