-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathv4 (with comments)
136 lines (127 loc) · 3.84 KB
/
v4 (with comments)
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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
int rPin = 4;
int bPin = 5;
int gPin = 6; //pin numbers for each LED
int sSensor1 = A0;
int sSensor2 = A1;
int bSensor1 = A2;
int bSensor2 = A3;
int bSensor3 = A4;
int bSensor4 = A5; //pin numbers for each pressure sensor
const int SWITCH = 9; //pin number for switch
//const int BUTTON = 9;
int val = 0; //reads SWITCH's value
int ss1Val=0;
int ss2Val=0;
int bs1Val=0;
int bs2Val=0;
int bs3Val=0;
int bs4Val=0; //reads corresponding sensor value
//int state = 0;
int goodPositions[2][6]; //2D array of all good positions.
//Each row is different position with corresponding sensor readings.
//Add as many good positions we determine
int currPosition[6]; //array of current sensor readings; need to add load sensors here
int row = 2; //#rows
int col = 6; //#cols in 2D array of good positions
boolean comparePosition(int r){
int count = 0; //check how many of sensor values are not being pressed
for(int c=0; c<col; c++){ //going through each sensor value
if(currPosition[c]<20){ //case 1: checking if each are pretty much not being pressed
count++;
}
}
if(count==col){
return true; //if all sensor values are pretty much 0, we have a good position
}
for(int c=0; c<col; c++){
if(currPosition[c]<goodPositions[r][c]){ //sensor isn't being pressed, when it should be if it's a good position
return false;
}
}
return true; //if all readings are good, we have a good position
}
boolean isGood(){
for(int i=0; i<row; i++){
if(comparePosition(i) == true){ //this makes sure we take each row (position) separately
return true; //if current position aligns with any good position, it's good.
}
}
return false;
}
void setColor(int red, int blue, int green){
analogWrite(rPin, red);
analogWrite(bPin, blue);
analogWrite(gPin, green);
}
void checkSensorValues(){
ss1Val= analogRead(sSensor1);
ss2Val= analogRead(sSensor2);
bs1Val= analogRead(bSensor1);
bs2Val= analogRead(bSensor2);
bs3Val= (analogRead(bSensor3)-558)*2; //problem with sensor...
bs4Val= (analogRead(bSensor4)-558)*2; //problem with sensor...
}
void printSensorValues(){
Serial.print("Sensor 1= ");
Serial.println(ss1Val);
Serial.print("Sensor 2= ");
Serial.println(ss2Val);
Serial.print("Sensor 3= ");
Serial.println(bs1Val);
Serial.print("Sensor 4= ");
Serial.println(bs2Val);
Serial.print("Sensor 5= ");
Serial.println(bs3Val);
Serial.print("Sensor 6= ");
Serial.println(bs4Val);
}
/*void updateButton(){
val= digitalRead(BUTTON);
if(val==HIGH){
state++;
delay(500);
}
if(state>1){
state=0;
}
}*/
void setup() {
// put your setup code here, to run once:
pinMode(rPin, OUTPUT);
pinMode(bPin, OUTPUT);
pinMode(gPin, OUTPUT);
/*pinMode(bSensor3, INPUT);
pinMode(bSensor4, INPUT);*/
pinMode(SWITCH, INPUT);
// pinMode(BUTTON, INPUT);
Serial.begin(9600);
for (int a = 0; a < col; a++){
goodPositions[0][a]=20;
} //saves first good position
for (int b = 0; b < col; b++){
goodPositions[1][b]=400; //all are pressed
}
} //saves second good position
void loop() {
val = digitalRead(SWITCH); //update switch value each loop
checkSensorValues(); //update sensor readings each loop
printSensorValues();
currPosition[0] = ss1Val;
currPosition[1] = ss2Val;
currPosition[2] = bs1Val;
currPosition[3] = bs2Val;
currPosition[4] = bs3Val;
currPosition[5] = bs4Val; //updates the currPosition array each loop
if(val==HIGH){ //if the switch is on...
if(isGood()){ //check if the current position is good
setColor(0, 0, 255); //if yes, green light
}
else{
setColor(255, 0, 0); //if not, red light
}
}
else{
setColor(0, 0, 0); //if the switch isn't on, no light
}
delay(500); //this is just to make sensor readings on serial monitor not so fast
}