Skip to content

Commit 5e9981c

Browse files
runger1101001runger1101001
authored
Merge pull request #39 from dekutree64/dev
Improvements to LinearHall
2 parents 9cc5ab8 + 3da319d commit 5e9981c

File tree

3 files changed

+266
-0
lines changed

3 files changed

+266
-0
lines changed

‎examples/encoders/linearhall/linearhall.ino

+111
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
/**
2+
*
3+
* Velocity motion control example
4+
* Steps:
5+
* 1) Configure the motor and sensor
6+
* 2) Run the code
7+
* 3) Set the target velocity (in radians per second) from serial terminal
8+
*/
9+
#include <SimpleFOC.h>
10+
#include <SimpleFOCDrivers.h>
11+
#include <encoders/linearhall/LinearHall.h>
12+
13+
// BLDC motor & driver instance
14+
BLDCMotor motor = BLDCMotor(11);
15+
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
16+
// Stepper motor & driver instance
17+
//StepperMotor motor = StepperMotor(50);
18+
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
19+
20+
// hall sensor instance
21+
LinearHall sensor = LinearHall(A0, A1, 11);
22+
23+
// velocity set point variable
24+
float target_velocity = 0;
25+
// instantiate the commander
26+
Commander command = Commander(Serial);
27+
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
28+
29+
void setup() {
30+
31+
// driver config
32+
// power supply voltage [V]
33+
driver.voltage_power_supply = 12;
34+
driver.init();
35+
// link the motor and the driver
36+
motor.linkDriver(&driver);
37+
38+
// aligning voltage [V]
39+
motor.voltage_sensor_align = 3;
40+
41+
// set motion control loop to be used
42+
motor.controller = MotionControlType::velocity;
43+
44+
// contoller configuration
45+
// default parameters in defaults.h
46+
47+
// velocity PI controller parameters
48+
motor.PID_velocity.P = 0.2f;
49+
motor.PID_velocity.I = 2;
50+
motor.PID_velocity.D = 0;
51+
// default voltage_power_supply
52+
motor.voltage_limit = 6;
53+
// jerk control using voltage voltage ramp
54+
// default value is 300 volts per sec ~ 0.3V per millisecond
55+
motor.PID_velocity.output_ramp = 1000;
56+
57+
// velocity low pass filtering time constant
58+
motor.LPF_velocity.Tf = 0.01f;
59+
60+
// use monitoring with serial
61+
Serial.begin(115200);
62+
// comment out if not needed
63+
motor.useMonitoring(Serial);
64+
65+
// initialize motor
66+
motor.init();
67+
// initialize sensor hardware. This moves the motor to find the min/max sensor readings and
68+
// averages them to get the center values. The motor can't move until motor.init is called, and
69+
// motor.initFOC can't do its calibration until the sensor is intialized, so this must be done inbetween.
70+
// You can then take the values printed to the serial monitor and pass them to sensor.init to
71+
// avoid having to move the motor every time. In that case it doesn't matter whether sensor.init
72+
// is called before or after motor.init.
73+
sensor.init(&motor);
74+
Serial.print("LinearHall centerA: ");
75+
Serial.print(sensor.centerA);
76+
Serial.print(", centerB: ");
77+
Serial.println(sensor.centerB);
78+
// link the motor to the sensor
79+
motor.linkSensor(&sensor);
80+
// align sensor and start FOC
81+
motor.initFOC();
82+
83+
// add target command T
84+
command.add('T', doTarget, "target voltage");
85+
86+
Serial.println(F("Motor ready."));
87+
Serial.println(F("Set the target velocity using serial terminal:"));
88+
_delay(1000);
89+
}
90+
91+
92+
void loop() {
93+
// main FOC algorithm function
94+
// the faster you run this function the better
95+
// Arduino UNO loop ~1kHz
96+
// Bluepill loop ~10kHz
97+
motor.loopFOC();
98+
99+
// Motion control function
100+
// velocity, position or voltage (defined in motor.controller)
101+
// this function can be run at much lower frequency than loopFOC() function
102+
// You can also use motor.move() and set the motor.target in the code
103+
motor.move(target_velocity);
104+
105+
// function intended to be used with serial plotter to monitor motor variables
106+
// significantly slowing the execution down!!!!
107+
// motor.monitor();
108+
109+
// user communication
110+
command.run();
111+
}

‎src/encoders/linearhall/LinearHall.cpp

