AS5040, AS5045, and interface code

For the AS5040 and AS5045 rotary encoder.

AS5040, AS5045, and interface code

Postby john » Mon Dec 13, 2010 11:14 pm

Image

We have fully assembled and tested 10bit and 12bit boards for $19.99 each, also N35H diametric magnets (120C/248F operating temperature), soon to be stocked N35EH diametric magnets (200C/392F) here: http://www.madscientisthut.com/Shopping/agora.cgi?product=CNC%20/%20Robotic%20Sensors

I've gotten an AS5040 10-bit magnetic rotary encoder, and an AS5045 12-bit, working, checking all the non-programmed interfaces. Both can be one-time programmed to output different code schemes, but they're not very useful for what I need so I'm not messing with them.

In both cases I piggybacked the chip/breakout board off an Arduino, using the 3.3v supply. I rather randomly chose to use pins 2, 6, and 7 of the Arduino for the digital input: 6 is a chip select that also signals the start of an interface round, 7 acts as the clock from the Arduino to the chip, and 2 acts as the data transfer from the chip to the Arduino. These are trivially remapped.
The code includes a debug setting that'll print out any faults the AS504x notices, but if you turn off the debug setting you'll just get degrees out. It reads from the chip and transmits to the host computer roughly once a second. If anyone wants I've also written a sketch using delaymicroseconds() to get info about 10,000 times faster than that, which I'd be glad to post.
Here's the Arduino sketch for the 5045:

const int ledPin = 13; // LED connected to digital pin 13, used as a heartbeat
const int clockPin = 7; // output to clock
const int CSnPin = 6; // output to chip select
const int inputPin = 2; // read AS5045

int inputstream = 0; // one bit read from pin
long packeddata = 0; // two bytes concatenated from inputstream
long angle = 0; // holds processed angle value
long anglemask = 262080; // 0x111111111111000000: mask to obtain first 12 digits with position info
long statusmask = 63; // 0x000000000000111111; mask to obtain last 6 digits containing status info
long statusbits; // holds status/error information
int DECn; // bit holding decreasing magnet field error data
int INCn; // bit holding increasing magnet field error data
int OCF; // bit holding startup-valid bit
int COF; // bit holding cordic DSP processing error data
int LIN; // bit holding magnet field displacement error data
int debug = 1; // SET THIS TO 0 TO DISABLE PRINTING OF ERROR CODES

void setup()
{
Serial.begin(9600);
pinMode(ledPin, OUTPUT); // visual signal of I/O to chip: heartbeat
pinMode(clockPin, OUTPUT); // SCK
pinMode(CSnPin, OUTPUT); // CSn -- has to toggle high and low to signal chip to start data transfer
pinMode(inputPin, INPUT); // SDA
}

