jschoch/FOCals31313.cpp

## FOCals31313.cpp
#include "FOCals31313.h"
#define kNOERROR 0
#define kDATATOOLONGERROR 1
#define kRECEIVEDNACKONADDRESSERROR 2
#define kRECEIVEDNACKONDATAERROR 3
#define kOTHERERROR 4

int deviceAddress = 96;

MagneticSensorI2CConfig_s ALS31313_I2C_CONFIG = {
  .chip_address = 0x36,
  .bit_resolution = 12,
  .angle_register = 0x0E,
  .data_start_bit = 11
};


void FOCals31313::init(TwoWire* _wire){
  wire = _wire;
  wire->begin(16,17);
  wire->setClock(1000000);
  uint16_t error = write(deviceAddress, 0x24, 0x2C413534);

    if (error != kNOERROR)
    {
        Serial.print("Error while trying to enter customer access mode. error = ");
        Serial.println(error);
    }

    wire->requestFrom(0x02, 4);
    uint32_t data = wire->read() << 24;
    data += wire->read()  << 16;
    data += wire->read()  << 8;
    data += wire->read() ;

    bool a = bitRead(data,20);
    bool b = bitRead(data,19);

    Serial.print("sensor hall mode: ");
    Serial.print(a,BIN);
    Serial.println(b,BIN);

    bool ze = bitRead(data,8);
    bool ye = bitRead(data,7);
    bool xe = bitRead(data,6);

    Serial.print(ze,BIN);
    Serial.print(",");
    Serial.print(ye,BIN);
    Serial.print(",");
    Serial.println(xe,BIN);

    Serial.println("raw config");
    Serial.println(data,BIN);
    //Serial.print(",");
    //readALS31300ADC(deviceAddress);
    angle_prev = 0;
    velocity_calc_timestamp = _micros();
    // full rotations tracking number
    full_rotation_offset = 0;
    angle_data_prev = getRawCount();
    zero_offset = 0;
    cpr = pow(2, ALS31313_I2C_CONFIG.bit_resolution);
    Serial.println(angle_data_prev,6);
  return;
}

uint16_t FOCals31313::write(int busAddress, uint8_t address, uint32_t value)
{
    // Write the address that is to be written to the device
    // and then the 4 bytes of data, MSB first
    wire->beginTransmission(busAddress);
    wire->write(address);
    wire->write((byte)(value >> 24));
    wire->write((byte)(value >> 16));
    wire->write((byte)(value >> 8));
    wire->write((byte)(value));
    return wire->endTransmission();
}

uint16_t FOCals31313::read(int busAddress, uint8_t address, uint32_t& value)
{
    // Write the address that is to be read to the device
    wire->beginTransmission(busAddress);
    wire->write(address);
    int error = wire->endTransmission(false);

    // if the device accepted the address,
    // request 4 bytes from the device
    // and then read them, MSB first
    if (error == kNOERROR)
    {
        wire->requestFrom(busAddress, 4);
        value = wire->read() << 24;
        value += wire->read() << 16;
        value += wire->read() << 8;
        value += wire->read();
    }

    return error;
}

