Zero positioning, EEPROM implementation

Today I am planning to finish this one, Will come up here with code.

04:34, 11.03.2011 : I am totally finished with current positioning & Zero Positioning of the robot. I can start to call Walloid a smart system ... Even in case of loosing power, or sudden improper shutdown of program, Walloid will still be able to recover and send commands to the motors without ANY crashing fear. Very effective logging system is another specification of current system.

Java CP

Logging systems which logs everything with time in details(Translating Error codes which exist in my own defined Arduino-Java Protocol)

Arduino code :

/*
*Differences with previous code : 
* 1 - It stops when reaching negative values which result in preventing it to crash with the motor
* 2 - It stops when reaching MAX values which result in preventing the feet to go loose
* 3 - Error protocol ready
* 4 - Current positioning & Zero positioning in place :-)
*/

#include EEPROM.h


byte pinEncA[] = {3,5,7}; // digital inputs
byte pinEncB[] = {2,4,6}; // 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;
int cyclecounter[] = {-1, -1, -1};
int cyclecounterold[] = {-1, -1, -1};

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



void setup(){
  
  Serial.begin(115200);
  //Serial.print("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] = -1;
    //cyclecounterold[i] = -1;
  }//end of for
  //findCurrentPosition();

}//end of setup()

void loop(){
    commandCenter();
    checkEncoder();
}//end of loop
int value;
void commandCenter(){
  //variables
  int temp = 0;
  if ( Serial.available() > 0 ){
    readValue = Serial.read();
  //delay(50);
    switch (readValue){
      case 'h' : //handshake
        Serial.print("h");
        delay(50);
        break;
      case 'P' : //handshake
        findCurrentPosition();
        delay(50);
        break;        
      case '0' ://motor #0
        int temp__;
        temp__ = 0;
        if(Serial.available() < 0)
          ;
        delay(500);
        temp__ = Serial.read();
        if (temp__ == 102){// -f -> forward
          //Serial.print("*** Right forward ***");
          digitalWrite(pinDir[0], LOW);
          digitalWrite(pinPwm[0], HIGH);
        }//end of if
        else if(temp__ == 98){// -b -> backward
          //Serial.print("*** Right backward ***");
          digitalWrite(pinDir[0], HIGH);
          digitalWrite(pinPwm[0], HIGH);
        }
        else{
          Serial.println("E00");
          //Serial.print(temp__);
        }
        f = false;
        b = false;
        l = true;
        r = false;
        break;
      case '1' ://motor #1
        int temp_;
        if(Serial.available() < 1)
          ;
          delay(500);
          temp_ = Serial.read();
        if (temp_ == 102){// -f
          //Serial.print("*1FW*");
          digitalWrite(pinDir[1], LOW);
          digitalWrite(pinPwm[1], HIGH);
        }//end of if
        else if(temp_ == 98){// -b
          //Serial.print("*1BW*");
          digitalWrite(pinDir[1], HIGH);
          digitalWrite(pinPwm[1], HIGH);
        }
        else{
         
          Serial.println("E00");
          //Serial.print(temp_);
        }
        f = true;
        b = false;
        l = false;
        r = false;
        break;
      case '2' ://l -> left
        temp_ = 0;
        if(Serial.available() < 1)
          ;
          delay(500);
          temp_ = Serial.read();
        if (temp_ == 102){//motor #2
          //Serial.print("*2FW*");
          digitalWrite(pinDir[2], LOW);
          digitalWrite(pinPwm[2], HIGH);
        }//end of if
        else if(temp_ == 98){// -b
          //Serial.print("*2BW*");
          digitalWrite(pinDir[2], HIGH);
          digitalWrite(pinPwm[2], HIGH);
        }
        else{
          Serial.println("E00");
          //Serial.print(temp_);
        }        
        f = false;
        b = false;
        l = false;
        r = true;
        break;
      case 82 : //R->RESET
        Serial.println("R");
        resetAll();
        f = false;
        b = false;
        l = false;
        r = false;
        break;
    }//end of switch  
  } 
  /*
  if (f || b || r || l){
    Serial.print("cycle[i]: ");
    convertToThreeDigits(cycle[i]);
  }//end of if
  */
}//end of commandCenter

//int angleMaalt;