void loop()
{
// CSn needs to cycle from high to low to initiate transfer. Then clock cycles. As it goes high
// again, data will appear on sda
digitalWrite(CSnPin, HIGH); // CSn high
digitalWrite(clockPin, HIGH); // CLK high
delay(1000); // wait for 1 second for no particular reason
digitalWrite(ledPin, HIGH); // signal start of transfer with LED
digitalWrite(CSnPin, LOW); // CSn low: start of transfer
delay(100); // delay for chip -- 1000x as long as it needs to be
digitalWrite(clockPin, LOW); // CLK goes low: start clocking
delay(10); // hold low for 10 ms
for (int x=0; x < 18; x++) // clock signal, 18 transitions, output to clock pin
{
digitalWrite(clockPin, HIGH); // clock goes high
delay(10); // wait 10ms
inputstream = digitalRead(inputPin); // read one bit of data from pin
//Serial.print(inputstream, DEC); // useful if you want to see the actual bits
packeddata = ((packeddata << 1) + inputstream); // left-shift summing variable, add pin value
digitalWrite(clockPin, LOW);
delay(10); // end of one clock cycle
} // end of entire clock cycle
//Serial.println(" ");
digitalWrite(ledPin, LOW); // signal end of transmission
// lots of diagnostics for verifying bitwise operations
//Serial.print("packed:");
//Serial.println(packeddata,DEC);
//Serial.print("pack bin: ");
// Serial.println(packeddata,BIN);
angle = packeddata & anglemask; // mask rightmost 6 digits of packeddata to zero, into angle.
//Serial.print("mask: ");
//Serial.println(anglemask, BIN);
//Serial.print("bin angle:");
//Serial.println(angle, BIN);
//Serial.print("angle: ");
//Serial.println(angle, DEC);
angle = (angle >> 6); // shift 18-digit angle right 6 digits to form 12-digit value
//Serial.print("angleshft:");
//Serial.println(angle, BIN);
//Serial.print("angledec: ");
//Serial.println(angle, DEC);
angle = angle * 0.08789; // angle * (360/4096) == actual degrees
Serial.print("angle: "); // and, finally, print it.
Serial.println(angle, DEC);
//Serial.println("--------------------");
//Serial.print("raw: "); // this was the prefix for the bit-by-bit diag output inside the loop.
if (debug)
{
statusbits = packeddata & statusmask;
DECn = statusbits & 2; // goes high if magnet moved away from IC
INCn = statusbits & 4; // goes high if magnet moved towards IC
LIN = statusbits & 8; // goes high for linearity alarm
COF = statusbits & 16; // goes high for cordic overflow: data invalid
OCF = statusbits & 32; // this is 1 when the chip startup is finished.
if (DECn && INCn) { Serial.println("magnet moved out of range"); }
else
{
if (DECn) { Serial.println("magnet moved away from chip"); }
if (INCn) { Serial.println("magnet moved towards chip"); }
}
if (LIN) { Serial.println("linearity alarm: magnet misaligned? Data questionable."); }
if (COF) { Serial.println("cordic overflow: magnet misaligned? Data invalid."); }
}

packeddata = 0; // reset both variables to zero so they don't just accumulate
angle = 0;
}
Last edited by john on Mon Dec 13, 2010 11:52 pm, edited 1 time in total.
john
Site Admin
 
Posts: 6
Joined: Tue Sep 21, 2010 9:20 am

Re: AS5040, AS5045, and interface code

Postby john » Mon Dec 13, 2010 11:46 pm

Here's the AS5040 code -- pretty much the same, with some changes in variable declarations and some math changes that reflect the 5040's 16-bit word vs. the 5045's 18-bit word.
I'm trying out some formatting and color games, so I hope this code snippet looks better than the last one.

const int ledPin = 13; //LED connected to digital pin 13
const int clockPin = 7; //output to clock
const int CSnPin = 6; //output to chip select
const int inputPin = 2; //read AS5040

int inputstream = 0; //one bit read from pin
long packeddata = 0; //two bytes concatenated from inputstream
long angle = 0; //holds processed angle value
long anglemask = 65472; //0x1111111111000000: mask to obtain first 10 digits with position info
long statusmask = 63; //0x000000000111111; mask to obtain last 6 digits containing status info
long statusbits; //holds status/error information
int DECn; //bit holding decreasing magnet field error data
int INCn; //bit holding increasing magnet field error data
int OCF; //bit holding startup-valid bit
int COF; //bit holding cordic DSP processing error data
int LIN; //bit holding magnet field displacement error data
int debug = 1; //SET THIS TO 0 TO DISABLE PRINTING OF ERROR CODES

void setup()
{
Serial.begin(9600);
pinMode(ledPin, OUTPUT); // visual signal of I/O to chip
pinMode(clockPin, OUTPUT); // SCK
pinMode(CSnPin, OUTPUT); // CSn -- has to toggle high and low to signal chip to start data transfer
pinMode(inputPin, INPUT); // SDA
}