+110
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
#include "LinearHall.h"
2+
3+
// This function can be overridden with custom ADC code on platforms with poor analogRead performance.
4+
__attribute__((weak)) void ReadLinearHalls(int hallA, int hallB, int *a, int *b)
5+
{
6+
*a = analogRead(hallA);
7+
*b = analogRead(hallB);
8+
}
9+
10+
LinearHall::LinearHall(int _hallA, int _hallB, int _pp){
11+
centerA = 512;
12+
centerB = 512;
13+
pinA = _hallA;
14+
pinB = _hallB;
15+
pp = _pp;
16+
}
17+
18+
float LinearHall::getSensorAngle() {
19+
ReadLinearHalls(pinA, pinB, &lastA, &lastB);
20+
//offset readings using center values, then compute angle
21+
float reading = _atan2(lastA - centerA, lastB - centerB);
22+
23+
//handle rollover logic between each electrical revolution of the motor
24+
if (reading > prev_reading) {
25+
if (reading - prev_reading >= PI) {
26+
if (electrical_rev - 1 < 0) {
27+
electrical_rev = pp - 1;
28+
} else {
29+
electrical_rev = electrical_rev - 1;
30+
}
31+
}
32+
} else if (reading < prev_reading) {
33+
if (prev_reading - reading >= PI) {
34+
if (electrical_rev + 1 >= pp) {
35+
electrical_rev = 0;
36+
} else {
37+
electrical_rev = electrical_rev + 1;
38+
}
39+
}
40+
}
41+
42+
//convert result from electrical angle and electrical revolution count to shaft angle in radians
43+
float result = (reading + PI) / _2PI;
44+
result = _2PI * (result + electrical_rev) / pp;
45+
46+
//update previous reading for rollover handling
47+
prev_reading = reading;
48+
return result;
49+
}
50+
51+
void LinearHall::init(int _centerA, int _centerB) {
52+
// Skip configuring the pins here because they normally default to input anyway, and
53+
// this makes it possible to use ADC channel numbers instead of pin numbers when using
54+
// custom implementation of ReadLinearHalls, to avoid having to remap them every update.
55+
// If pins do need to be configured, it can be done by user code before calling init.
56+
//pinMode(pinA, INPUT);
57+
//pinMode(pinB, INPUT);
58+
59+
centerA = _centerA;
60+
centerB = _centerB;
61+
62+
//establish initial reading for rollover handling
63+
electrical_rev = 0;
64+
ReadLinearHalls(pinA, pinB, &lastA, &lastB);
65+
prev_reading = _atan2(lastA - centerA, lastB - centerB);
66+
}
67+
68+
void LinearHall::init(FOCMotor *motor) {
69+
if (!motor->enabled) {
70+
SIMPLEFOC_DEBUG("LinearHall::init failed. Call after motor.init, but before motor.initFOC.");
71+
return;
72+
}
73+
74+
// See comment in other version of init for why these are commented out
75+
//pinMode(pinA, INPUT);
76+
//pinMode(pinB, INPUT);
77+
78+
int minA, maxA, minB, maxB;
79+
80+
ReadLinearHalls(pinA, pinB, &lastA, &lastB);
81+
minA = maxA = centerA = lastA;
82+
minB = maxB = centerB = lastB;
83+
84+
// move one mechanical revolution forward
85+
for (int i = 0; i <= 2000; i++)
86+
{
87+
float angle = _3PI_2 + _2PI * i * pp / 2000.0f;
88+
motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle);
89+
90+
ReadLinearHalls(pinA, pinB, &lastA, &lastB);
91+
92+
if (lastA < minA)
93+
minA = lastA;
94+
if (lastA > maxA)
95+
maxA = lastA;
96+
centerA = (minA + maxA) / 2;
97+
98+
if (lastB < minB)
99+
minB = lastB;
100+
if (lastB > maxB)
101+
maxB = lastB;
102+
centerB = (minB + maxB) / 2;
103+
104+
_delay(2);
105+
}
106+
107+
//establish initial reading for rollover handling
108+
electrical_rev = 0;
109+
prev_reading = _atan2(lastA - centerA, lastB - centerB);
110+
}

‎src/encoders/linearhall/LinearHall.h

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
#ifndef LINEAR_HALL_SENSOR_LIB_H
2+
#define LINEAR_HALL_SENSOR_LIB_H
3+
4+
#include <SimpleFOC.h>
5+
6+
// This function can be overridden with custom ADC code on platforms with poor analogRead performance.
7+
void ReadLinearHalls(int hallA, int hallB, int *a, int *b);
8+
9+
/**
10+
* This sensor class is for two linear hall effect sensors such as 49E, which are
11+
* positioned 90 electrical degrees apart (if one is centered on a rotor magnet,
12+
* the other is half way between rotor magnets).
13+
* It can also be used for a single magnet mounted to the motor shaft (set pp to 1).
14+
*
15+
* For more information, see this forum thread and PDF
16+
* https://community.simplefoc.com/t/40-cent-magnetic-angle-sensing-technique/1959
17+
* https://gist.github.com/nanoparticle/00030ea27c59649edbed84f0a957ebe1
18+
*/
19+
class LinearHall: public Sensor{
20+
public:
21+
LinearHall(int hallA, int hallB, int pp);
22+
23+
void init(int centerA, int centerB); // Initialize without moving motor
24+
void init(class FOCMotor *motor); // Move motor to find center values
25+
26+
// Get current shaft angle from the sensor hardware, and
27+
// return it as a float in radians, in the range 0 to 2PI.
28+
// - This method is pure virtual and must be implemented in subclasses.
29+
// Calling this method directly does not update the base-class internal fields.
30+
// Use update() when calling from outside code.
31+
float getSensorAngle() override;
32+
33+
int centerA;
34+
int centerB;
35+
int lastA, lastB;
36+
37+
private:
38+
int pinA;
39+
int pinB;
40+
int pp;
41+
int electrical_rev;
42+
float prev_reading;
43+
};
44+
45+
#endif

0 commit comments

Comments
 (0)