float FOCals31313::getAngle(){
  float angle_data = getRawCount();

  // tracking the number of rotations
  // in order to expand angle range form [0,2PI]
  // to basically infinity
  float d_angle = angle_data - angle_data_prev;
  //Serial.print(d_angle);
  //Serial.print(",");
  // if overflow happened track it as full rotation
  if(abs(d_angle) > (6) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
  // save the current angle value for the next steps
  // in order to know if overflow happened
  angle_data_prev = angle_data;
  //Serial.print(angle_data_prev,6);
  //Serial.print(",");
  // zero offset adding
  angle_data -= (int)zero_offset;
  //Serial.print(angle_data,6);
  //Serial.print(",");
  //Serial.print(full_rotation_offset);
  //Serial.print(",");
  //Serial.print(cpr);
  //Serial.println(" .");
  // return the full angle
  // (number of full rotations)*2PI + current sensor angle
  //return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI);
  return natural_direction * (full_rotation_offset +  angle_data ) ;
}
void FOCals31313::readALS31300ADC_FastLoop(int busAddress)
{

}

void FOCals31313::readALS31300ADC_FullLoop(int busAddress)
{

}

float FOCals31313::getRawCount(){
  uint32_t value0x27;

    // Read the register the I2C loop mode is in
    uint16_t error = read(deviceAddress, 0x27, value0x27);
    if (error != kNOERROR)
    {
        Serial.print("Unable to read the ALS31300. error = ");
        Serial.println(error);
    }

    // I2C loop mode is in bits 2 and 3 so mask them out
    // and set them to the no loop mode
    value0x27 = (value0x27 & 0xFFFFFFF3) | (0x0 << 2);

    // Write the new values to the register the I2C loop mode is in
    error = write(deviceAddress, 0x27, value0x27);
    if (error != kNOERROR)
    {
        Serial.print("Unable to read the ALS31300. error = ");
        Serial.println(error);
    }


    // Write the address that is going to be read from the ALS31300
    Wire.beginTransmission(deviceAddress);
    Wire.write(0x28);
    error = Wire.endTransmission(false);

    // The ALS31300 accepted the address
    if (error == kNOERROR)
    {
        // Start the read and request 8 bytes
        // which are the contents of register 0x28 and 0x29
        Wire.requestFrom(deviceAddress, 8);

        // Read the first 4 bytes which are the contents of register 0x28
        uint32_t value0x28 = Wire.read() << 24;
        value0x28 += Wire.read() << 16;
        value0x28 += Wire.read() << 8;
        value0x28 += Wire.read();

        // Read the next 4 bytes which are the contents of register 0x29
        uint32_t value0x29 = Wire.read() << 24;
        value0x29 += Wire.read() << 16;
        value0x29 += Wire.read() << 8;
        value0x29 += Wire.read();

        // Take the most significant byte of each axis from register 0x28 and combine it with the least
        // significant 4 bits of each axis from register 0x29, then sign extend the 12th bit.
        int x = SignExtendBitfield(((value0x28 >> 20) & 0x0FF0) | ((value0x29 >> 16) & 0x0F), 12);
        int y = SignExtendBitfield(((value0x28 >> 12) & 0x0FF0) | ((value0x29 >> 12) & 0x0F), 12);
        int z = SignExtendBitfield(((value0x28 >> 4) & 0x0FF0) | ((value0x29 >> 8) & 0x0F), 12);

        /* /    This is for the LEATR-500
        // Convert the X, Y and Z values into radians
        float rx = (float)x / 4096.0 * M_TWOPI;
        float ry = (float)y / 4096.0 * M_TWOPI;
        float rz = (float)z / 4096.0 * M_TWOPI;
        */


        // This is for the LEAtR-1000
        //float rx = (float)x / 2048.0 * M_TWOPI;
        //float ry = (float)y / 2048.0 * M_TWOPI;
        //float rz = (float)z / 2048.0 * M_TWOPI;


        /*
        //  This is for the LEATR-JOY
        float rx = (float)x / 1024.0 * M_TWOPI;
        float ry = (float)y / 1024.0 * M_TWOPI;
        float rz = (float)z / 256.0 * M_TWOPI;
        */


        // Use a four quadrant Arc Tan to convert 2
        // axis to an angle (which is in radians) then
        // convert the angle from radians to degrees
        // for display.
        //return atan2f(ry, rx) * 180.0 / M_PI;
        return atan2f(x, y);
        }

}

void FOCals31313::readALS31300ADC(int busAddress)
{
    uint32_t value0x27;

    // Read the register the I2C loop mode is in
    uint16_t error = read(busAddress, 0x27, value0x27);
    if (error != kNOERROR)
    {
        Serial.print("Unable to read the ALS31300. error = ");
        Serial.println(error);
    }

    // I2C loop mode is in bits 2 and 3 so mask them out
    // and set them to the no loop mode
    value0x27 = (value0x27 & 0xFFFFFFF3) | (0x0 << 2);

    // Write the new values to the register the I2C loop mode is in
    error = write(busAddress, 0x27, value0x27);
    if (error != kNOERROR)
    {
        Serial.print("Unable to read the ALS31300. error = ");
        Serial.println(error);
    }

    for (int count = 0; count < 30; ++count)
    {
        // Write the address that is going to be read from the ALS31300
        Wire.beginTransmission(busAddress);
        Wire.write(0x28);
        uint16_t error = Wire.endTransmission(false);

        // The ALS31300 accepted the address
        if (error == kNOERROR)
        {
            // Start the read and request 8 bytes
            // which are the contents of register 0x28 and 0x29
            Wire.requestFrom(busAddress, 8);

            // Read the first 4 bytes which are the contents of register 0x28
            uint32_t value0x28 = Wire.read() << 24;
            value0x28 += Wire.read() << 16;
            value0x28 += Wire.read() << 8;
            value0x28 += Wire.read();

            // Read the next 4 bytes which are the contents of register 0x29
            uint32_t value0x29 = Wire.read() << 24;
            value0x29 += Wire.read() << 16;
            value0x29 += Wire.read() << 8;
            value0x29 += Wire.read();

            // Take the most significant byte of each axis from register 0x28 and combine it with the least
            // significant 4 bits of each axis from register 0x29, then sign extend the 12th bit.
            int x = SignExtendBitfield(((value0x28 >> 20) & 0x0FF0) | ((value0x29 >> 16) & 0x0F), 12);
            int y = SignExtendBitfield(((value0x28 >> 12) & 0x0FF0) | ((value0x29 >> 12) & 0x0F), 12);
            int z = SignExtendBitfield(((value0x28 >> 4) & 0x0FF0) | ((value0x29 >> 8) & 0x0F), 12);


            bool a = bitRead(value0x29,7);
            bool b = bitRead(value0x29,6);
            Serial.print("hall mode: ");
            Serial.print(a,BIN);
            Serial.println(b,BIN);
            // Display the values of x, y and z
            //sprintf(xyzB,"%d x: %d y: %d z: %d", count, x,y,z);
            //sprintf(xyzB,"%d ", count);


            Serial.print(count);
            Serial.print(", ");
            Serial.print(x);
            Serial.print(", ");
            Serial.print(y);
            Serial.print(", ");
            Serial.println(z);

            // Look at the datasheet for the sensitivity of the part used.
            // In this case, full scale range is 500 gauss, other sensitivities
            // are 1000 gauss and 2000 gauss.
            // Sensitivity of 500 gauss = 4.0 lsb/g
            // Sensitivity of 1000 gauss = 2.0 lsb/g
            // Sensitivity of 2000 gauss = 1.0 lsb/g

            float mx = (float)x / 4.0;
            float my = (float)y / 4.0;
            float mz = (float)z / 4.0;

            Serial.print("MX, MY, MZ = ");
            Serial.print(mx);
            Serial.print(", ");
            Serial.print(my);
            Serial.print(", ");
            Serial.print(mz);
            Serial.println(" Gauss");

            //    This is for the LEATR-500
            // Convert the X, Y and Z values into radians
            float rx = (float)x / 4096.0 * M_TWOPI;
            float ry = (float)y / 4096.0 * M_TWOPI;
            float rz = (float)z / 4096.0 * M_TWOPI;


            /*
            // This is for the LEAtR-1000
            float rx = (float)x / 2048.0 * M_TWOPI;
            float ry = (float)y / 2048.0 * M_TWOPI;
            float rz = (float)z / 2048.0 * M_TWOPI;
            */

            /*
            //  This is for the LEATR-JOY
            float rx = (float)x / 1024.0 * M_TWOPI;
            float ry = (float)y / 1024.0 * M_TWOPI;
            float rz = (float)z / 256.0 * M_TWOPI;
            */


            // Use a four quadrant Arc Tan to convert 2
            // axis to an angle (which is in radians) then
            // convert the angle from radians to degrees
            // for display.
            float angleXY = atan2f(ry, rx) * 180.0 / M_PI;
            float angleXZ = atan2f(rz, rx) * 180.0 / M_PI;
            float angleYZ = atan2f(rz, ry) * 180.0 / M_PI;

            Serial.print("angleXY, angleXZ, angleYZ = ");
            Serial.print(angleXY,4);
            Serial.print(", ");
            Serial.print(angleXZ,4);
            Serial.print(", ");
            Serial.print(angleYZ,4);
            Serial.println(" Degrees");
            delay(1000);
        }
        else
        {
            Serial.print("Unable to read the ALS31300. error = ");
            Serial.println(error);
            break;
        }
    }

}

long FOCals31313::SignExtendBitfield(uint32_t data, int width)
{
    long x = (long)data;
    long mask = 1L << (width - 1);

    if (width < 32)
    {
        x = x & ((1 << width) - 1); // make sure the upper bits are zero
    }

    return (long)((x ^ mask) - mask);
}

## FOCals31313.h
#ifndef FOCals31313_h
#define FOCals31313_h
#include "Arduino.h"
#include <sensors/MagneticSensorI2C.h>
#include <Wire.h>
#include <common/base_classes/Sensor.h>
//#include "../common/foc_utils.h"
//#include "../common/time_utils.h"

class FOCals31313: public MagneticSensorI2C{
  public:


  using MagneticSensorI2C::MagneticSensorI2C;


  using MagneticSensorI2C::getVelocity;
  using MagneticSensorI2C::initRelativeZero;
  using MagneticSensorI2C::initAbsoluteZero;
  using MagneticSensorI2C::hasAbsoluteZero;
  using MagneticSensorI2C::needsAbsoluteZeroSearch;
  //using MagneticSensorI2C::getAngle;
  float getAngle() override;
  virtual void init(TwoWire* _wire = &Wire);
  void readALS31300ADC_FastLoop(int busAddress);
  void readALS31300ADC_FullLoop(int busAddress);
  void readALS31300ADC(int busAddress);
  float getRawCount(void);
  uint16_t write(int busAddress, uint8_t address, uint32_t value);
  uint16_t read(int busAddress, uint8_t address, uint32_t& value);
  private:
    long SignExtendBitfield(uint32_t data, int width);
  /* the two wire instance for this sensor */

    TwoWire* wire;
    float cpr; //!< Maximum range of the magnetic sensor
    uint16_t lsb_used; //!< Number of bits used in LSB register
    uint8_t lsb_mask;
    uint8_t msb_mask;

    // I2C variables
    uint8_t angle_register_msb; //!< I2C angle register to read
    uint8_t chip_address; //!< I2C chip select pins


    word zero_offset; //!< user defined zero offset


    // total angle tracking variables
    float full_rotation_offset; //!<number of full rotations made
    float angle_data_prev; //!< angle in previous position calculation step

    // velocity calculation variables
    float angle_prev; //!< angle in previous velocity calculation step
    long velocity_calc_timestamp; //!< last velocity calculation timestamp
};


#endif

## simpleFOC_als31313.ino
/**
 *
 * Velocity motion control example
 * Steps:
 * 1) Configure the motor and encoder
 * 2) Run the code
 * 3) Set the target velocity (in radians per second) from serial terminal
 *
 *
 *
 * NOTE :
 * > Arduino UNO example code for running velocity motion control using an encoder with index significantly
 * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager.
 *
 * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins
 * > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger
 *
 * > If you don't want to use index pin initialize the encoder class without index pin number:
 * > For example:
 * > - Encoder encoder = Encoder(2, 3, 8192);
 * > and initialize interrupts like this:
 * > - encoder.enableInterrupts(doA,doB)
 *
 * Check the docs.simplefoc.com for more info about the possible encoder configuration.
 *
 */
#include <SimpleFOC.h>
#include "FOCals31313.h"
// software interrupt library

#include <neotimer.h>
Neotimer monitor_timer = Neotimer(500);

// BLDC motor & driver instance
//BLDCMotor motor = BLDCMotor(11);
//BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
// Stepper motor & driver instance
//StepperMotor motor = StepperMotor(50);
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);
#define IN1 25
#define IN2 26
#define IN3 27
#define IN4 14
StepperMotor motor = StepperMotor(50);
StepperDriver4PWM driver = StepperDriver4PWM(IN1,IN2,IN3,IN4,21,22);
FOCals31313 sensor = FOCals31313(0x41, 12, 0xFE, 8);