void loop()
{
// CSn needs to cycle from high to low to initiate transfer. Then clock cycles. As it goes high
// again, data will appear on sda
digitalWrite(CSnPin, HIGH); // CSn high
digitalWrite(clockPin, HIGH); // CLK high
delay(1000);// wait for 1 second for no particular reason
digitalWrite(ledPin, HIGH); // signal start of transfer with LED
digitalWrite(CSnPin, LOW); // CSn low: start of transfer
delay(100); // delay for chip -- 1000x as long as it needs to be
digitalWrite(clockPin, LOW); // CLK goes low: start clocking
delay(10); // hold low for 10 ms
for (int x=0; x <16; x++) // clock signal, 16 transitions, output to clock pin
{
digitalWrite(clockPin, HIGH); //clock goes high
delay(10); // wait 10ms
inputstream =digitalRead(inputPin); // read one bit of data from pin
//Serial.print(inputstream, DEC);
packeddata = ((packeddata << 1) + inputstream);// left-shift summing variable, add pin value
digitalWrite(clockPin, LOW);
delay(10); // end of one clock cycle
}
// end of entire clock cycle
//Serial.println(" ");
digitalWrite(ledPin, LOW); // signal end of transmission
// lots of diagnostics for verifying bitwise operations
//Serial.print("packed:");
//Serial.println(packeddata,DEC);
//Serial.print("pack bin: ");
// Serial.println(packeddata,BIN);
angle = packeddata & anglemask; // mask rightmost 6 digits of packeddata to zero, into angle.
//Serial.print("mask: ");
//Serial.println(anglemask, BIN);
//Serial.print("bin angle:");
//Serial.println(angle, BIN);
//Serial.print("angle: ");
//Serial.println(angle, DEC);
angle = (angle >> 6); // shift 16-digit angle right 6 digits to form 10-digit value
//Serial.print("angleshft:");
//Serial.println(angle, BIN);
//Serial.print("angledec: ");
//Serial.println(angle, DEC);
angle = angle * 0.3515; // angle * (360/1024) == actual degrees
Serial.print("angle: "); // and, finally, print it.
Serial.println(angle, DEC);
//Serial.println("--------------------");
//Serial.print("raw: "); // this was the prefix for the bit-by-bit diag output inside the loop.
if (debug)
{
statusbits = packeddata & statusmask;
DECn = statusbits & 2; // goes high if magnet moved away from IC
INCn = statusbits & 4; // goes high if magnet moved towards IC
LIN = statusbits & 8; // goes high for linearity alarm
COF = statusbits & 16; // goes high for cordic overflow: data invalid
OCF = statusbits & 32; // this is 1 when the chip startup is finished.
if (DECn && INCn) { Serial.println("magnet moved out of range"); }
else
{
if (DECn) { Serial.println("magnet moved away from chip"); }
if (INCn) { Serial.println("magnet moved towards chip"); }
}
if (LIN) { Serial.println("linearity alarm: magnet misaligned? Data questionable."); }
if (COF) { Serial.println("cordic overflow: magnet misaligned? Data invalid."); }
}

packeddata = 0; // reset both variables to zero so they don't just accumulate
angle = 0;
}

Last edited by john on Mon Dec 13, 2010 11:53 pm, edited 1 time in total.
john
Site Admin
 
Posts: 6
Joined: Tue Sep 21, 2010 9:20 am

Re: AS5040, AS5045, and interface code

Postby john » Mon Dec 13, 2010 11:51 pm

And, finally, here's a link to a video of me trying the AS5040 board out:http://www.youtube.com/watch?v=gXewDdlUT_Q. It shows the analog PWM-out signal, the A/B quadrature output, and the results of the digital output dumped to a terminal. It works fine on both HyperTerm and minicom as long as they're set to 9600 8N1.
john
Site Admin
 
Posts: 6
Joined: Tue Sep 21, 2010 9:20 am

Re: AS5040, AS5045, and interface code

Postby gregcotten » Tue Oct 18, 2011 2:49 pm

john wrote:If anyone wants I've also written a sketch using delaymicroseconds() to get info about 10,000 times faster than that, which I'd be glad to post.


