add endstop detection
This commit is contained in:
parent
77896cfc55
commit
526ffacd4f
@ -5,12 +5,10 @@ float norm(float x, float in_min, float in_max, float out_min = -1.0, float out_
|
||||
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
|
||||
}
|
||||
|
||||
LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos)
|
||||
LinearHallSensor::LinearHallSensor(uint8_t ch1, uint8_t ch2)
|
||||
{
|
||||
_analogPin1 = ch1;
|
||||
_analogPin2 = ch2;
|
||||
_minPositionEndValue = minPos;
|
||||
_maxPositionEndValue = maxPos;
|
||||
}
|
||||
|
||||
void LinearHallSensor::init(BLDCMotor motor)
|
||||
@ -22,68 +20,129 @@ void LinearHallSensor::init(BLDCMotor motor)
|
||||
|
||||
uint32_t senseA = analogRead(_analogPin1);
|
||||
uint32_t senseB = analogRead(_analogPin2);
|
||||
uint32_t oldSenseA = 0;
|
||||
uint32_t oldSenseB = 0;
|
||||
|
||||
_minCh1 = senseA;
|
||||
_maxCh1 = senseA;
|
||||
_minCh2 = senseB;
|
||||
_maxCh2 = senseB;
|
||||
|
||||
// Swipe motor
|
||||
for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.005)
|
||||
{
|
||||
motor.move(i);
|
||||
delay(1);
|
||||
senseA = analogRead(_analogPin1);
|
||||
senseB = analogRead(_analogPin2);
|
||||
if (senseA > _maxCh1)
|
||||
_maxCh1 = senseA;
|
||||
else if (senseA < _minCh1)
|
||||
_minCh1 = senseA;
|
||||
if (senseB > _maxCh1)
|
||||
_maxCh1 = senseB;
|
||||
else if (senseB < _minCh2)
|
||||
_minCh2 = senseB;
|
||||
}
|
||||
delay(100);
|
||||
for (float i = _maxPositionEndValue; i >= _minPositionEndValue; i = i - 0.005)
|
||||
{
|
||||
motor.move(i);
|
||||
delay(1);
|
||||
senseA = analogRead(_analogPin1);
|
||||
senseB = analogRead(_analogPin2);
|
||||
if (senseA > _maxCh1)
|
||||
_maxCh1 = senseA;
|
||||
else if (senseA < _minCh1)
|
||||
_minCh1 = senseA;
|
||||
if (senseB > _maxCh1)
|
||||
_maxCh1 = senseB;
|
||||
else if (senseB < _minCh2)
|
||||
_minCh2 = senseB;
|
||||
}
|
||||
// Swipe motor to search hard end and find max analog values of sensors
|
||||
bool endFound = false;
|
||||
const float step = 0.0025;
|
||||
uint8_t currentCheck = 0; // current check number
|
||||
const uint8_t nCheck = 15; // number of times to check if its the same value
|
||||
const uint8_t epsilon = 3;
|
||||
float currentPosition = 0.0;
|
||||
|
||||
// Min offset with 50 values average
|
||||
float sum = 0.0;
|
||||
for (uint8_t i = 0; i <= 49; i++)
|
||||
Serial.println("\tFinding end stops");
|
||||
while (!endFound)
|
||||
{
|
||||
_offset = 0.0;
|
||||
sum += readSensorCallback();
|
||||
Serial.print("Position =" + String(currentPosition) + '\t');
|
||||
Serial.print("SenseA =" + String(senseA) + "\tSenseB =" + String(senseB));
|
||||
Serial.print("\tOldseA =" + String(oldSenseA) + "\tOldseB =" + String(oldSenseB) + "\tchecked =" + String(currentCheck) + "\r");
|
||||
senseA = analogRead(_analogPin1);
|
||||
senseB = analogRead(_analogPin2);
|
||||
|
||||
// Check if new extremes sensor values
|
||||
if (senseA > _maxCh1)
|
||||
_maxCh1 = senseA;
|
||||
else if (senseA < _minCh1)
|
||||
_minCh1 = senseA;
|
||||
if (senseB > _maxCh1)
|
||||
_maxCh1 = senseB;
|
||||
else if (senseB < _minCh2)
|
||||
_minCh2 = senseB;
|
||||
|
||||
// Compare with previous position
|
||||
if ((abs((int)(senseA - oldSenseA)) < epsilon) && (abs((int)(senseB - oldSenseB)) < epsilon))
|
||||
{
|
||||
if (currentCheck == nCheck)
|
||||
endFound = true;
|
||||
else
|
||||
currentCheck++;
|
||||
}
|
||||
else
|
||||
currentCheck = 0;
|
||||
|
||||
// Replace previous values
|
||||
oldSenseA = senseA;
|
||||
oldSenseB = senseB;
|
||||
|
||||
// Move to new position
|
||||
currentPosition += step;
|
||||
motor.move(currentPosition);
|
||||
delay(2);
|
||||
}
|
||||
_maxPositionEndValue = currentPosition - (step * nCheck);
|
||||
currentPosition = _maxPositionEndValue;
|
||||
motor.move(currentPosition);
|
||||
Serial.println("Found first end stop : Max position = " + String(_maxPositionEndValue));
|
||||
delay(100);
|
||||
|
||||
// Swipe motor to search other hard end and find eventually new max analog values of sensors
|
||||
endFound = false;
|
||||
currentCheck = 0;
|
||||
while (!endFound)
|
||||
{
|
||||
senseA = analogRead(_analogPin1);
|
||||
senseB = analogRead(_analogPin2);
|
||||
|
||||
// Check if new extremes sensor values
|
||||
if (senseA > _maxCh1)
|
||||
_maxCh1 = senseA;
|
||||
else if (senseA < _minCh1)
|
||||
_minCh1 = senseA;
|
||||
if (senseB > _maxCh1)
|
||||
_maxCh1 = senseB;
|
||||
else if (senseB < _minCh2)
|
||||
_minCh2 = senseB;
|
||||
|
||||
// Compare with previous position
|
||||
if ((abs((int)(senseA - oldSenseA)) < epsilon) && (abs((int)(senseB - oldSenseB)) < epsilon))
|
||||
{
|
||||
if (currentCheck == nCheck)
|
||||
endFound = true;
|
||||
else
|
||||
currentCheck++;
|
||||
}
|
||||
else
|
||||
currentCheck = 0;
|
||||
|
||||
// Replace previous values
|
||||
oldSenseA = senseA;
|
||||
oldSenseB = senseB;
|
||||
|
||||
// Move to new position
|
||||
currentPosition -= step;
|
||||
motor.move(currentPosition);
|
||||
delay(2);
|
||||
}
|
||||
_minPositionEndValue = currentPosition + (step * nCheck);
|
||||
Serial.println("Found second end stop : Min position = " + String(_minPositionEndValue));
|
||||
delay(100);
|
||||
|
||||
// Set min offset with 50 values average
|
||||
float sum = 0.0;
|
||||
_offset = 0.0;
|
||||
for (uint8_t i = 0; i <= 49; i++)
|
||||
sum += readSensorCallback();
|
||||
_offset = -sum / 50.0;
|
||||
|
||||
Serial.println("\n\rmaxA: " + String(_maxCh1) + "\tminA: " + String(_minCh1) + "\tmaxB: " + String(_maxCh2) + "\tminB: " + String(_minCh2));
|
||||
Serial.println("Offset: " + String(_offset));
|
||||
|
||||
// Detect max angle
|
||||
for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.01)
|
||||
// Read max angle
|
||||
for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.005)
|
||||
{
|
||||
motor.move(i);
|
||||
delay(1);
|
||||
_maxSensor = readSensorCallback();
|
||||
}
|
||||
Serial.println("current motor angle: " + String(motor.electricalAngle()));
|
||||
delay(250);
|
||||
|
||||
// take average value of 50 values
|
||||
// Set max take average value of 50 values
|
||||
sum = 0.0;
|
||||
for (uint8_t i = 0; i <= 49; i++)
|
||||
sum += readSensorCallback();
|
||||
@ -92,7 +151,7 @@ void LinearHallSensor::init(BLDCMotor motor)
|
||||
Serial.println("Max sensor: " + String(_maxSensor));
|
||||
|
||||
// Recovering previous controller type and go in the middle
|
||||
for (float i = _maxPositionEndValue; i >= _minPositionEndValue / 2.0; i = i - 0.01)
|
||||
for (float i = _maxPositionEndValue; i >= (_maxPositionEndValue + _minPositionEndValue) / 2.0; i = i - 0.01)
|
||||
{
|
||||
motor.move(i);
|
||||
delay(1);
|
||||
|
@ -15,10 +15,8 @@ class LinearHallSensor
|
||||
Linear Hall Sensor class constructor
|
||||
@param ch1 Analog pin for channel 1
|
||||
@param ch2 Analog pin for channel 2
|
||||
@param minPos Minimum position in Openloop
|
||||
@param maxPos Maximum position in Openloop
|
||||
*/
|
||||
LinearHallSensor(uint8_t ch1, uint8_t ch2, float minPos, float maxPos);
|
||||
LinearHallSensor(uint8_t ch1, uint8_t ch2);
|
||||
|
||||
|
||||
/**
|
||||
|
97
src/main.cpp
97
src/main.cpp
@ -3,22 +3,20 @@
|
||||
#include <linearHallSensor.h>
|
||||
#include <pinout.h>
|
||||
|
||||
BLDCMotor motor = BLDCMotor(4);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3);
|
||||
BLDCMotor motor1 = BLDCMotor(4);
|
||||
BLDCDriver3PWM driver1 = BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3);
|
||||
LinearHallSensor linearSensor1 = LinearHallSensor(M3_Hall1, M3_Hall2);
|
||||
|
||||
LinearHallSensor linearSensor = LinearHallSensor(M2_Hall1, M2_Hall2, -0.75, 2.35);
|
||||
|
||||
void initSensor()
|
||||
void initSensor1()
|
||||
{
|
||||
linearSensor.init(motor);
|
||||
linearSensor1.init(motor1);
|
||||
}
|
||||
float callback1()
|
||||
{
|
||||
return linearSensor1.readSensorCallback();
|
||||
}
|
||||
|
||||
float callback()
|
||||
{
|
||||
return linearSensor.readSensorCallback();
|
||||
}
|
||||
|
||||
GenericSensor sensor = GenericSensor(callback, initSensor);
|
||||
GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
|
||||
|
||||
float targetX = 0.0;
|
||||
float targetY = 0.0;
|
||||
@ -51,68 +49,45 @@ void setup()
|
||||
{
|
||||
|
||||
Serial.begin(115200);
|
||||
delay(2000);
|
||||
delay(5000);
|
||||
Serial.println("INIT");
|
||||
|
||||
// pinMode(M1_PWM1, OUTPUT);
|
||||
// pinMode(M1_PWM2, OUTPUT);
|
||||
// pinMode(M1_PWM3, OUTPUT);
|
||||
// digitalWrite(M1_PWM1, LOW);
|
||||
// digitalWrite(M1_PWM2, LOW);
|
||||
// digitalWrite(M1_PWM3, LOW);
|
||||
pinMode(LED_BUILTIN, OUTPUT); // Lightup LED
|
||||
digitalWrite(LED_BUILTIN, LOW);
|
||||
|
||||
// pinMode(M2_PWM1, OUTPUT);
|
||||
// pinMode(M2_PWM2, OUTPUT);
|
||||
// pinMode(M2_PWM3, OUTPUT);
|
||||
// digitalWrite(M2_PWM1, LOW);
|
||||
// digitalWrite(M2_PWM2, LOW);
|
||||
// digitalWrite(M2_PWM3, LOW);
|
||||
|
||||
// pinMode(M3_PWM1, OUTPUT);
|
||||
// pinMode(M3_PWM2, OUTPUT);
|
||||
// pinMode(M3_PWM3, OUTPUT);
|
||||
// digitalWrite(M3_PWM1, LOW);
|
||||
// digitalWrite(M3_PWM2, LOW);
|
||||
// digitalWrite(M3_PWM3, LOW);
|
||||
|
||||
pinMode(LED_BUILTIN, OUTPUT); // Lightup LED
|
||||
digitalWrite(LED_BUILTIN, HIGH);
|
||||
|
||||
driver.voltage_power_supply = 5.0;
|
||||
driver.init();
|
||||
motor.linkDriver(&driver);
|
||||
motor.useMonitoring(Serial);
|
||||
motor.controller = MotionControlType::angle;
|
||||
motor.foc_modulation = FOCModulationType::SinePWM;
|
||||
motor.voltage_limit = 1.5;
|
||||
motor.voltage_sensor_align = 0.8;
|
||||
motor.PID_velocity.P = 0.075f;
|
||||
motor.PID_velocity.I = 0.01;
|
||||
motor.PID_velocity.D = 0.0;
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
motor.P_angle.P = 120.0;
|
||||
motor.P_angle.I = 10.0;
|
||||
motor.velocity_limit = 50;
|
||||
|
||||
motor.useMonitoring(Serial);
|
||||
driver1.voltage_power_supply = 7.1;
|
||||
driver1.init();
|
||||
motor1.linkDriver(&driver1);
|
||||
motor1.useMonitoring(Serial);
|
||||
motor1.controller = MotionControlType::angle;
|
||||
motor1.foc_modulation = FOCModulationType::SinePWM;
|
||||
motor1.voltage_limit = 1.0;
|
||||
motor1.voltage_sensor_align = 1.0;
|
||||
motor1.PID_velocity.P = 0.05f;
|
||||
motor1.PID_velocity.I = 0.01;
|
||||
motor1.PID_velocity.D = 0.0;
|
||||
motor1.LPF_velocity.Tf = 0.01f;
|
||||
motor1.P_angle.P = 150.0;
|
||||
motor1.P_angle.I = 10.0;
|
||||
motor1.velocity_limit = 25;
|
||||
|
||||
// Init sensor
|
||||
motor.init();
|
||||
motor1.init();
|
||||
Serial.println("calibrating sensor in open loop...");
|
||||
sensor.init();
|
||||
sensor1.init();
|
||||
Serial.println("Done");
|
||||
delay(1000);
|
||||
|
||||
motor.linkSensor(&sensor);
|
||||
motor.init();
|
||||
motor1.linkSensor(&sensor1);
|
||||
motor1.init();
|
||||
// motor.initFOC(5.48, CCW);
|
||||
motor.initFOC();
|
||||
motor1.initFOC();
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
serialLoop();
|
||||
motor.move(target);
|
||||
motor.loopFOC();
|
||||
motor.monitor();
|
||||
motor1.move(target);
|
||||
motor1.loopFOC();
|
||||
motor1.monitor();
|
||||
}
|
Loading…
Reference in New Issue
Block a user