Thursday, January 13, 2011

It matters to choose BYTE or INTEGER, BUG fixed + Handshaked protocol added

Just now fixed a bug in my code. Choosing BYTE as my variable type made some confusions on startup for the program as it couldn't initialize the variable with 0 value. The problem was fixed as soon as I changed the type to INTEGER. This resulted in errors in my program as no value made some random cases to launch which was totally wrong. This means that the bug on startup of my program which I talked to my supervisor today about is fixed :-) I was thinking as it just happened on startup to be a noise or some wrong reading from ports on Arduino, but it seems to be just a confusion on defining the variable as byte !!! I have also changed the startup value from 0 to -1 which result in no possible meaningful combination ...

Differences with previous code : Handshake added, Startup bug fixed, Some changes in standard writing to serial port from Arduino side.


//the code is entering saving with Java and we are printing as little as possible in Serial ports
// 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;
int cyclecounter[] = {0, 0, 0};
int cyclecounterold[] = {0, 0, 0};

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



void setup(){
  
  Serial.begin(115200);
  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] = -1;
    cyclecounterold[i] = -1;
  }//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 'h' : //handshake
        Serial.println("h");
        break;
      case '0' ://motor #0
        int temp__;
        temp__ = 0;
        if(Serial.available() < 0)
          ;
        delay(500);
        temp__ = Serial.read();
        if (temp__ == 102){// -f -> forward
          Serial.println("*** Right forward ***");
          digitalWrite(pinDir[0], LOW);
          digitalWrite(pinPwm[0], HIGH);
        }//end of if
        else if(temp__ == 98){// -b -> backward
          Serial.println("*** Right backward ***");
          digitalWrite(pinDir[0], HIGH);
          digitalWrite(pinPwm[0], HIGH);
        }
        else{
         
          Serial.print("error ");
          Serial.println(temp__);
        }
        f = false;
        b = false;
        l = false;
        r = true;
        break;
      case '1' ://motor #1
        int temp_;
        if(Serial.available() < 1)
          ;
          delay(500);
          temp_ = Serial.read();
        if (temp_ == 102){// -f
          Serial.println("*1FW*");
          digitalWrite(pinDir[1], LOW);
          digitalWrite(pinPwm[1], HIGH);
        }//end of if
        else if(temp_ == 98){// -b
          Serial.println("*1BW*");
          digitalWrite(pinDir[1], HIGH);
          digitalWrite(pinPwm[1], HIGH);
        }
        else{
         
          Serial.print("error ");
          Serial.println(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.println("*2FW*");
          digitalWrite(pinDir[2], LOW);
          digitalWrite(pinPwm[2], HIGH);
        }//end of if
        else if(temp_ == 98){// -b
          Serial.println("*2BW*");
          digitalWrite(pinDir[2], HIGH);
          digitalWrite(pinPwm[2], HIGH);
        }
        else{
         
          Serial.print("error ");
          Serial.println(temp_);
        }        
        f = false;
        b = false;
        l = true;
        r = false;
        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("C");
          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;
    
      switch (cyclecounter[i]){
        case (0) : {
           if ( cyclecounterold[i] == 0){
              ;//Do nothing
           }
           else if ( cyclecounterold[i] == 1){
             cycle[i]++;
              Serial.print("C");
              Serial.print(i);
              Serial.print("-");            
              Serial.println(cycle[i]);
           }
           else if ( cyclecounterold[i] == 2){
                Serial.println("Error reading from Encoder-");
            }
           else if ( cyclecounterold[i] == 3){
              cycle[i]--;
              Serial.print("C");
              Serial.print(i);
              Serial.print("-");            
              Serial.println(cycle[i]);
            }
            else 
                ;//Serial.print("ERROR");
            break;
        }//end of case-0
        case (1) : {
            if ( cyclecounterold[i] == 0){
              cycle[i]++;              
              Serial.print("C");
              Serial.print(i);
              Serial.print("-");            
              Serial.println(cycle[i]);
            }
            if ( cyclecounterold[i] == 2){
              cycle[i]--;
              Serial.print("C");
              Serial.print(i);
              Serial.print("-");            
              Serial.println(cycle[i]);            
            }
            else if ( cyclecounterold[i] == 3){
                Serial.println("Error reading from Encoder--");
            }
            else 
                ;//Serial.print("ERROR");
            break;
        }
        case (2) : {
            if ( cyclecounterold[i] == 1){
              cycle[i]--;
              Serial.print("C");
              Serial.print(i);
              Serial.print("-");            
              Serial.println(cycle[i]);
            } 
            else if ( cyclecounterold[i] == 2)
              ;
            else if ( cyclecounterold[i] == 3){
              cycle[i]++;
              Serial.print("C");
              Serial.print(i);
              Serial.print("-");            
              Serial.println(cycle[i]);
            }
            else if ( cyclecounterold[i] == 0){
                Serial.println("Error reading from Encoder---");
            }
            else 
                ;//Serial.print("ERROR");
            break;
        }//end of case-2
        case(3) : {
            if (cyclecounterold[i] == 0){
              cycle[i]--;             
              Serial.print("C");
              Serial.print(i);
              Serial.print("-");
              Serial.println(cycle[i]);
            }
            else if (cyclecounterold[i] == 1){
              Serial.println("Error reading from Encoder----");
            }
            else if ( cyclecounterold[i] == 2){
              cycle[i]++;              
              Serial.print("C");
              Serial.print(i);
              Serial.print("-");            
              Serial.println(cycle[i]);
            }
            else if ( cyclecounterold[i] == 3)
              ;
            else 
              ;//Serial.print("ERROR");
            break;
        }//end of case-3
      }
      
      cyclecounterold[i] = cyclecounter[i];
      
  }//end of for

}//end of checkEncoder()

No comments:

Post a Comment