I'd love to see that if you can post it!
gregcotten
 
Posts: 3
Joined: Tue Oct 11, 2011 9:37 pm

Re: AS5040, AS5045 fast interface

Postby john » Wed Oct 19, 2011 11:27 pm

Here's a version that's a lot faster on updates. I pretty much just changed all the short delays to microseconds and left the longer delays as milliseconds. This will report 90 samples a second or thereabouts. If you want to play with making it faster, just start decreasing longdelay (but probably not less than 1) and shortdelay (but probably not less than 40, because of the Arduino's clock rate.)
Note that because of some issues with word length, the AS5040 works both straight and in debug mode (where it displays errors like the magnet being too far away) while the AS5045 works fine when it's not in debug mode but in debug mode gives the right angle and the right error codes but detects an error with every reading, even if there isn't actually an error. I haven't gotten around to fixing that, since it does work in normal mode.
This is the version for the 5040. If you want to use the 5045 instead you need to change the anglemask value, make x go to < 18, and a couple other minor changes that should be obvious from the previously posted '45 code.


const int ledPin = 13; //LED connected to digital pin 13
const int clockPin = 7; //output to clock
const int CSnPin = 6; //output to chip select
const int inputPin = 2; //read AS5040

int inputstream = 0; //one bit read from pin
long packeddata = 0; //two bytes concatenated from inputstream
long angle = 0; //holds processed angle value
long anglemask = 65472; //0x1111111111000000: mask to obtain first 10 digits with position info
long statusmask = 63; //0x000000000111111; mask to obtain last 6 digits containing status info
long statusbits; //holds status/error information
int DECn; //bit holding decreasing magnet field error data
int INCn; //bit holding increasing magnet field error data
int OCF; //bit holding startup-valid bit
int COF; //bit holding cordic DSP processing error data
int LIN; //bit holding magnet field displacement error data
int debug = 0; //SET THIS TO 0 TO DISABLE PRINTING OF ERROR CODES
int shortdelay = 100; // this is the microseconds of delay in the data clock
int longdelay = 10; // this is the milliseconds between readings

void setup()
{
  Serial.begin(9600);
  pinMode(ledPin, OUTPUT); // visual signal of I/O to chip
  pinMode(clockPin, OUTPUT); // SCK
  pinMode(CSnPin, OUTPUT); // CSn -- has to toggle high and low to signal chip to start data transfer
  pinMode(inputPin, INPUT); // SDA
}

void loop()
{
// CSn needs to cycle from high to low to initiate transfer. Then clock cycles. As it goes high
// again, data will appear on sda
  digitalWrite(CSnPin, HIGH); // CSn high
  digitalWrite(clockPin, HIGH); // CLK high
  delay(longdelay);// time between readings
  digitalWrite(ledPin, HIGH); // signal start of transfer with LED
  digitalWrite(CSnPin, LOW); // CSn low: start of transfer
  delayMicroseconds(shortdelay); // delay for chip initialization
  digitalWrite(clockPin, LOW); // CLK goes low: start clocking
  delayMicroseconds(shortdelay); // hold low
  for (int x=0; x <16; x++) // clock signal, 16 transitions, output to clock pin
  {
    digitalWrite(clockPin, HIGH); //clock goes high
    delayMicroseconds(shortdelay); //
    inputstream =digitalRead(inputPin); // read one bit of data from pin
//Serial.print(inputstream, DEC);
    packeddata = ((packeddata << 1) + inputstream);// left-shift summing variable, add pin value
    digitalWrite(clockPin, LOW);
    delayMicroseconds(shortdelay); // end of one clock cycle
  }
// end of entire clock cycle
//Serial.println(" ");
  digitalWrite(ledPin, LOW); // signal end of transmission
// lots of diagnostics for verifying bitwise operations
//Serial.print("packed:");
//Serial.println(packeddata,DEC);
//Serial.print("pack bin: ");
//Serial.println(packeddata,BIN);
  angle = packeddata & anglemask; // mask rightmost 6 digits of packeddata to zero, into angle.
//Serial.print("mask: ");
//Serial.println(anglemask, BIN);
//Serial.print("bin angle:");
//Serial.println(angle, BIN);
//Serial.print("angle: ");
//Serial.println(angle, DEC);
  angle = (angle >> 6); // shift 16-digit angle right 6 digits to form 10-digit value
//Serial.print("angleshft:");
//Serial.println(angle, BIN);
//Serial.print("angledec: ");
//Serial.println(angle, DEC);
  angle = angle * 0.3515; // angle * (360/1024) == actual degrees
  Serial.print("angle: "); // and, finally, print it.
  Serial.println(angle, DEC);
//Serial.println("--------------------");
//Serial.print("raw: "); // this was the prefix for the bit-by-bit diag output inside the loop.
  if (debug)
  {
    statusbits = packeddata & statusmask;
    DECn = statusbits & 2; // goes high if magnet moved away from IC
    INCn = statusbits & 4; // goes high if magnet moved towards IC
    LIN = statusbits & 8; // goes high for linearity alarm
    COF = statusbits & 16; // goes high for cordic overflow: data invalid
    OCF = statusbits & 32; // this is 1 when the chip startup is finished.
    if (DECn && INCn) { Serial.println("magnet moved out of range"); }
    else
    {
      if (DECn) { Serial.println("magnet moved away from chip"); }
      if (INCn) { Serial.println("magnet moved towards chip"); }
    }
    if (LIN) { Serial.println("linearity alarm: magnet misaligned? Data questionable."); }
    if (COF) { Serial.println("cordic overflow: magnet misaligned? Data invalid."); }
  }

  packeddata = 0; // reset both variables to zero so they don't just accumulate
  angle = 0;
}

john
Site Admin
 
Posts: 6
Joined: Tue Sep 21, 2010 9:20 am

Re: AS5040, AS5045, and interface code

Postby gregcotten » Thu Oct 20, 2011 9:22 am

Thanks John!

I'm going to do some experimenting to see if I can make the delay between the 18-bit readings even shorter. Initially, before you posted this code, I tried removing the delays between bit readings but I suppose that might sometimes re-read the same information (although the numbers seemed correct?). I'm trying to cram readings from 6 devices every 2 milliseconds, with a little time to spare to do some PID calculations. I'm open to daisy-chaining 2 sets of 3 encoders together. That being said, have you looked into the digitalWriteFast library? (http://code.google.com/p/digitalwritefast/) It supposedly speeds up digital reading and writing significantly if you know the pin assignments at compile time. I would appreciate your input on this as it would be nice to know if I'm expecting to do too much in too little time (2 milliseconds)!

Also, with the '45 have you experienced much "bounce"? I'm currently in the process of building some sort of LEGO thing like you did to keep things steady but if I just sit the magnet on top of the device and let the readings happen there seems to be a lot of variance every sample (+/- 15 around some center point).

Thanks,
Greg
gregcotten
 
Posts: 3
Joined: Tue Oct 11, 2011 9:37 pm

Re: AS5040, AS5045, and interface code

Postby john » Sat Oct 22, 2011 11:26 pm

I'll look at the fast write library. I was under the impression that the code I posted was getting close to the limits of the Arduino's clock speed, but I'm nothing like an expert at this.

As for bounce -- I've had varied experiences. The board is mostly output stuff; the only component that affects the chip stability is the capacitor between the 3.3v pin and ground. (and I use an X7R-rated cap in that location.) I can't claim the board design is perfect, but I can't see how anything other than that cap could result in a problem with readings. That makes me think that problems are either in the chip, or external noise of some type. (Though it's not clear how a chip that detects magnetic fields is going to get confused by noise.) It's also possible there's some weird timing issue with my code and the Arduino's clock frequency, so every once in a while it drops a bit. Do you see glitches if you run the original one-sample-a-second code for a couple hours? If I had a bit more time it'd be useful to cross-compile the code into AVR assembly and run that, to cancel out any problems stemming from the compiler in the guts of the Arduino IDE.
john
Site Admin
 
