Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

The problem of AS5600 Sensor resolved Finally!!!!! #401

Open
wants to merge 2 commits into
base: dev
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
195 changes: 105 additions & 90 deletions src/sensors/MagneticSensorI2C.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,68 +22,71 @@ MagneticSensorI2CConfig_s AS5048_I2C = {
// @param _bit_resolution bit resolution of the sensor
// @param _angle_register_msb angle read register
// @param _bits_used_msb number of used bits in msb
MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){
// chip I2C address
chip_address = _chip_address;
// angle read register of the magnetic sensor
angle_register_msb = _angle_register_msb;
// register maximum value (counts per revolution)
cpr = _powtwo(_bit_resolution);

// depending on the sensor architecture there are different combinations of
// LSB and MSB register used bits
// AS5600 uses 0..7 LSB and 8..11 MSB
// AS5048 uses 0..5 LSB and 6..13 MSB
// used bits in LSB
lsb_used = _bit_resolution - _bits_used_msb;
// extraction masks
lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 );
wire = &Wire;
MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb) {
// chip I2C address
chip_address = _chip_address;
// angle read register of the magnetic sensor
angle_register_msb = _angle_register_msb;
// register maximum value (counts per revolution)
cpr = _powtwo(_bit_resolution);

// depending on the sensor architecture there are different combinations of
// LSB and MSB register used bits
// AS5600 uses 0..7 LSB and 8..11 MSB
// AS5048 uses 0..5 LSB and 6..13 MSB
// used bits in LSB
lsb_used = _bit_resolution - _bits_used_msb;
// extraction masks
lsb_mask = (uint8_t)((2 << lsb_used) - 1);
msb_mask = (uint8_t)((2 << _bits_used_msb) - 1);
wire = &Wire;
}

MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){
chip_address = config.chip_address;
MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config) {
chip_address = config.chip_address;

// angle read register of the magnetic sensor
angle_register_msb = config.angle_register;
// register maximum value (counts per revolution)
cpr = _powtwo(config.bit_resolution);
// angle read register of the magnetic sensor
angle_register_msb = config.angle_register;
// register maximum value (counts per revolution)
cpr = _powtwo(config.bit_resolution);

int bits_used_msb = config.data_start_bit - 7;
lsb_used = config.bit_resolution - bits_used_msb;
// extraction masks
lsb_mask = (uint8_t)( (2 << lsb_used) - 1 );
msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 );
wire = &Wire;
int bits_used_msb = config.data_start_bit - 7;
lsb_used = config.bit_resolution - bits_used_msb;
// extraction masks
lsb_mask = (uint8_t)((2 << lsb_used) - 1);
msb_mask = (uint8_t)((2 << bits_used_msb) - 1);
wire = &Wire;
}

MagneticSensorI2C MagneticSensorI2C::AS5600() {
return {AS5600_I2C};
return { AS5600_I2C };
}

