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

Improvements to LinearHall #39

Merged
merged 3 commits into from
Feb 14, 2024
Merged
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
111 changes: 111 additions & 0 deletions examples/encoders/linearhall/linearhall.ino
Original file line number Diff line number Diff line change
@@ -0,0 +1,111 @@
/**
*
* Velocity motion control example
* Steps:
* 1) Configure the motor and sensor
* 2) Run the code
* 3) Set the target velocity (in radians per second) from serial terminal
*/
#include <SimpleFOC.h>
#include <SimpleFOCDrivers.h>
#include <encoders/linearhall/LinearHall.h>

// 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);

// hall sensor instance
LinearHall sensor = LinearHall(A0, A1, 11);

// velocity set point variable
float target_velocity = 0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }

void setup() {

// driver config
// power supply voltage [V]
driver.voltage_power_supply = 12;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);

// aligning voltage [V]
motor.voltage_sensor_align = 3;

// set motion control loop to be used
motor.controller = MotionControlType::velocity;

// contoller configuration
// default parameters in defaults.h

// velocity PI controller parameters
motor.PID_velocity.P = 0.2f;
motor.PID_velocity.I = 2;
motor.PID_velocity.D = 0;
// default voltage_power_supply
motor.voltage_limit = 6;
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
motor.PID_velocity.output_ramp = 1000;

// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.01f;

// use monitoring with serial
Serial.begin(115200);
// comment out if not needed
motor.useMonitoring(Serial);

// initialize motor
motor.init();
// initialize sensor hardware. This moves the motor to find the min/max sensor readings and
// averages them to get the center values. The motor can't move until motor.init is called, and
// motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween.
// You can then take the values printed to the serial monitor and pass them to sensor.init to
// avoid having to move the motor every time. In that case it doesn't matter whether sensor.init
// is called before or after motor.init.
sensor.init(&motor);
Serial.print("LinearHall centerA: ");
Serial.print(sensor.centerA);
Serial.print(", centerB: ");
Serial.println(sensor.centerB);
// link the motor to the sensor
motor.linkSensor(&sensor);
// align sensor and start FOC
motor.initFOC();

// add target command T
command.add('T', doTarget, "target voltage");

Serial.println(F("Motor ready."));
Serial.println(F("Set the target velocity using serial terminal:"));
_delay(1000);
}


void loop() {
// main FOC algorithm function
// the faster you run this function the better
// Arduino UNO loop ~1kHz
// Bluepill loop ~10kHz
motor.loopFOC();

// Motion control function
// velocity, position or voltage (defined in motor.controller)
// this function can be run at much lower frequency than loopFOC() function
// You can also use motor.move() and set the motor.target in the code
motor.move(target_velocity);

// function intended to be used with serial plotter to monitor motor variables
// significantly slowing the execution down!!!!
// motor.monitor();

// user communication
command.run();
}
110 changes: 110 additions & 0 deletions src/encoders/linearhall/LinearHall.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
#include "LinearHall.h"

// This function can be overridden with custom ADC code on platforms with poor analogRead performance.
__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b)
{
*a = analogRead(hallA);
*b = analogRead(hallB);
}

LinearHall::LinearHall(int _hallA, int _hallB, int _pp){
centerA = 512;
centerB = 512;
pinA = _hallA;
pinB = _hallB;
pp = _pp;
}

float LinearHall::getSensorAngle() {
ReadLinearHalls(pinA, pinB, &lastA, &lastB);
//offset readings using center values, then compute angle
float reading = _atan2(lastA - centerA, lastB - centerB);

//handle rollover logic between each electrical revolution of the motor
if (reading > prev_reading) {
if (reading - prev_reading >= PI) {
if (electrical_rev - 1 < 0) {
electrical_rev = pp - 1;
} else {
electrical_rev = electrical_rev - 1;
}
}
} else if (reading < prev_reading) {
if (prev_reading - reading >= PI) {
if (electrical_rev + 1 >= pp) {
electrical_rev = 0;
} else {
electrical_rev = electrical_rev + 1;
}
}
}

//convert result from electrical angle and electrical revolution count to shaft angle in radians
float result = (reading + PI) / _2PI;
result = _2PI * (result + electrical_rev) / pp;

//update previous reading for rollover handling
prev_reading = reading;
return result;
}

void LinearHall::init(int _centerA, int _centerB) {
// Skip configuring the pins here because they normally default to input anyway, and
// this makes it possible to use ADC channel numbers instead of pin numbers when using
// custom implementation of ReadLinearHalls, to avoid having to remap them every update.
// If pins do need to be configured, it can be done by user code before calling init.
//pinMode(pinA, INPUT);
//pinMode(pinB, INPUT);

centerA = _centerA;
centerB = _centerB;

//establish initial reading for rollover handling
electrical_rev = 0;
ReadLinearHalls(pinA, pinB, &lastA, &lastB);
prev_reading = _atan2(lastA - centerA, lastB - centerB);
}

void LinearHall::init(FOCMotor *motor) {
if (!motor->enabled) {
SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC.");
return;
}

// See comment in other version of init for why these are commented out
//pinMode(pinA, INPUT);
//pinMode(pinB, INPUT);

int minA, maxA, minB, maxB;

ReadLinearHalls(pinA, pinB, &lastA, &lastB);
minA = maxA = centerA = lastA;
minB = maxB = centerB = lastB;

// move one mechanical revolution forward
for (int i = 0; i <= 2000; i++)
{
float angle = _3PI_2 + _2PI * i * pp / 2000.0f;
motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle);

ReadLinearHalls(pinA, pinB, &lastA, &lastB);

if (lastA < minA)
minA = lastA;
if (lastA > maxA)
maxA = lastA;
centerA = (minA + maxA) / 2;

if (lastB < minB)
minB = lastB;
if (lastB > maxB)
maxB = lastB;
centerB = (minB + maxB) / 2;

_delay(2);
}

//establish initial reading for rollover handling
electrical_rev = 0;
prev_reading = _atan2(lastA - centerA, lastB - centerB);
}
45 changes: 45 additions & 0 deletions src/encoders/linearhall/LinearHall.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
#ifndef LINEAR_HALL_SENSOR_LIB_H
#define LINEAR_HALL_SENSOR_LIB_H

#include <SimpleFOC.h>

// This function can be overridden with custom ADC code on platforms with poor analogRead performance.
void ReadLinearHalls(int hallA, int hallB, int *a, int *b);

/**
* This sensor class is for two linear hall effect sensors such as 49E, which are
* positioned 90 electrical degrees apart (if one is centered on a rotor magnet,
* the other is half way between rotor magnets).
* It can also be used for a single magnet mounted to the motor shaft (set pp to 1).
*
* For more information, see this forum thread and PDF
* https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959
* https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1
*/
class LinearHall: public Sensor{
public:
LinearHall(int hallA, int hallB, int pp);

void init(int centerA, int centerB); // Initialize without moving motor
void init(class FOCMotor *motor); // Move motor to find center values

// Get current shaft angle from the sensor hardware, and
// return it as a float in radians, in the range 0 to 2PI.
// - This method is pure virtual and must be implemented in subclasses.
// Calling this method directly does not update the base-class internal fields.
// Use update() when calling from outside code.
float getSensorAngle() override;

int centerA;
int centerB;
int lastA, lastB;

private:
int pinA;
int pinB;
int pp;
int electrical_rev;
float prev_reading;
};

#endif
Loading