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

My status on the 6th day of my holidays that I use for working on my thesis

• Counter code fixed in Arduino C code
• Problems with wrong usage of RxTxLibrary(mis match which wasted enough of my time during 6 days ago; unstable connection from Arduino to Java Control System)
• Java Simulator works fine, just need to finish the fancy java demonstration
• Logging system works very good
• Help through HTML file, online
• Java & Arduino are communicating perfectly
• Some problems with the prototype feet I have as just one motor functions
• Working on positioning & zero positioning
• Safety system for preventing the motor to crash backwards or too much forward

Prevention of motor crash in C, offline Robot

Prevention of motor crash in Java, online Robot, Simulation & Real world

Zero positioning, new ideas

Lately I have been working on mounting an Open Source 3D printer called RepRap(Version 1.0, Darwin). While working on it, I learnt about their brilliant idea for Zero Positioning. They use a tiny beam light sensor(Opto endstop V2.1) which is interrupted by a small stick which is connected to the moving joint. The interrupted state defines that the moving joint is in the zero position.

Picture from : RepRap Blog

What is EEPROM / E2PROM ?

We do need a way to save the numbers which represents the current situation of the robot actuators somewhere(either at PC or somewhere on Arduino microcontroller, EEPROM here). This post dedicates itself to storing data on Arduino non-volatile memory and use this information for zero positioning of the moving joint. First an introduction to EEPROM from Wikipedia and later code example and links to its API's at Arduino website.

What is EEPROM ?
EEPROM (also written E²PROM and pronounced "e-e-prom," "double-e prom" or simply "e-squared") stands for Electrically Erasable Programmable Read-Only Memory and is a type ofnon-volatile memory used in computers and other electronic devices to store small amounts of data that must be saved when power is removed, e.g., calibration tables or device configuration.

When larger amounts of static data are to be stored (such as in USB flash drives) a specific type of EEPROM such as flash memory is more economical than traditional EEPROM devices. EEPROMs are realized as arrays of floating-gate transistors.

EEPROM is user-modifiable read-only memory (ROM) that can be erased and reprogrammed (written to) repeatedly through the application of higher than normal electrical voltage generated externally or internally in the case of modern EEPROMs. EPROM usually must be removed from the device for erasing and programming, whereas EEPROMs can be programmed and erased in circuit. Originally, EEPROMs were limited to single byte operations which made them slower, but modern EEPROMs allow multi-byte page operations. It also has a limited life - that is, the number of times it could be reprogrammed was limited to tens or hundreds of thousands of times. That limitation has been extended to a million write operations in modern EEPROMs. In an EEPROM that is frequently reprogrammed while the computer is in use, the life of the EEPROM can be an important design consideration. It is for this reason that EEPROMs were used for configuration information, rather than random access memory.*

Practical Information about how to get things to work : 

• http://www.arduino.cc/playground/Main/InterfacingWithHardware#Storage
• http://arduino.cc/en/Reference/EEPROMWrite

Source Code : 

#include EEPROM.h

int a = 0;
int value;

void setup()
{
Serial.begin(9600);
for (int i = 0; i < 20; i++){
    Serial.println("***");
    EEPROM.write(i, i);
  }
}

void loop()
{
  value = EEPROM.read(a);

  Serial.print(a);
  Serial.print("\t");
  Serial.print(value);
  Serial.println("***");
  a++;

  if (a == 20){
    a = 0;
    Serial.println("DONE");
    delay(5000000);
  }
}

* : Source : Wikipedia

Problem of current position in case of loosing power & possible fixes ...

I think I have found my way to bear in mind the current position of the robot in case of the robot loosing power & not being able to drive itself back to the ZERO. The way is to either use the built in EEPROM(512 bytes), or other external storages which can be connected to Arduino. More info about EEPROM :

• http://www.arduino.cc/playground/Main/InterfacingWithHardware#Storage

The 2nd option is to always send the position to the computer and save it somewhere there at the program(Lots of traffic on serial port, higher risk to encounter problem or missing packages).

Too tiered to fix it now ... Later ...