v7

int rPin = 4; 
int bPin = 2;
int gPin = 6; //pin numbers for each LED
int lSensor1 = 3;
int lSensor2 = 5;
int sSensor1 = A0; 
int sSensor2 = A1; 
int bSensor1 = A2;
int bSensor2 = A3;
int bSensor3 = A4;
int bSensor4 = A5; //pin numbers for each pressure sensor
int SWITCH = 7; //pin number for switch
int vib1 = 8;
int vib2 = 9;
int vib3 = 10;
int vib4 = 11;
int vib5 = 12;
int vib6 = 13; //pin number for vibrating disks
//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;
int ls1Val = 0;
int ls2Val = 0; //reads corresponding sensor value
//int state = 0; 
int goodPositions[3][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 = 3; //#rows
int col = 6; //#cols in 2D array of good positions

boolean comparePosition(int r){
  if(currPosition[0]>400 && currPosition[1]>400 && currPosition[2]<20 && currPosition[3]<20 && currPosition[4]<20 && currPosition[5]<20 && (ls1Val==ls2Val)){
    return true; 
  }
  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)-720)*3; 
  bs4Val= (analogRead(bSensor4)-720)*3; 
  ls1Val= digitalRead(lSensor1);
  ls2Val= digitalRead(lSensor2); 
}

void printSensorValues(){
    Serial.print("s1= ");
    Serial.println(ss1Val);
    Serial.print("s2= "); 
    Serial.println(ss2Val); 
    Serial.print("b1= ");
    Serial.println(bs1Val);
    Serial.print("b2= "); 
    Serial.println(bs2Val); 
    Serial.print("b3= ");
    Serial.println(bs3Val);
    Serial.print("b4= "); 
    Serial.println(bs4Val); 
    Serial.print("l1= ");
    Serial.println(ls1Val);
    Serial.print("l2= "); 
    Serial.println(ls2Val); 
}

void turnOffVib(){
  digitalWrite(vib1, LOW);
  digitalWrite(vib2, LOW);
  digitalWrite(vib3, LOW);
  digitalWrite(vib4, LOW);
  digitalWrite(vib5, LOW);
  digitalWrite(vib6, LOW);
}

void vibrate(){
  for(int i=0; i < col; i++){
    if(currPosition[i]>400){
      digitalWrite(8+i, HIGH);
    }
  }
}

/*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(vib1, OUTPUT);
  pinMode(vib2, OUTPUT);
  pinMode(vib3, OUTPUT);
  pinMode(vib4, OUTPUT);
  pinMode(vib5, OUTPUT);
  pinMode(vib6, OUTPUT);
  /*pinMode(bSensor3, INPUT);
  pinMode(bSensor4, INPUT);*/
  pinMode(SWITCH, INPUT);
 // pinMode(BUTTON, INPUT);
  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 

  goodPositions[2][0] = 400;
  goodPositions[2][1] = 400;
  goodPositions[2][2] = 20;
  goodPositions[2][3] = 20;
  goodPositions[2][4] = 20;
  goodPositions[2][5] = 20; //saves third good position
  turnOffVib();
  Serial.begin(9600);
}  

void loop() {
  
  val = digitalRead(SWITCH); //update switch value each loop
  /*if(val == HIGH){
    Serial.println("switch: on");
  }
  else{
    Serial.println("switch: off");
  }*/
  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...
    checkSensorValues(); //update sensor readings each loop
    printSensorValues();
      if(isGood()){ //check if the current position is good
        setColor(0, 0, 255); //if yes, green light
        turnOffVib();
      }
      else{
          setColor(255, 0, 0); //if not, red light
          vibrate();
      }
  }
  else{
    setColor(0, 0, 0); //if the switch isn't on, no light
    turnOffVib();
  }
  delay(500); //this is just to make sensor readings on serial monitor not so fast
}