Here's the code, and output from the console.
#include <Servo.h> // Use Servo library, included with IDE
#define term 13
Servo myServo; // Create Servo object to control the servo
const uint8_t p8 = 4;
const uint8_t p7 = 12;
const uint8_t p6 = 11;
const uint8_t p5 = 10;
const uint8_t p4 = 9;
const uint8_t p3 = 8;
const uint8_t p2 = 7;
const uint8_t p1 = 6;
// For the list below, the subscript 0-127 is the decimal equivalent from // the encoder. The data elements are the sequence number associated with // the decimal equivalent produced by the encoder for the corresponding // sequence number.
byte gray [128][2]= {{1,56},{2,40},{3,55},{4,24},{6,39},{7,52},{8,8},{9,57},
{12,23},{14,36},{15,13},{16,120},{18,41},{19,54},{23,53},{24,7},
{28,20},{29,19},{30,125},{31,18},{32,104},{33,105},{36,25},{37,106},{38,38},
{41,58},{46,37},{47,14},{48,119},{49,118},{53,107},{56,4},{58,3},{60,109},
{61,108},{62,2},{63,1},{64,88},{66,89},{71,51},{72,9},{73,10},{74,90},
{76,22},{77,11},{79,12},{82,42},{83,43},{92,21},{94,126},{95,127},{96,103},
{98,102},{106,91},{112,116},{113,117},{116,115},{120,93},{121,94},{122,92},
{124,114},{125,95},{126,113},{127,0},{128,72},{129,71},{131,68},{132,73},
{135,29},{137,70},{139,69},{142,35},{143,34},{144,121},{146,122},{148,74},
{151,30},{152,6},{154,123},{158,124},{159,17},{163,67},{164,26},{166,27},
{167,28},{169,59},{175,15},{184,5},{188,110},{190,111},{191,16},{192,87},
{193,84},{195,45},{196,86},{197,85},{199,50},{203,46},{207,33},{209,83},
{211,44},{212,75},{215,31},{223,32},{224,100},{225,61},{226,101},{227,66},
{229,62},{231,49},{232,99},{233,60},{235,47},{239,48},{240,77},{241,82},
{242,78},{243,65},{244,76},{245,63},{247,64},{248,98},{249,81},{250,79},
{251,80},{252,97},{253,96},{254,112}};
//byte comnd[50]={0};
//byte& target = comnd[0];
//byte status[50]={0};
//byte& current = status[0];
int list [100] = {0};
int listptr =0;
int curangle =0;
int difference =0;
int rate =0;
int dir = 0;
int graycur =0;
int target =7;
int current;
int stop=1;
int stringPos =0;
int cntr=0; // check this later on for overflow
//char inString[6];
//int input;
//int c;
//char inByte;
void setup() {
Serial.begin(9600);
myServo.attach(2); // Servo is connected to digital pin 2
pinMode(p8, INPUT);
pinMode(p7, INPUT);
pinMode(p6, INPUT);
pinMode(p5, INPUT);
pinMode(p4, INPUT);
pinMode(p3, INPUT);
pinMode(p2, INPUT);
pinMode(p1, INPUT);
digitalWrite(p8,HIGH);
digitalWrite(p7,HIGH);
digitalWrite(p6,HIGH);
digitalWrite(p5,HIGH);
digitalWrite(p4,HIGH);
digitalWrite(p3,HIGH);
digitalWrite(p2,HIGH);
digitalWrite(p1,HIGH);
}
void loop() {
if(stop==1) {
target = consoleinput();
if(target==0) goto end;
Serial.print("target = ");
Serial.println(target);
target = target / 2.8125; // convert angle to encoder position 0-126
Serial.println("target = " + String(target));
graycur= GrayInput();
current = gray[graylookup(graycur)][1];
Serial.println("current = " + String(current));
if(abs(target-current)
if(target < current) dir = 1;
else dir = -1;
steer(dir,1);
list[0]= target;
list[1]= current;
listptr=2;
stop=2;
}
while(stop==2) {
cntr++;
graycur= GrayInput();
current = gray[graylookup(graycur)][1];
list[listptr]=current;
listptr++;
difference = target-current;
if((cntr % 10)==0) Serial.println("graybin gray curnt diff target");
Serial.print(graycur,BIN);
Serial.print(" ");
Serial.print(graycur);
Serial.print(" ");
Serial.print(current);
Serial.print(" ");
Serial.print(difference);
Serial.print(" ");
Serial.println(target);
//Serial.println("difference = " + String(difference));
//Serial.println(String(graycur)+" "+String(current)+" "+String(difference)+" "+String(target));
if(abs(difference) < 9 && abs(difference)>2) {
rate = 1;
steer(dir, rate);
}
if(abs(difference) < 6 && current != 0) {
rate = 0;
stop=1;
target=0;
cntr=0;
steer(dir, rate);
//for(int j=0; j=listptr-1; j++) Serial.print(String(list[j])+" ");
//Serial.println(" ");
listptr=0;
}
//Serial.println("dir = " + String(dir)+" rate = "+String(rate));
}
end: delay(1);
}
int consoleinput(){
char inString[6];
int input;
int c;
char inByte;
while(Serial.available()) {
inByte = Serial.read();
Serial.println("inByte = " + String(inByte));
//c = Serial.available();
//Serial.println("serial.available is "+String(c));
if((inByte >= '0') && (inByte <= '9')){
inString[stringPos] = inByte;
stringPos ++;
}
if(inByte == 'x'){
Serial.println(" \r received");
for(c=0; c<stringPos; c++) Serial.print(inString[c]);
Serial.println(" ");
input = atoi(inString); //convert string to int
Serial.println(input);
for ( c = 0; c < stringPos; c++){
inString[c] = 0;
}
stringPos = 0;
return input;
}
}
}
int steer(int dir, int rate) { // 1300 is clockwise, 1500 stop, 1700 counter-clockwise
// dir = 1 cw dir = -1 ccw rate = 0 stop, 1 slow, 2 fast
myServo.writeMicroseconds(1500 +(70* rate*(0 - dir)));
}
// Return the current values of the input pins
byte GrayInput(){
byte lbits = 0;
if ( digitalRead(p8)) bitSet(lbits,7);
if ( digitalRead(p7)) bitSet(lbits,6);
if ( digitalRead(p6)) bitSet(lbits,5);
if ( digitalRead(p5)) bitSet(lbits,4);
if ( digitalRead(p4)) bitSet(lbits,3);
if ( digitalRead(p3)) bitSet(lbits,2);
if ( digitalRead(p2)) bitSet(lbits,1);
if ( digitalRead(p1)) bitSet(lbits,0);
return lbits;
}
int graylookup(int intarg) {
int n=0;
int j=0;
for(int i=1;i<=7;i++) {
j = 1<<(7-i);
if(intarg==gray[(j+n)][0]) goto end;
if(intarg>=gray[(j+n)][0])n = n+j ;
}
end: j=j+n;
return j;
}
and here is the console output
target = 49
target = 17
current = 122
graybin gray curnt diff target
10010010 146 122 -105 17
graybin gray curnt diff target
10010010 146 122 -105 17
graybin gray curnt diff target
10010000 144 121 -104 17
graybin gray curnt diff target
10000 16 120 -103 17
graybin gray curnt diff target
110000 48 119 -102 17
graybin gray curnt diff target
110000 48 119 -102 17
graybin gray curnt diff target
110001 49 118 -101 17
graybin gray curnt diff target
110001 49 118 -101 17
graybin gray curnt diff target
1110001 113 117 -100 17
graybin gray curnt diff target
1110001 113 117 -100 17
graybin gray curnt diff target
1110001 113 117 -100 17many lines deleted here
graybin gray curnt diff target
11101001 233 60 -43 17
graybin gray curnt diff target
11101001 233 60 -43 17
10101001 169 59 -42 17
101001 41 58 -41 17
many lines deleted here
101001 41 58 -41 17
101001 41 58
at this point, a few lines of gibberish - doesn't copy or print here - is seen and the it stops completely.
Sometimes it works perfectly, I have tried to figure out what makes one pass work and another not. This code is about steering a large robot using an absolute encoder with 8bit gray code output to determine the position of the wheel. The actual drive is a gear motor with chain drive, the encoder providing the feedback. In the code you see here, I am using a continuous servo to drive the encoder to make it easier to develop and test the code.
The console input often drops the first and or second digit entered and other times works perfectly.