Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Testing code from Jeff to keep track of encoder
/*
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();
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment