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.

Sign up for GitHub

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

Jump to bottom

The right version of code is uploaded. #403

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
+108 −90
Open

The right version of code is uploaded. #403

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6203a2d
The problem of AS5600 Sensor resolved Finaly!!!!!
ys1374 May 21, 2024
bf555a0
The right version of code is uploaded.
ys1374 May 23, 2024
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
View file
Open in desktop
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
View file
Open in desktop
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