diff --git a/examples/encoders/linearhall/linearhall.ino b/examples/encoders/linearhall/linearhall.ino new file mode 100644 index 0000000..8103e2c --- /dev/null +++ b/examples/encoders/linearhall/linearhall.ino @@ -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 +#include +#include + +// 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(); +} \ No newline at end of file diff --git a/src/encoders/linearhall/LinearHall.cpp b/src/encoders/linearhall/LinearHall.cpp new file mode 100644 index 0000000..96cfae3 --- /dev/null +++ b/src/encoders/linearhall/LinearHall.cpp @@ -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); +} diff --git a/src/encoders/linearhall/LinearHall.h b/src/encoders/linearhall/LinearHall.h new file mode 100644 index 0000000..d17035e --- /dev/null +++ b/src/encoders/linearhall/LinearHall.h @@ -0,0 +1,45 @@ +#ifndef LINEAR_HALL_SENSOR_LIB_H +#define LINEAR_HALL_SENSOR_LIB_H + +#include + +// 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