Encoders & Cycles 2, All DC motors are monitored now -> All_Encoders.pde

This code can monitor the changes in cycles of all the DC motors used in the robot feet.

// Encoders inputs
byte pinEncA[] = {7,5,3}; // digital inputs
byte pinEncB[] = {6,4,2}; // digital inputs

// MOTOR OUTPUTS
byte pinDir[] = {13,8,12}; // digital output, controls direction for motor nr. [x]
byte pinPwm[] = {10,11,9}; // analog output, controls motor speed, nr. [x]

//Global variables
int readValue;
int pwmValue;
int motorDir;
//int channelA = 2;
//int channelB = 3;
int cycle[] = {0, 0, 0};

boolean f = false, b = false, r = false, l = false;
byte cyclecounter[] = {0, 0, 0};
byte cyclecounterold[] = {0, 0, 0};

//13 , 10 right, down
//8 , 11 middle, top
//9, 12 left, down



void setup(){
  
  Serial.begin(9600);
  Serial.println("WELCOME TO C1S0");  
  for (int i=0; i < 3; i++){
    pinMode(pinDir[i], OUTPUT);
    pinMode(pinPwm[i], OUTPUT);
    //in case, just to be sure ...
    digitalWrite(pinPwm[i], LOW);
    // just in case to be sure
    digitalWrite(pinDir[i], LOW);
    pinMode(pinEncA[i], INPUT);
    pinMode(pinEncB[i], INPUT);
    digitalWrite(pinEncA[i], LOW);
    digitalWrite(pinEncB[i], LOW);
    cyclecounter[i] = 0;
    cyclecounterold[i] = 0;
  }//end of for
  
}//end of setup()

void loop(){
    commandCenter();
    checkEncoder();
}//end of loop

void commandCenter(){
  //variables
  int temp = 0;
  if ( Serial.available() > 0 ){
    readValue = Serial.read();
    switch (readValue){
      case 102 ://f -> forward
        Serial.println("*** Forward ***");
        digitalWrite(pinDir[1], LOW);
        digitalWrite(pinPwm[1], HIGH);
        f = true;
        b = false;
        l = false;
        r = false;
        break;
      case 98 ://b -> back
        Serial.println("*** Backward ***");
        digitalWrite(pinDir[1], HIGH);
        digitalWrite(pinPwm[1], HIGH);
        f = false;
        b = true;
        l = false;
        r = false;        
        break;
      case 108 ://l -> left
        int temp_;
        temp_ = 0;
        if(Serial.available() < 1)
          ;
          delay(5);
          temp_ = Serial.read();
        if (temp_ == 102){// -f
          Serial.println("*** Left forward ***");
          digitalWrite(pinDir[2], LOW);
          digitalWrite(pinPwm[2], HIGH);
        }//end of if
        else if(temp_ == 98){// -b
          Serial.println("*** Left backward ***");
          digitalWrite(pinDir[2], HIGH);
          digitalWrite(pinPwm[2], HIGH);
        }
        else Serial.println("*** NOTHING ***");
        f = false;
        b = false;
        l = true;
        r = false;
        break;
      case 114 ://r -> right
        int temp__;
        temp__ = 0;
        if(Serial.available() < 0)
          ;
        delay(5);
        temp__ = Serial.read();
        if (temp__ == 102){// -f
          Serial.println("*** Right forward ***");
          digitalWrite(pinDir[0], LOW);
          digitalWrite(pinPwm[0], HIGH);
        }//end of if
        else if(temp__ == 98){// -b
          Serial.println("*** Right backward ***");
          digitalWrite(pinDir[0], HIGH);
          digitalWrite(pinPwm[0], HIGH);
        }
        f = false;
        b = false;
        l = false;
        r = true;
        break;
      case 82 : //R->RESET
        Serial.println("*** Resetting ***");
        for (int i=0; i < 3; i++){
          digitalWrite(pinPwm[i], LOW);//Setting all PWM's to LOW
          Serial.print("*** Cycle[");
          Serial.print(i);
          Serial.print("] = ");
          Serial.println(cycle[i]);          
        }//end of for
        f = false;
        b = false;
        l = false;
        r = false;
        break;
    }//end of switch  
  } 
  /*
  if (f || b || r || l){
    Serial.print("cycle[i]: ");
    Serial.println(cycle[i]);
  }//end of if
  */
}//end of commandCenter