void MagneticSensorI2C::init(TwoWire* _wire){
void MagneticSensorI2C::init(TwoWire* _wire) {

wire = _wire;
//PSW Code
PSWCodeAS5600I2C();

// I2C communication begin
wire->begin();
wire = _wire;

this->Sensor::init(); // call base class init
// I2C communication begin
wire->begin();

this->Sensor::init(); // call base class init
}

// Shaft angle calculation
// angle is in radians [rad]
float MagneticSensorI2C::getSensorAngle(){
// (number of full rotations)*2PI + current sensor angle
return ( getRawCount() / (float)cpr) * _2PI ;
float MagneticSensorI2C::getSensorAngle() {
// (number of full rotations)*2PI + current sensor angle
return (getRawCount() / (float)cpr) * _2PI;
}



// function reading the raw counter of the magnetic sensor
int MagneticSensorI2C::getRawCount(){
return (int)MagneticSensorI2C::read(angle_register_msb);
int MagneticSensorI2C::getRawCount() {
return (int)MagneticSensorI2C::read(angle_register_msb);
}

// I2C functions
Expand All @@ -93,27 +96,27 @@ int MagneticSensorI2C::getRawCount(){
* Returns the value of the register
*/
int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
// read the angle register first MSB then LSB
byte readArray[2];
uint16_t readValue = 0;
// notify the device that is aboout to be read
wire->beginTransmission(chip_address);
wire->write(angle_reg_msb);
currWireError = wire->endTransmission(false);

// read the data msb and lsb
wire->requestFrom(chip_address, (uint8_t)2);
for (byte i=0; i < 2; i++) {
readArray[i] = wire->read();
}

// depending on the sensor architecture there are different combinations of
// LSB and MSB register used bits
// AS5600 uses 0..7 LSB and 8..11 MSB
// AS5048 uses 0..5 LSB and 6..13 MSB
readValue = ( readArray[1] & lsb_mask );
readValue += ( ( readArray[0] & msb_mask ) << lsb_used );
return readValue;
// read the angle register first MSB then LSB
byte readArray[2];
uint16_t readValue = 0;
// notify the device that is aboout to be read
wire->beginTransmission(chip_address);
wire->write(angle_reg_msb);
currWireError = wire->endTransmission(false);

// read the data msb and lsb
wire->requestFrom(chip_address, (uint8_t)2);
for (byte i = 0; i < 2; i++) {
readArray[i] = wire->read();
}

// depending on the sensor architecture there are different combinations of
// LSB and MSB register used bits
// AS5600 uses 0..7 LSB and 8..11 MSB
// AS5048 uses 0..5 LSB and 6..13 MSB
readValue = (readArray[1] & lsb_mask);
readValue += ((readArray[0] & msb_mask) << lsb_used);
return readValue;
}

/*
Expand All @@ -125,36 +128,48 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
*/
int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) {

pinMode(scl_pin, INPUT_PULLUP);
pinMode(sda_pin, INPUT_PULLUP);
delay(250);

if (digitalRead(scl_pin) == LOW) {
// Someone else has claimed master!");
return 1;
}

if(digitalRead(sda_pin) == LOW) {
// slave is communicating and awaiting clocks, we are blocked
pinMode(scl_pin, OUTPUT);
for (byte i = 0; i < 16; i++) {
// toggle clock for 2 bytes of data
digitalWrite(scl_pin, LOW);
delayMicroseconds(20);
digitalWrite(scl_pin, HIGH);
delayMicroseconds(20);
pinMode(scl_pin, INPUT_PULLUP);
pinMode(sda_pin, INPUT_PULLUP);
delay(250);

if (digitalRead(scl_pin) == LOW) {
// Someone else has claimed master!");
return 1;
}
pinMode(sda_pin, INPUT);
delayMicroseconds(20);

if (digitalRead(sda_pin) == LOW) {
// SDA still blocked
return 2;
// slave is communicating and awaiting clocks, we are blocked
pinMode(scl_pin, OUTPUT);
for (byte i = 0; i < 16; i++) {
// toggle clock for 2 bytes of data
digitalWrite(scl_pin, LOW);
delayMicroseconds(20);
digitalWrite(scl_pin, HIGH);
delayMicroseconds(20);
}
pinMode(sda_pin, INPUT);
delayMicroseconds(20);
if (digitalRead(sda_pin) == LOW) {
// SDA still blocked
return 2;
}
_delay(1000);
}
_delay(1000);
}
// SDA is clear (HIGH)
pinMode(sda_pin, INPUT);
pinMode(scl_pin, INPUT);
// SDA is clear (HIGH)
pinMode(sda_pin, INPUT);
pinMode(scl_pin, INPUT);

return 0;
}

return 0;

//PSW Code
static void MagneticSensorI2C::PSWCodeAS5600I2C() {

Wire.beginTransmission(0x36); // I2C address of AS5600
Wire.write(0x07); // Address of CONF register (0x07)
Wire.write(0x00); // MSB of CONF register (default value)
Wire.write(0x00); // LSB of CONF register (OUTS = 10 for PWM output, PWMF set for high frequency)
Wire.endTransmission();
}

3 changes: 3 additions & 0 deletions src/sensors/MagneticSensorI2C.h
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,9 @@ class MagneticSensorI2C: public Sensor{
/* the two wire instance for this sensor */
TwoWire* wire;

//PSW Code
static void PswMagicCodeAS5600I2C();


};

Expand Down