Testing code from Jeff to keep track of encoder

encoder_track.cpp

/*

Program to track the absolute position of an AMS_AS5048B encoder since CPU reset even though the encoder rolls over the 14bit register 0-16348 that is built in.
In other words, the encoder can keep spinning and this program keeps track of the total rotations/distance travelled or however you want to represent
knowing the absolute position.  The value of "ticks" will be stored in "rotational_position".

*/
#include <Wire.h>

//unit consts
#define AS5048B_ANGLMSB_REG 0xFE //bits 0..7
#define AS5048B_RESOLUTION 16384.0 //14 bits
#define AS5048_ADDRESS 0x40

// Code to read a 16-bit register, deal with rollover, accumulate total distance.
uint16_t current_position;
uint16_t last_position;
int16_t position_movement;
int32_t rotational_position;

uint16_t bitshift_cur_pos;
uint16_t bitshift_last_pos;
int16_t bitshift_pos_delta;

unsigned long prev_time_poll_AS5048B =  0;
const int poll_AS5048B_Interval = 100; // 1000/10=100=10 Hz
const int infoInterval = 3000;
const int resetInterval = 300000;  // 5 minutes = 5 X 60K/minute
unsigned long prev_time_stamp_info =  0;
unsigned long prev_time_stamp_reset = 0;


uint16_t AMS_AS5048B_readReg16() {  //reference: https://github.com/sosandroid/AMS_AS5048B
	byte requestResult;
	byte readArray[2];
	uint16_t readValue = 0;
	Wire.beginTransmission(AS5048_ADDRESS);
	Wire.write(AS5048B_ANGLMSB_REG);
	requestResult = Wire.endTransmission(false);
	if (requestResult){
		Serial.print("I2C error: ");
		Serial.println(requestResult);
	}
	Wire.requestFrom(AS5048_ADDRESS, 2);
	for (byte i=0; i < 2; i++) {
		readArray[i] = Wire.read();
	}

	readValue = (((uint16_t) readArray[0]) << 6);
	readValue += (readArray[1] & 0x3F);
	return readValue;
}

void pub_info() {
	Serial.print("current_position: ");
	Serial.print(current_position);
	Serial.print(" bitshift_pos_delta: ");
	Serial.print(bitshift_pos_delta);
	Serial.print(", position_movement: ");
	Serial.print(position_movement);
	Serial.print(", rotational_position: ");
	Serial.println(rotational_position);
}

void setup() {
	Serial.begin(9600);
	while (!Serial) ; //wait until Serial ready
	Wire.begin();
	current_position = AMS_AS5048B_readReg16();
	last_position = current_position;
}

void loop() {
	if (millis() - prev_time_poll_AS5048B >= poll_AS5048B_Interval) {
		current_position = AMS_AS5048B_readReg16();
		//reference: next 5 lines are based on magic code from Jeff Sampson - thank you!
		bitshift_cur_pos = current_position << 2; //Bitshiftleft - The leftmost 2 bits in current_position are shifted out of existence
		bitshift_last_pos = last_position << 2;
		bitshift_pos_delta = (bitshift_cur_pos - bitshift_last_pos);
		position_movement = bitshift_pos_delta >> 2; //BitshiftRight 2 bits
		rotational_position += position_movement;	// Update the absolute position values. (Position of this encoder since CPU reset.)	
		last_position = current_position;		
		prev_time_poll_AS5048B = millis();
	}

	if (millis() - prev_time_stamp_info >= infoInterval) {  // provide an informational message to ensure all is OK
		pub_info();
		prev_time_stamp_info = millis();
	}

	if (millis() - prev_time_stamp_reset >= resetInterval) {  // reset the cumulative counter in order to support debugging
		current_position = AMS_AS5048B_readReg16();
		last_position = current_position;	
		rotational_position = 0;
        Serial.print("RESETTING - current_position: ");
        Serial.print(current_position);
        Serial.print(", rotational_position: ");
        Serial.println(rotational_position);		
		prev_time_stamp_reset = millis();
	}	
}