Posts: 6
Joined: Tue Sep 21, 2010 9:20 am

Re: AS5040, AS5045, and interface code

Postby gregcotten » Sun Oct 30, 2011 6:01 pm

John,

Using the digitalWriteFast library I was able to set shortDelay to 2 microseconds for everything (I removed the longDelay delay from the code and replaced it with delayMicroseconds(shortDelay). When I get the chance I will try 1 microseconds. I haven't had any problems thus far. I am using the encoder to modify an existing standard servo for a motion control application. The readings on the '45 have been great (I exchanged the one I was having trouble with) and I have a PID control loop going that keeps the rotation of the motor in any position I specify. I'll post a video of it once I have the other servos modified.

-Greg
gregcotten
 
Posts: 3
Joined: Tue Oct 11, 2011 9:37 pm

Re: AS5040, AS5045, and interface code

Postby Mohit Jindal » Tue Aug 28, 2012 6:00 am

Hi John, I hope you are still active here. I want to use AS5045 with arduino uno 3. Can you please tell me how to connect as5045 with uno 3 ? I want to know which pins go where ?
Also i saw the video. You are movin encoder forward and backword. I want to rotate the motor shaft to get its position/angle. Please tell me what changes i have to make in your code ? Thanks. :mrgreen: :roll: :?
Mohit Jindal
 
Posts: 1
Joined: Sun Aug 26, 2012 7:25 am

Re: AS5040, AS5045, and interface code

Postby lsbueno » Thu Oct 11, 2012 6:08 am

Here's some c/avr-gcc code for the atmega32, based on the original arduino code above. You'll need to use the uart lib that you can get from here

Code: Select all
/*
 * For Atmega32. Tested with external crystal osc at 8MHz
 *
 */
#define FALSE 0
#define TRUE 1
#define DEBUG TRUE
#define UART_BAUD_RATE      9600   
#define ENCODER_PORT_SETUP DDRC
#define ENCODER_PORT_IN PINC
#define ENCODER_PORT_OU PORTC
#define ENCODER_CHIP_SELECT PC0
#define ENCODER_CLOCK PC1
#define ENCODER_RX PC2
//Minimum is 100ns. We will wait 1 us
#define ENCODER_T_CLK_FE_DELAY 1
#define ENCODER_T_CLK_PULSE 1
#define DELAY_BETWEEN_READS 3000
#define encoder_pin_low(ENCODER_PIN) ENCODER_PORT_OU &= ~(1 << ENCODER_PIN)
#define encoder_pin_high(ENCODER_PIN) ENCODER_PORT_OU |= (1 << ENCODER_PIN)
#define encoder_read_pin() (ENCODER_PORT_IN & (1 << ENCODER_RX)) ? 1 : 0
#define get_angle(encoder_value) (encoder_value >> 6) * 360 / 4096

#define CSN_HIGH encoder_pin_high(ENCODER_CHIP_SELECT)
#define CSN_LOW encoder_pin_low(ENCODER_CHIP_SELECT)

#define CLOCK_HIGH encoder_pin_high(ENCODER_CLOCK)
#define CLOCK_LOW encoder_pin_low(ENCODER_CLOCK)

#define CLOCK_PAUSE _delay_us(ENCODER_T_CLK_PULSE)


#include <avr/io.h>
#include <stdio.h>
#include <avr/interrupt.h>
#include <util/atomic.h>
#include <avr/common.h>
#include <util/delay.h>

#include "uart.h"

static int uart_putchar(unsigned char c, FILE *stream) {
    if (c == '\n')
        uart_putc('\r');
    uart_putc(c);
    return 0;
}

FILE uart_output = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE);