//int angleMaalt;

void checkEncoder(){
  int A;
  int B;
  int i;
  for (i = 0; i < 3; i++){
      A = digitalRead(pinEncA[i]);
      B = digitalRead(pinEncB[i]);
    
      if ( (A==LOW) && (B==LOW) )
        cyclecounter[i] = 0;
      else if ( (A==LOW) && (B==HIGH) )
        cyclecounter[i] = 1;
      else if ( (A==HIGH) && (B==HIGH) )
        cyclecounter[i] = 2;
      else if ( (A==HIGH) && (B==LOW) )
        cyclecounter[i] = 3;
    
      if (cyclecounter[i] == 0){
        Serial.print("CAME IN AS 0");
         if ( cyclecounterold[i] == 0){
            Serial.print("Case old is 0");
         }
         else if ( cyclecounterold[i] == 3){
            //angleMaalt++;
            Serial.println(cycle[i]);            
            Serial.print("*** Changed cycle[");
            Serial.print(i);
            Serial.print("] = ");
            cycle[i]--;
            Serial.println(cycle[i]);
            Serial.println("++++++");            
            Serial.print("cyccleCounter is : ");
            Serial.println(cyclecounter[i]);
            Serial.print("cyccleCounterOld is : ");              
            Serial.println(cyclecounterold[i]);
          }
          else if ( cyclecounterold[i] == 2){
              Serial.println("Error reading from Encoder-");
              Serial.println("cyclecounter[i]");
              Serial.println(cyclecounter[i]);
              Serial.println("cyclecounterold[i]");
              Serial.println(cyclecounterold[i]);
          }
          else Serial.print("CRAP");
      }
      else if (cyclecounter[i] == 1){
          /*if ( cyclecounterold[i] == 2)
            angleMaalt--;
          else if ( cyclecounterold[i] == 0)
            angleMaalt++;
          else*/ if ( cyclecounterold[i] == 3){
              Serial.println("Error reading from Encoder--");
              Serial.println("cyclecounter[i]");
              Serial.println(cyclecounter[i]);
              Serial.println("cyclecounterold[i]");
              Serial.println(cyclecounterold[i]);
          }
      }
      else if (cyclecounter[i] == 2){
          /*if ( cyclecounterold[i] == 3)
            angleMaalt--;
          else if ( cyclecounterold[i] == 1)
            angleMaalt++;
          else*/ if ( cyclecounterold[i] == 0){
              Serial.println("Error reading from Encoder---");
              Serial.println("cyclecounter[i]");
              Serial.println(cyclecounter[i]);
              Serial.println("cyclecounterold[i]");
              Serial.println(cyclecounterold[i]);
          }
      }
      else if (cyclecounter[i] == 3){       
          if (cyclecounterold[i] == 0){
            //angleMaalt--;
            Serial.println(cycle[i]);            
            Serial.print("*** Changed cycle[");
            Serial.print(i);
            Serial.print("] = ");
            cycle[i]++;
            Serial.println(cycle[i]);
            Serial.print("++++++");            
            Serial.print("cyccleCounter is : ");
            Serial.println(cyclecounter[i]);
            Serial.print("cyccleCounterOld is : ");              
            Serial.println(cyclecounterold[i]);
          }
          /*else if ( cyclecounterold[i] == 2)
            angleMaalt++;*/
          else if ( cyclecounterold[i] == 1){
              Serial.println("Error reading from Encoder----");
              Serial.println("cyclecounter[i]");
              Serial.println(cyclecounter[i]);
              Serial.println("cyclecounterold[i]");
              Serial.println(cyclecounterold[i]);
          }
      }

    
      cyclecounterold[i] = cyclecounter[i];
  }//end of for

}//end of checkEncoder()