void checkEncoder(){
  int A;
  int B;
  int i;
  for (i = 0; i < 3; i++){
      cyclecounterold[i] = cyclecounter[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;   
    
      switch (cyclecounter[i]){
        case (0) : {
           if ( cyclecounterold[i] == 0){
              ;//Do nothing
           }
           else if ( cyclecounterold[i] == 1){
             if (cycle[i] + 1 > 213){
                 criticalSituation(i, 1);
             }
             else{
                cycle[i]++;
                saveCurrentPosition();
                Serial.print("C");
                Serial.print(i);
                Serial.print("*");            
                convertToThreeDigits(cycle[i]);
             }
           }
           else if ( cyclecounterold[i] == 2){
                Serial.println("E01");//Error reading from Encoder-
            }
           else if ( cyclecounterold[i] == 3){
             if (cycle[i] -1 < 0){
                 criticalSituation(i, 0);
             }
             else{
                cycle[i]--;
                saveCurrentPosition();
                Serial.print("C");
                Serial.print(i);
                Serial.print("*");            
                convertToThreeDigits(cycle[i]);
             }
            }
            else 
                ;//Serial.print("ERROR");
            break;
        }//end of case-0
        case (1) : {
            if ( cyclecounterold[i] == 0){
               if (cycle[i] -1 < 0){
                 criticalSituation(i, 0);
             }
             else{
              cycle[i]--;
              saveCurrentPosition();
              Serial.print("C");
              Serial.print(i);
              Serial.print("*");            
              convertToThreeDigits(cycle[i]);            
             }
            }
            if ( cyclecounterold[i] == 2){
             if (cycle[i] + 1 > 213){
                 criticalSituation(i, 1);
             }
             else{
                cycle[i]++;
                saveCurrentPosition();
                Serial.print("C");
                Serial.print(i);
                Serial.print("*");            
                convertToThreeDigits(cycle[i]);
             }
            }
            else if ( cyclecounterold[i] == 3){
                Serial.println("E01");//Error reading from Encoder-
            }
            else 
                ;//Serial.print("ERROR");
            break;
        }
        case (2) : {
            if ( cyclecounterold[i] == 1){
             if (cycle[i] -1 < 0){
                 criticalSituation(i, 0);
             }
             else{              
              cycle[i]--;
              saveCurrentPosition();
              Serial.print("C");
              Serial.print(i);
              Serial.print("*");            
              convertToThreeDigits(cycle[i]);
             }
            } 
            else if ( cyclecounterold[i] == 2)
              ;
            else if ( cyclecounterold[i] == 3){
             if (cycle[i] + 1 > 213){
                 criticalSituation(i, 1);
             }
             else{
                cycle[i]++;
                saveCurrentPosition();
                Serial.print("C");
                Serial.print(i);
                Serial.print("*");            
                convertToThreeDigits(cycle[i]);
             }
            }
            else if ( cyclecounterold[i] == 0){
                Serial.println("E01");//Error reading from Encoder-
            }
            else 
                ;//Serial.print("ERROR");
            break;
        }//end of case-2
        case(3) : {
            if (cyclecounterold[i] == 0){
             if (cycle[i] + 1 > 213){
                 criticalSituation(i, 1);
             }
             else{
                cycle[i]++;
                saveCurrentPosition();
                Serial.print("C");
                Serial.print(i);
                Serial.print("*");            
                convertToThreeDigits(cycle[i]);
             }
            }
            else if (cyclecounterold[i] == 1){
              Serial.println("E01");//Error reading from Encoder-
            }
            else if ( cyclecounterold[i] == 2){
             if (cycle[i] -1 < 0){
                 criticalSituation(i, 0);
             }
             else{              
              cycle[i]--;
              saveCurrentPosition();
              Serial.print("C");
              Serial.print(i);
              Serial.print("*");            
              convertToThreeDigits(cycle[i]);
             }
            }
            else if ( cyclecounterold[i] == 3)
              ;
            else 
              ;//Serial.print("ERROR");
            break;
        }//end of case-3
      }
  }//end of for
}//end of checkEncoder()

int criticalSituation(int motorNumber, int type){
      digitalWrite(pinPwm[motorNumber], LOW);//Setting all PWM's to LOW
      if (type == 0)
        Serial.println("E02");//*** FATAL ERROR, CRASH POSSIBLE, STOPPING MOTOR ***
      else if (type == 1)
        Serial.println("E03");//*** FATAL ERROR, ROBOT ARM MIGHT GO LOOSE, STOPPING MOTOR ***
      //Serial.print(motorNumber);
      return 0;
}//end of criticalSituation

int resetAll(){
  for (int i=0; i < 3; i++){
          digitalWrite(pinPwm[i], LOW);//Setting all PWM's to LOW
          Serial.print("C");
          Serial.print(i);
          Serial.print("=");
          convertToThreeDigits(cycle[i]);
        }//end of for
        return 0;
}//end of resetAll()


void convertToThreeDigits(int input){
  if (input >= 0 && input < 10){
    /*switch (input){
      case 0 :
        Serial.print("00");
        break;
      case 1 :
        Serial.print("01");
        break;      
      case 2 :
        Serial.print("02");
        break;      
      case 3 :
        Serial.print("03");
        break;      
      case 4 :
        Serial.print("04");
        break;      
      case 5 :
        Serial.print("05");
        break;      
      case 6 :
        Serial.print("06");
        break;      
      case 7 :
        Serial.print("07");
        break;      
      case 8 :
        Serial.print("08");
        break;      
      case 9 :
        Serial.print("09");
        break;      
      default :
        Serial.print("00");
        break;      
    }//end of switch
    */
    Serial.print("00");
    Serial.print(input);
  }
  else if ( input >= 10 && input < 100){
    Serial.print("0");
    Serial.print(input);
  }
  else{
    Serial.print(input);    
  }
}//end of convertToThreeDigits()

void saveCurrentPosition(){
  for (int i = 0; i < 3; i++){
    EEPROM.write(i, cycle[i]);
  }  
}//end of saveCurrentPosition

void findCurrentPosition(){
  for (int i = 0; i < 3; i++){
    value = EEPROM.read(i);
    cycle[i] = value;
    Serial.print("P");
    Serial.print(i);
    Serial.print("*");
    convertToThreeDigits(value);
  }
}//end of findCurrentPosition