void encoder_init(void) {
    ENCODER_PORT_SETUP = (1 << ENCODER_CLOCK) | (1 << ENCODER_CHIP_SELECT);
}

uint32_t encoder_read(void) {
    uint32_t value = 0;
    uint8_t pin_value = 0;

    CSN_HIGH;
    CLOCK_HIGH;

    CLOCK_PAUSE;

    //CSN low initiates read-out
    CSN_LOW;
    CLOCK_PAUSE;

    //On clock low data is latched into output shift register
    CLOCK_LOW;
    CLOCK_PAUSE;

    for (uint8_t x = 0; x < 18; x++) {
        // Every clock high shifts one bit of data
        CLOCK_HIGH;
        CLOCK_PAUSE;
        pin_value = encoder_read_pin();
        value = ((value << 1) + pin_value);
        CLOCK_LOW;
        CLOCK_PAUSE;
    }

    return value;
}

//Required first_bit > last_bit
uint8_t calc_parity(uint32_t encoder_value, uint8_t first_bit, uint8_t last_bit) {
    if (first_bit < last_bit){
        return 0xFF;
    }
    uint8_t xor_val = (encoder_value >> first_bit) & 1;
    for (uint8_t i = 1; i < first_bit - last_bit; i++) {
        xor_val ^= (((encoder_value >> (first_bit - i)) & 1) ? 1 : 0);
    }
    return xor_val;
}

