Skip to content

Commit

Permalink
Fixed the stupid doesn't work issues
Browse files Browse the repository at this point in the history 
Messed up commit history, but the code works!
  • Loading branch information
@tekdemo
tekdemo committed Jun 17, 2014
1 parent bc28f5b commit 2704b6f
Show file tree
Hide file tree
Showing 2 changed files with 71 additions and 134 deletions.
155 changes: 53 additions & 102 deletions AS5050.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,19 +63,17 @@ AS5050::AS5050(byte pin, byte spi_speed){
//pull pin mode low to assert slave
//digitalWrite(_pin,LOW);

//Reboot the chip, in case there were issues during various transmissions
write(REG_MASTER_RESET,0x0);


//Prepare the chip
write(REG_MASTER_RESET,0x0); //do a full reset in case the chip glitched in the last power cycle
//Read angle twice to initialize chip and get to a known good state
//Reading once won't work, as _last_angle will be set incorrectly
angle();
_init_angle=angle();
//angle() will glitch on startup if it's >768, reset it
rotations=0;

//Set to True to reverse the angle returned by the library
mirror=false;
//By default, we don't want the angle to be reversed
mirrored=true;
};

unsigned int AS5050::send(unsigned int reg_a){
Expand All @@ -93,7 +91,6 @@ unsigned int AS5050::send(unsigned int reg_a){

digitalWrite(_pin,HIGH); //End Transaction
SPI.end();

return response.value;
};

Expand Down Expand Up @@ -137,111 +134,66 @@ unsigned int AS5050::write(unsigned int reg,unsigned int data){
data=send(REG_NOP); //Get confirmation from chip

//save error and parity data
error.transaction=data & (RES_ERROR_FLAG); //save error data
error.parity=__builtin_parity(data&(~RES_PARITY)) != (data&RES_PARITY);
error.transaction|=error.parity;
error.transaction|=data&RES_ERROR_FLAG;
error.parity|=__builtin_parity(data&(~RES_PARITY)) != (data&RES_PARITY); //save parity errors
error.transaction=error.parity; //save error data
error.transaction|=reg&RES_ERROR_FLAG;


return data; //remove parity and EF bits and return data.
};

int AS5050::raw_angle(){
//This function strips out the error and parity
//data in the data frame, and handles the errors
unsigned int data;
int angle;

data=read(REG_ANGLE);
/* Response from chip is this:
14 | 13 | 12 ... 2 | 1 | 0
AH | AL | <data 10bit > | EF | PAR
*/

//Parity data is generated in read() function

//TODO this needs some work to avoid magic numbers
angle=(data&0x3FFE)>>2; //strip away alarm bits, then parity and error flags

//Allow for the user to mirror the rotation, in case the angle seems backwards
if(mirror)angle=AS5050_ANGULAR_RESOLUTION-angle;


#if AS5050_AUTO_ERROR_HANDLING
switch(data&(RES_ALARM_HIGH|RES_ALARM_LOW)){
case RES_ALARM_HIGH: //gain too high, decrease gain
gain=read(REG_GAIN_CONTROL); //get information about current gain
write(REG_GAIN_CONTROL,--gain); //increment gain and send it back
break;
case RES_ALARM_LOW: //gain too low, increase gain
gain=read(REG_GAIN_CONTROL); //get information about current gain
write(REG_GAIN_CONTROL,++gain); //increment gain and send it back
break;
default:handleErrors();
break;
}
#endif

return angle;
}



int AS5050::angle(){
unsigned int anglesum=0;
int angle;
int last_sample=_last_angle;
int _rotations=rotations; //work with a local copy to make function re-entrant when called from interrupts


/*BUG : near the zerog crossing, one of the samples will jump to 1024
The resulting data looks like this:
1024
1024
5
1020
which obviously has a REALLY bad average of 768. Need to scan for
differences of 512 or more and then promote or demote the value, or
take care of rollovers within each scan
*/
//TODO until the above bug is fixed, remove loop
for(byte i=0;i<NUM_ANGLE_SAMPLES; i++){

angle=raw_angle();

//We need to make sure that there's no parity errors, or else our angle will be corrupted.
if( error.parity ){
//undo the latest read, and use the last known good data
angle=last_sample;
}

//keep up our running sum
anglesum+=angle;
//This function strips out the error and parity
//data in the data frame, and handles the errors
unsigned int data=read(REG_ANGLE);

/* Response from chip is this:
14 | 13 | 12 ... 2 | 1 | 0
AH | AL | <data 10bit > | EF | PAR
*/

//save error data to the error register
//Check parity of transaction
//Save the parity error for analysis
error.parity=__builtin_parity(data&(~RES_PARITY)) != (data&RES_PARITY);
error.transaction|=error.parity;
//Save angular errors
error.transaction=(data|RES_ALARM_HIGH|RES_ALARM_LOW);



//Automatically handle errors if we've enabled it
#if AS5050_AUTO_ERROR_HANDLING==1
if(error.transaction){
handleErrors();
//If there's a parity error, the angle might be invalid so prevent glitching by swapping in the last angle
if(error.transaction&RES_PARITY) return _last_angle;
}
#endif

//TODO this needs some work to avoid magic numbers
int angle=(data&0x3FFE)>>2; //strip away alarm bits, then parity and error flags

//track rollovers for continous angle monitoring
if(last_sample>768 && angle<=256)_rotations+=1;
else if(last_sample<256 && angle>=768)_rotations-=1;

//preserve the good data for future possible errors
last_sample=angle;
//Allow the user to reverse the logical rotation
if(mirrored){angle=(AS5050_ANGULAR_RESOLUTION-1)-angle;}

//track rollovers for continous angle monitoring
if(_last_angle>768 && angle<=256)rotations+=1;
else if(_last_angle<256 && angle>=768)rotations-=1;
_last_angle=angle;


//If we did encounter errors, we need to correct them before the next sample.
if(error.transaction){ handleErrors(); }
return angle;

//TODO make sure that this can run continuously without exceeding chip hold times
//delayMicroseconds(10);
}
int AS5050::angle(byte nsamples){
int sum=0;
for(byte i=0;i<nsamples;i++){
sum+=angle();
}

//average samples and find true angle
angle=(anglesum+NUM_ANGLE_SAMPLES/2)/NUM_ANGLE_SAMPLES; //the anglesum+numsamples/2 performs fair rounding

//Update the globals
rotations=_rotations;
_last_angle=angle;
return angle;
//this performs fair rounding on arbitrary integers
return (sum+nsamples/2)/nsamples;
}


float AS5050::angleDegrees(){
//Rewrite of arduino's map function, to make sure we don't lose resolution
//return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
Expand Down Expand Up @@ -281,7 +233,6 @@ void AS5050::setHome(){
_init_angle=0;
}


unsigned int AS5050::handleErrors(){ //now, handle errors:
error.status=read(REG_ERROR_STATUS);

Expand Down
50 changes: 18 additions & 32 deletions AS5050.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,15 +49,6 @@
#define AS5050_RESET_ON_ERRORS 1
#endif

//Number of averaged samples for a given angle
//You really want this to be a multiple of 2 so the compiler can optimize it
#ifndef NUM_ANGLE_SAMPLES
#define NUM_ANGLE_SAMPLES 4
#endif




//Command values for reading and writing
#define AS_WRITE (0x0000)
#define AS_READ (0x8000)
Expand All @@ -67,7 +58,7 @@
Shifted 1 to make room for parity bit
OR with AS_READ/AS_WRITE before sending command
*/
#define REG_POWER_ON_RESET (0x3F22) //write 0x5A to this to deactivate power on reset
#define REG_POWER_ON_RESET (0x3F22)
#define REG_SOFTWARE_RESET (0x3C00)
#define REG_MASTER_RESET (0x33A5)
#define REG_CLEAR_ERROR (0x3380)/*Read to reset errors*/
Expand All @@ -90,23 +81,20 @@ OR with AS_READ/AS_WRITE before sending command
#define RES_ERROR_ANGLE (RES_PARITY| RES_ERROR_FLAG|RES_ALARM_HIGH|RES_ALARM_LOW)

//Error Status Register
#define ERR_PARITY (1<<0)
#define ERR_CLKMON (1<<1)
#define ERR_ADDMON (1<<2)
//#define RES_ERR_RESERVED3 (1<<3)
#define ERR_WOW (1<<4)
#define ERR_MODE (1<<5)
//#define RES_ERR_RESERVED6 (1<<6)
//#define RES_ERR_RESERVED7 (1<<7)
#define ERR_DACOV (1<<8) // DAC overflow
#define ERR_DSPOV (1<<9) //DSP PRocessor overflow
#define ERR_RANERR (1<<10) //Range error
#define ERR_DSPALO (1<<11) //DSP Alarm low
#define ERR_DSPAHI (1<<12) //DSP Alarm High
//#define RES_ERR_RESERVED13 (1<<13)



#define ERR_PARITY (1<<13)
#define ERR_CLKMON (1<<12)
#define ERR_ADDMON (1<<11)
//#define RES_ERR_RESERVED3 (1<<10)
#define ERR_WOW (1<<9)
#define ERR_MODE (1<<8)
//#define RES_ERR_RESERVED6 (1<<7)
//#define RES_ERR_RESERVED7 (1<<6)
#define ERR_DACOV (1<<5) // DAC overflow
#define ERR_DSPOV (1<<4) //DSP PRocessor overflow
#define ERR_RANERR (1<<3) //Range error
#define ERR_DSPALO (1<<2) //DSP Alarm low
#define ERR_DSPAHI (1<<1) //DSP Alarm High
//#define RES_ERR_RESERVED13 (1<<0)

//Set various limits on some values
#define AS5050_ANGULAR_RESOLUTION 1024
Expand Down Expand Up @@ -136,8 +124,8 @@ class AS5050{
unsigned int write(unsigned int,unsigned int);
unsigned int handleErrors();
//These functions return the physical angle on the chip
int raw_angle(); //returns raw angle direct from chip
int angle(); //returns sampled averages and counts rollovers
int angle();
int angle(byte nsamples);
float angleDegrees();
float angleRad();
//These functions return the absolute angle from initial startup
Expand Down Expand Up @@ -166,14 +154,12 @@ class AS5050{

//Keep track of how many full rotations we've gone through
int rotations;
bool mirrored;

//Set to true to reverse the angles returned by this library
bool mirror;
private:
byte _pin;
int _last_angle;
int _init_angle;

};

#endif

0 comments on commit 2704b6f

Please sign in to comment.