void setup() {
  Serial.begin(115200);


  // software interrupts
  //PciManager.registerListener(&listenerIndex);
  // link the motor to the sensor
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

  // limiting motor movements
  motor.voltage_limit = 12;   // [V]
  motor.velocity_limit = 20; // [rad/s]
  sensor.init();
  motor.linkSensor(&sensor);
  motor.controller = ControlType::velocity;
  motor.init();
  motor.initFOC();
  Serial.println("Setup done");

  _delay(1000);

}


void loop() {
  motor.loopFOC();
  if(monitor_timer.repeat()){
    Serial.println(motor.shaft_angle);
    Serial.println(sensor.getAngle(),6);
  }


}
	#include "FOCals31313.h"
	#define kNOERROR 0
	#define kDATATOOLONGERROR 1
	#define kRECEIVEDNACKONADDRESSERROR 2
	#define kRECEIVEDNACKONDATAERROR 3
	#define kOTHERERROR 4

	int deviceAddress = 96;

	MagneticSensorI2CConfig_s ALS31313_I2C_CONFIG = {
	.chip_address = 0x36,
	.bit_resolution = 12,
	.angle_register = 0x0E,
	.data_start_bit = 11
	};



	void FOCals31313::init(TwoWire* _wire){
	wire = _wire;
	wire->begin(16,17);
	wire->setClock(1000000);
	uint16_t error = write(deviceAddress, 0x24, 0x2C413534);

	if (error != kNOERROR)
	{
	Serial.print("Error while trying to enter customer access mode. error = ");
	Serial.println(error);
	}

	wire->requestFrom(0x02, 4);
	uint32_t data = wire->read() << 24;
	data += wire->read() << 16;
	data += wire->read() << 8;
	data += wire->read() ;

	bool a = bitRead(data,20);
	bool b = bitRead(data,19);

	Serial.print("sensor hall mode: ");
	Serial.print(a,BIN);
	Serial.println(b,BIN);

	bool ze = bitRead(data,8);
	bool ye = bitRead(data,7);
	bool xe = bitRead(data,6);

	Serial.print(ze,BIN);
	Serial.print(",");
	Serial.print(ye,BIN);
	Serial.print(",");
	Serial.println(xe,BIN);

	Serial.println("raw config");
	Serial.println(data,BIN);
	//Serial.print(",");
	//readALS31300ADC(deviceAddress);
	angle_prev = 0;
	velocity_calc_timestamp = _micros();
	// full rotations tracking number
	full_rotation_offset = 0;
	angle_data_prev = getRawCount();
	zero_offset = 0;
	cpr = pow(2, ALS31313_I2C_CONFIG.bit_resolution);
	Serial.println(angle_data_prev,6);
	return;
	}

	uint16_t FOCals31313::write(int busAddress, uint8_t address, uint32_t value)
	{
	// Write the address that is to be written to the device
	// and then the 4 bytes of data, MSB first
	wire->beginTransmission(busAddress);
	wire->write(address);
	wire->write((byte)(value >> 24));
	wire->write((byte)(value >> 16));
	wire->write((byte)(value >> 8));
	wire->write((byte)(value));
	return wire->endTransmission();
	}

	uint16_t FOCals31313::read(int busAddress, uint8_t address, uint32_t& value)
	{
	// Write the address that is to be read to the device
	wire->beginTransmission(busAddress);
	wire->write(address);
	int error = wire->endTransmission(false);

	// if the device accepted the address,
	// request 4 bytes from the device
	// and then read them, MSB first
	if (error == kNOERROR)
	{
	wire->requestFrom(busAddress, 4);
	value = wire->read() << 24;
	value += wire->read() << 16;
	value += wire->read() << 8;
	value += wire->read();
	}

	return error;
	}

	float FOCals31313::getAngle(){
	float angle_data = getRawCount();

	// tracking the number of rotations
	// in order to expand angle range form [0,2PI]
	// to basically infinity
	float d_angle = angle_data - angle_data_prev;
	//Serial.print(d_angle);
	//Serial.print(",");
	// if overflow happened track it as full rotation
	if(abs(d_angle) > (6) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
	// save the current angle value for the next steps
	// in order to know if overflow happened
	angle_data_prev = angle_data;
	//Serial.print(angle_data_prev,6);
	//Serial.print(",");
	// zero offset adding
	angle_data -= (int)zero_offset;
	//Serial.print(angle_data,6);
	//Serial.print(",");
	//Serial.print(full_rotation_offset);
	//Serial.print(",");
	//Serial.print(cpr);
	//Serial.println(" .");
	// return the full angle
	// (number of full rotations)*2PI + current sensor angle
	//return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI);
	return natural_direction * (full_rotation_offset + angle_data ) ;
	}
	void FOCals31313::readALS31300ADC_FastLoop(int busAddress)
	{

	}

	void FOCals31313::readALS31300ADC_FullLoop(int busAddress)
	{

	}

	float FOCals31313::getRawCount(){
	uint32_t value0x27;

	// Read the register the I2C loop mode is in
	uint16_t error = read(deviceAddress, 0x27, value0x27);
	if (error != kNOERROR)
	{
	Serial.print("Unable to read the ALS31300. error = ");
	Serial.println(error);
	}

	// I2C loop mode is in bits 2 and 3 so mask them out
	// and set them to the no loop mode
	value0x27 = (value0x27 & 0xFFFFFFF3) \| (0x0 << 2);

	// Write the new values to the register the I2C loop mode is in
	error = write(deviceAddress, 0x27, value0x27);
	if (error != kNOERROR)
	{
	Serial.print("Unable to read the ALS31300. error = ");
	Serial.println(error);
	}


	// Write the address that is going to be read from the ALS31300
	Wire.beginTransmission(deviceAddress);
	Wire.write(0x28);
	error = Wire.endTransmission(false);

	// The ALS31300 accepted the address
	if (error == kNOERROR)
	{
	// Start the read and request 8 bytes
	// which are the contents of register 0x28 and 0x29
	Wire.requestFrom(deviceAddress, 8);

	// Read the first 4 bytes which are the contents of register 0x28
	uint32_t value0x28 = Wire.read() << 24;
	value0x28 += Wire.read() << 16;
	value0x28 += Wire.read() << 8;
	value0x28 += Wire.read();

	// Read the next 4 bytes which are the contents of register 0x29
	uint32_t value0x29 = Wire.read() << 24;
	value0x29 += Wire.read() << 16;
	value0x29 += Wire.read() << 8;
	value0x29 += Wire.read();

	// Take the most significant byte of each axis from register 0x28 and combine it with the least
	// significant 4 bits of each axis from register 0x29, then sign extend the 12th bit.
	int x = SignExtendBitfield(((value0x28 >> 20) & 0x0FF0) \| ((value0x29 >> 16) & 0x0F), 12);
	int y = SignExtendBitfield(((value0x28 >> 12) & 0x0FF0) \| ((value0x29 >> 12) & 0x0F), 12);
	int z = SignExtendBitfield(((value0x28 >> 4) & 0x0FF0) \| ((value0x29 >> 8) & 0x0F), 12);

	/* / This is for the LEATR-500
	// Convert the X, Y and Z values into radians
	float rx = (float)x / 4096.0 * M_TWOPI;
	float ry = (float)y / 4096.0 * M_TWOPI;
	float rz = (float)z / 4096.0 * M_TWOPI;
	*/


	// This is for the LEAtR-1000
	//float rx = (float)x / 2048.0 * M_TWOPI;
	//float ry = (float)y / 2048.0 * M_TWOPI;
	//float rz = (float)z / 2048.0 * M_TWOPI;


	/*
	// This is for the LEATR-JOY
	float rx = (float)x / 1024.0 * M_TWOPI;
	float ry = (float)y / 1024.0 * M_TWOPI;
	float rz = (float)z / 256.0 * M_TWOPI;
	*/



	// Use a four quadrant Arc Tan to convert 2
	// axis to an angle (which is in radians) then
	// convert the angle from radians to degrees
	// for display.
	//return atan2f(ry, rx) * 180.0 / M_PI;
	return atan2f(x, y);
	}

	}

	void FOCals31313::readALS31300ADC(int busAddress)
	{
	uint32_t value0x27;

	// Read the register the I2C loop mode is in
	uint16_t error = read(busAddress, 0x27, value0x27);
	if (error != kNOERROR)
	{
	Serial.print("Unable to read the ALS31300. error = ");
	Serial.println(error);
	}

	// I2C loop mode is in bits 2 and 3 so mask them out
	// and set them to the no loop mode
	value0x27 = (value0x27 & 0xFFFFFFF3) \| (0x0 << 2);

	// Write the new values to the register the I2C loop mode is in
	error = write(busAddress, 0x27, value0x27);
	if (error != kNOERROR)
	{
	Serial.print("Unable to read the ALS31300. error = ");
	Serial.println(error);
	}

	for (int count = 0; count < 30; ++count)
	{
	// Write the address that is going to be read from the ALS31300
	Wire.beginTransmission(busAddress);
	Wire.write(0x28);
	uint16_t error = Wire.endTransmission(false);

	// The ALS31300 accepted the address
	if (error == kNOERROR)
	{
	// Start the read and request 8 bytes
	// which are the contents of register 0x28 and 0x29
	Wire.requestFrom(busAddress, 8);

	// Read the first 4 bytes which are the contents of register 0x28
	uint32_t value0x28 = Wire.read() << 24;
	value0x28 += Wire.read() << 16;
	value0x28 += Wire.read() << 8;
	value0x28 += Wire.read();

	// Read the next 4 bytes which are the contents of register 0x29
	uint32_t value0x29 = Wire.read() << 24;
	value0x29 += Wire.read() << 16;
	value0x29 += Wire.read() << 8;
	value0x29 += Wire.read();

	// Take the most significant byte of each axis from register 0x28 and combine it with the least
	// significant 4 bits of each axis from register 0x29, then sign extend the 12th bit.
	int x = SignExtendBitfield(((value0x28 >> 20) & 0x0FF0) \| ((value0x29 >> 16) & 0x0F), 12);
	int y = SignExtendBitfield(((value0x28 >> 12) & 0x0FF0) \| ((value0x29 >> 12) & 0x0F), 12);
	int z = SignExtendBitfield(((value0x28 >> 4) & 0x0FF0) \| ((value0x29 >> 8) & 0x0F), 12);


	bool a = bitRead(value0x29,7);
	bool b = bitRead(value0x29,6);
	Serial.print("hall mode: ");
	Serial.print(a,BIN);
	Serial.println(b,BIN);
	// Display the values of x, y and z
	//sprintf(xyzB,"%d x: %d y: %d z: %d", count, x,y,z);
	//sprintf(xyzB,"%d ", count);


	Serial.print(count);
	Serial.print(", ");
	Serial.print(x);
	Serial.print(", ");
	Serial.print(y);
	Serial.print(", ");
	Serial.println(z);

	// Look at the datasheet for the sensitivity of the part used.
	// In this case, full scale range is 500 gauss, other sensitivities
	// are 1000 gauss and 2000 gauss.
	// Sensitivity of 500 gauss = 4.0 lsb/g
	// Sensitivity of 1000 gauss = 2.0 lsb/g
	// Sensitivity of 2000 gauss = 1.0 lsb/g

	float mx = (float)x / 4.0;
	float my = (float)y / 4.0;
	float mz = (float)z / 4.0;

	Serial.print("MX, MY, MZ = ");
	Serial.print(mx);
	Serial.print(", ");
	Serial.print(my);
	Serial.print(", ");
	Serial.print(mz);
	Serial.println(" Gauss");

	// This is for the LEATR-500
	// Convert the X, Y and Z values into radians
	float rx = (float)x / 4096.0 * M_TWOPI;
	float ry = (float)y / 4096.0 * M_TWOPI;
	float rz = (float)z / 4096.0 * M_TWOPI;


	/*
	// This is for the LEAtR-1000
	float rx = (float)x / 2048.0 * M_TWOPI;
	float ry = (float)y / 2048.0 * M_TWOPI;
	float rz = (float)z / 2048.0 * M_TWOPI;
	*/

	/*
	// This is for the LEATR-JOY
	float rx = (float)x / 1024.0 * M_TWOPI;
	float ry = (float)y / 1024.0 * M_TWOPI;
	float rz = (float)z / 256.0 * M_TWOPI;
	*/



	// Use a four quadrant Arc Tan to convert 2
	// axis to an angle (which is in radians) then
	// convert the angle from radians to degrees
	// for display.
	float angleXY = atan2f(ry, rx) * 180.0 / M_PI;
	float angleXZ = atan2f(rz, rx) * 180.0 / M_PI;
	float angleYZ = atan2f(rz, ry) * 180.0 / M_PI;

	Serial.print("angleXY, angleXZ, angleYZ = ");
	Serial.print(angleXY,4);
	Serial.print(", ");
	Serial.print(angleXZ,4);
	Serial.print(", ");
	Serial.print(angleYZ,4);
	Serial.println(" Degrees");
	delay(1000);
	}
	else
	{
	Serial.print("Unable to read the ALS31300. error = ");
	Serial.println(error);
	break;
	}
	}

	}

	long FOCals31313::SignExtendBitfield(uint32_t data, int width)
	{
	long x = (long)data;
	long mask = 1L << (width - 1);

	if (width < 32)
	{
	x = x & ((1 << width) - 1); // make sure the upper bits are zero
	}

	return (long)((x ^ mask) - mask);
	}
	#ifndef FOCals31313_h
	#define FOCals31313_h
	#include "Arduino.h"
	#include <sensors/MagneticSensorI2C.h>
	#include <Wire.h>
	#include <common/base_classes/Sensor.h>
	//#include "../common/foc_utils.h"
	//#include "../common/time_utils.h"

	class FOCals31313: public MagneticSensorI2C{
	public:


	using MagneticSensorI2C::MagneticSensorI2C;


	using MagneticSensorI2C::getVelocity;
	using MagneticSensorI2C::initRelativeZero;
	using MagneticSensorI2C::initAbsoluteZero;
	using MagneticSensorI2C::hasAbsoluteZero;
	using MagneticSensorI2C::needsAbsoluteZeroSearch;
	//using MagneticSensorI2C::getAngle;
	float getAngle() override;
	virtual void init(TwoWire* _wire = &Wire);
	void readALS31300ADC_FastLoop(int busAddress);
	void readALS31300ADC_FullLoop(int busAddress);
	void readALS31300ADC(int busAddress);
	float getRawCount(void);
	uint16_t write(int busAddress, uint8_t address, uint32_t value);
	uint16_t read(int busAddress, uint8_t address, uint32_t& value);
	private:
	long SignExtendBitfield(uint32_t data, int width);
	/* the two wire instance for this sensor */

	TwoWire* wire;
	float cpr; //!< Maximum range of the magnetic sensor
	uint16_t lsb_used; //!< Number of bits used in LSB register
	uint8_t lsb_mask;
	uint8_t msb_mask;

	// I2C variables
	uint8_t angle_register_msb; //!< I2C angle register to read
	uint8_t chip_address; //!< I2C chip select pins


	word zero_offset; //!< user defined zero offset


	// total angle tracking variables
	float full_rotation_offset; //!<number of full rotations made
	float angle_data_prev; //!< angle in previous position calculation step

	// velocity calculation variables
	float angle_prev; //!< angle in previous velocity calculation step
	long velocity_calc_timestamp; //!< last velocity calculation timestamp
	};



	#endif
	/**
	*
	* Velocity motion control example
	* Steps:
	* 1) Configure the motor and encoder
	* 2) Run the code
	* 3) Set the target velocity (in radians per second) from serial terminal
	*
	*
	*
	* NOTE :
	* > Arduino UNO example code for running velocity motion control using an encoder with index significantly
	* > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager.
	*
	* > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins
	* > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger
	*
	* > If you don't want to use index pin initialize the encoder class without index pin number:
	* > For example:
	* > - Encoder encoder = Encoder(2, 3, 8192);
	* > and initialize interrupts like this:
	* > - encoder.enableInterrupts(doA,doB)
	*
	* Check the docs.simplefoc.com for more info about the possible encoder configuration.
	*
	*/
	#include <SimpleFOC.h>
	#include "FOCals31313.h"
	// software interrupt library

	#include <neotimer.h>
	Neotimer monitor_timer = Neotimer(500);

	// BLDC motor & driver instance
	//BLDCMotor motor = BLDCMotor(11);
	//BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
	// Stepper motor & driver instance
	//StepperMotor motor = StepperMotor(50);
	//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
	#define IN1 25
	#define IN2 26
	#define IN3 27
	#define IN4 14
	StepperMotor motor = StepperMotor(50);
	StepperDriver4PWM driver = StepperDriver4PWM(IN1,IN2,IN3,IN4,21,22);
	FOCals31313 sensor = FOCals31313(0x41, 12, 0xFE, 8);

	void setup() {
	Serial.begin(115200);




	// software interrupts
	//PciManager.registerListener(&listenerIndex);
	// link the motor to the sensor
	driver.voltage_power_supply = 12;
	driver.init();
	// link the motor and the driver
	motor.linkDriver(&driver);

	// limiting motor movements
	motor.voltage_limit = 12; // [V]
	motor.velocity_limit = 20; // [rad/s]
	sensor.init();
	motor.linkSensor(&sensor);
	motor.controller = ControlType::velocity;
	motor.init();
	motor.initFOC();
	Serial.println("Setup done");

	_delay(1000);

	}



	void loop() {
	motor.loopFOC();
	if(monitor_timer.repeat()){
	Serial.println(motor.shaft_angle);
	Serial.println(sensor.getAngle(),6);
	}


	}