void debug(uint32_t encoder_value) {
    printf("raw: ");
    for (uint8_t i = 0; i < 32; i++) {
        printf("%ld", (encoder_value >> (31 - i)) & 0x01);
    }
    printf("\n");
    //status is at the 6 rightmost bits
    //OCF, COF, LIN, MAG+, MAG-, PARITY
    uint8_t PARITY = encoder_value & 1; // goes high if magnet moved away from IC
    uint8_t DECn = encoder_value & (1 << 1); // goes high if magnet moved away from IC
    uint8_t INCn = encoder_value & (1 << 2); // goes high if magnet moved towards IC
    uint8_t LIN = encoder_value & (1 << 3); // goes high for linearity alarm
    uint8_t COF = encoder_value & (1 << 4); // goes high for cordic overflow: data invalid
    uint8_t OCF = encoder_value & (1 << 5); // this is 1 when the chip startup is finished.
    if (DECn && INCn) {
        printf("magnet moved out of range\n");
    } else {
        if (DECn) {
            printf("magnet moved away from chip\n");
        }
        if (INCn) {
            printf("magnet moved towards chip\n");
        }
    }
    if (LIN) {
        printf("linearity alarm: magnet misaligned? Data questionable.\n");
    }
    if (COF) {
        printf("cordic overflow: magnet misaligned? Data invalid.\n");
    }
    if (!OCF) {
        printf("OCF compensation algorithm didnt finished! Data not valid\n");
    }
    if (calc_parity(encoder_value, 17,1) != PARITY) {
        printf("parity bit invalid\n");
    }
}

/* Main function */
int main(void) {

    uint32_t encoder_value = 0;
    uint16_t angle = 0;

    //led heartbeat
    DDRB |= (1 << PB0);
    PORTB |= (1 << PB0);

    uart_init(UART_BAUD_SELECT(UART_BAUD_RATE, F_CPU));

    sei(); //Enable global interrupts

    stdout = &uart_output;

    encoder_init();
       
    printf("System initialized\n");

    printf("HAL Encoder Running\n");

    while (TRUE) {
        _delay_ms(100);
        PORTB |= (1 << PB0);
        encoder_value = encoder_read();
        angle = get_angle(encoder_value);

        printf("Angle: %d\n", angle);
       
        if (DEBUG) {
            debug(encoder_value);
        }
        _delay_ms(DELAY_BETWEEN_READS);
        PORTB &= ~(1 << PB0);
    }
    return 0;
}
lsbueno
 
Posts: 1
Joined: Wed Oct 10, 2012 8:54 pm

Next

Return to 10/12Bit Rotary Encoder

Who is online

Users browsing this forum: Google [Bot] and 1 guest

cron