lurenaud/DJI-Gimbal-FOC
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Closed loop 3 axis OK !

Browse Source
This commit is contained in:
Lucien RENAUD
2022-11-07 11:09:23 +01:00
parent 413894315a
commit 0d7631cfd8
9 changed files with 347 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/linearHallSensor.cpp
View File

@@ -67,19 +67,18 @@ void LinearHallSensor::init(BLDCMotor motor)
MotionControlType prevController = motor.controller;
motor.controller = MotionControlType::angle_openloop;
float prevVoltageLimit = motor.voltage_limit;
motor.voltage_limit = 1.5;
motor.voltage_limit = 2.0;
// 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 epsilon = 2;
const float step = 0.005;
const uint8_t epsilon = 3;
float currentPosition = 0.0;
const uint8_t N = 40;
const uint8_t N = 20;
uint32_t senseA[N];
uint32_t senseB[N];
int ptr = 0;
init_arr(senseA, N, 0);
@@ -104,8 +103,9 @@ void LinearHallSensor::init(BLDCMotor motor)
_maxCh1 = senseA[ptr];
else if (senseA[ptr] < _minCh1)
_minCh1 = senseA[ptr];
if (senseB[ptr] > _maxCh1)
_maxCh1 = senseB[ptr];
if (senseB[ptr] > _maxCh2)
_maxCh2 = senseB[ptr];
else if (senseB[ptr] < _minCh2)
_minCh2 = senseB[ptr];
@@ -117,10 +117,10 @@ void LinearHallSensor::init(BLDCMotor motor)
// Move to new position
currentPosition += step;
motor.move(currentPosition);
delay(1);
delay(3);
}
_maxPositionEndValue = currentPosition - (step * (ptr > N ? N : ptr));
currentPosition = _maxPositionEndValue - M_PI / 8;
currentPosition = _maxPositionEndValue - M_PI / 16;
delay(100);
motor.move(currentPosition);
Serial.println("\t- Found first end stop : Max position = " + String(_maxPositionEndValue));
@@ -128,7 +128,6 @@ void LinearHallSensor::init(BLDCMotor motor)
// Swipe motor to search other hard end, and find eventually new max analog values of sensors
endFound = false;
currentCheck = 0;
init_arr(senseA, N, 0);
init_arr(senseB, N, 0);
@@ -144,8 +143,9 @@ void LinearHallSensor::init(BLDCMotor motor)
_maxCh1 = senseA[ptr];
else if (senseA[ptr] < _minCh1)
_minCh1 = senseA[ptr];
if (senseB[ptr] > _maxCh1)
_maxCh1 = senseB[ptr];
if (senseB[ptr] > _maxCh2)
_maxCh2 = senseB[ptr];
else if (senseB[ptr] < _minCh2)
_minCh2 = senseB[ptr];
@@ -157,7 +157,7 @@ void LinearHallSensor::init(BLDCMotor motor)
// Move to new position
currentPosition -= step;
motor.move(currentPosition);
delay(1);
delay(3);
}
_minPositionEndValue = currentPosition + (step * (ptr > N ? N : ptr));
Serial.println("\t- Found second end stop : Min position = " + String(_minPositionEndValue));
@@ -168,7 +168,7 @@ void LinearHallSensor::init(BLDCMotor motor)
_offset = 0.0;
for (uint8_t i = 0; i <= 49; i++)
sum += readSensorCallback();
_offset = -sum / 50.0;
_offset = sum / 50.0;
Serial.println("\t- maxA: " + String(_maxCh1) + "\tminA: " + String(_minCh1) + "\tmaxB: " + String(_maxCh2) + "\tminB: " + String(_minCh2));
Serial.println("\t- Offset: " + String(_offset));
@@ -190,8 +190,8 @@ void LinearHallSensor::init(BLDCMotor motor)
Serial.println("\t- Max angle: " + String(_maxSensor));
// Recovering previous controller type and go in the middle
for (float i = _maxPositionEndValue; i >= (_maxPositionEndValue + _minPositionEndValue); i = i - 0.005)
// Recovering previous controller type and go to 0
for (float i = _maxPositionEndValue; i >= (0); i = i - 0.005)
{
motor.move(i);
delay(1);
@@ -225,5 +225,10 @@ float LinearHallSensor::readSensorCallback()
_estimatePosition += dist_angle(_currentPosition, _prevPosition);
_prevPosition = _currentPosition;
return (_estimatePosition + _offset) / 4.0;
return (_estimatePosition / 4.0 - _offset);
}
float LinearHallSensor::getMaxAngle()
{
return _maxSensor;
}

10
src/linearHallSensor.h
View File

@@ -34,6 +34,14 @@ class LinearHallSensor
float readSensorCallbackStateMachine();
float readSensorCallback();
/**
* @brief retrun the max angle of the motor
*
* @return max angle in rad
*/
float getMaxAngle();
private:
uint32_t _analogPin1;
@@ -49,7 +57,7 @@ class LinearHallSensor
float _currentPosition;
float _estimatePosition; // current position + offset
float _offset; // offset angle measured at init
float _maxSensor; // Max angle value measured (so command from 0 to that value)
float _maxSensor; // Max range angle value measured (so command from 0 to that value)
float _minPositionEndValue; // min pos in open loop mode
float _maxPositionEndValue;

253
src/main.cpp
View File

@@ -3,53 +3,60 @@
#include <linearHallSensor.h>
#include <pinout.h>
BLDCMotor motor1 = BLDCMotor(4);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(M1_PWM1, M1_PWM2, M1_PWM3);
LinearHallSensor linearSensor1 = LinearHallSensor(M1_Hall1, M1_Hall2);
BLDCMotor motor[3] =
{
BLDCMotor(4),
BLDCMotor(4),
BLDCMotor(4)};
BLDCMotor motor2 = BLDCMotor(4);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(M2_PWM2, M2_PWM1, M2_PWM3);
LinearHallSensor linearSensor2 = LinearHallSensor(M2_Hall1, M2_Hall2);
BLDCDriver3PWM driver[3] =
{
BLDCDriver3PWM(M1_PWM1, M1_PWM2, M1_PWM3),
BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3),
BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3)};
BLDCMotor motor3 = BLDCMotor(4);
BLDCDriver3PWM driver3 = BLDCDriver3PWM(M3_PWM2, M3_PWM1, M3_PWM3);
LinearHallSensor linearSensor3 = LinearHallSensor(M3_Hall1, M3_Hall2);
LinearHallSensor linearSensor[3] =
{
LinearHallSensor(M1_Hall1, M1_Hall2),
LinearHallSensor(M2_Hall1, M2_Hall2),
LinearHallSensor(M3_Hall1, M3_Hall2)};
void initSensor0()
{
linearSensor[0].init(motor[0]);
}
float callback0()
{
return linearSensor[0].readSensorCallback();
}
void initSensor1()
{
linearSensor1.init(motor1);
linearSensor[1].init(motor[1]);
}
float callback1()
{
return linearSensor1.readSensorCallback();
return linearSensor[1].readSensorCallback();
}
void initSensor2()
{
linearSensor2.init(motor2);
linearSensor[2].init(motor[2]);
}
float callback2()
{
return linearSensor2.readSensorCallback();
return linearSensor[2].readSensorCallback();
}
void initSensor3()
{
linearSensor3.init(motor3);
}
float callback3()
{
return linearSensor3.readSensorCallback();
}
GenericSensor sensor[3] =
{
GenericSensor(callback0, initSensor0),
GenericSensor(callback1, initSensor1),
GenericSensor(callback2, initSensor2)};
GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
GenericSensor sensor2 = GenericSensor(callback2, initSensor2);
GenericSensor sensor3 = GenericSensor(callback3, initSensor3);
float target[3] = {0.5, 0.5, 0.5};
float targetX = 0.0;
float targetY = 0.0;
float target = 0.0;
String str;
void serialLoop()
{
@@ -60,9 +67,9 @@ void serialLoop()
received_chars += inChar;
if (inChar == '\n')
{
target = received_chars.toFloat();
target[0] = received_chars.toFloat();
Serial.print("Target = ");
Serial.print(target);
Serial.print(target[0]);
received_chars = "";
}
}
@@ -73,9 +80,46 @@ float mapfloat(float x, float in_min, float in_max, float out_min, float out_max
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}
void initMotor(u_int8_t m)
{
Serial.printf("\n\t\t### MOTOR %d ###\n\n", m + 1);
driver[m].voltage_power_supply = 7.0;
driver[m].pwm_frequency = 50000;
Serial.printf("Driver%d init: %d\n", m + 1, driver[m].init());
motor[m].linkDriver(&driver[m]);
motor[m].useMonitoring(Serial);
motor[m].controller = MotionControlType::angle;
motor[m].foc_modulation = FOCModulationType::SinePWM;
motor[m].voltage_limit = 2.0;
motor[m].voltage_sensor_align = 2.0;
motor[m].PID_velocity.P = 0.05f;
motor[m].PID_velocity.I = 0.008;
motor[m].PID_velocity.D = 0.0;
motor[m].LPF_velocity.Tf = 0.02f;
motor[m].P_angle.P = 150.0;
motor[m].P_angle.I = 5.0;
motor[m].velocity_limit = 20;
// Init sensor
motor[m].init();
Serial.println("calibrating sensor in open loop...");
sensor[m].init();
Serial.printf("Sensor %d done\n", m + 1);
motor[m].linkSensor(&sensor[m]);
motor[m].init();
if (m == 2)
motor[m].initFOC(3.0, CW);
else if (m == 1)
motor[m].initFOC(3.79, CW);
// motor[m].initFOC();
else
motor[m].initFOC(0.18, CW);
Serial.printf("Motor %d Done\n", m + 1);
}
void setup()
{
Serial.begin(115200);
delay(3000);
Serial.println("INIT");
@@ -83,115 +127,50 @@ void setup()
pinMode(LED_BUILTIN, OUTPUT); // Lightup LED
digitalWrite(LED_BUILTIN, LOW);
// ### MOTOR 1 ###
Serial.println("\n\t\t### MOTOR 1 ###\n");
driver1.voltage_power_supply = 7.0;
Serial.println("Driver1 init: " + String(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 1
motor1.init();
Serial.println("calibrating sensor in open loop...");
sensor1.init();
Serial.println("Sensor 1 done");
delay(1000);
motor1.linkSensor(&sensor1);
motor1.init();
motor1.initFOC(2.98, CW);
// motor1.initFOC();
Serial.println("Motor 1 Done");
delay(1000);
// ### MOTOR 2 ###
Serial.println("\n\t\t### MOTOR 2 ###\n");
motor2.useMonitoring(Serial);
driver2.voltage_power_supply = 7.0;
Serial.println("Driver init: " + String(driver2.init()));
motor2.linkDriver(&driver2);
motor2.controller = MotionControlType::angle;
motor2.foc_modulation = FOCModulationType::SinePWM;
motor2.voltage_limit = 1.0;
motor2.voltage_sensor_align = 1.0;
motor2.PID_velocity.P = 0.05f;
motor2.PID_velocity.I = 0.01;
motor2.PID_velocity.D = 0.0;
motor2.LPF_velocity.Tf = 0.01f;
motor2.P_angle.P = 150.0;
motor2.P_angle.I = 10.0;
motor2.velocity_limit = 25;
// Init sensor 2
motor2.init();
Serial.println("calibrating sensor 2 in open loop...");
sensor2.init();
Serial.println("Sensor 2 done");
delay(1000);
motor2.linkSensor(&sensor2);
motor2.init();
// motor.initFOC(5.48, CCW);
motor2.initFOC();
Serial.println("Motor 2 Done");
delay(1000);
// ### MOTOR 3 ###
Serial.println("\n\t\t### MOTOR 3 ###\n");
motor3.useMonitoring(Serial);
driver3.voltage_power_supply = 7.0;
Serial.println("Driver init: " + String(driver3.init()));
motor3.linkDriver(&driver3);
motor3.controller = MotionControlType::angle;
motor3.foc_modulation = FOCModulationType::SinePWM;
motor3.voltage_limit = 1.0;
motor3.voltage_sensor_align = 1.0;
motor3.PID_velocity.P = 0.05f;
motor3.PID_velocity.I = 0.01;
motor3.PID_velocity.D = 0.0;
motor3.LPF_velocity.Tf = 0.01f;
motor3.P_angle.P = 150.0;
motor3.P_angle.I = 10.0;
motor3.velocity_limit = 25;
// Init sensor 2
motor3.init();
Serial.println("calibrating sensor 2 in open loop...");
sensor3.init();
Serial.println("Sensor 2 done");
delay(1000);
motor3.linkSensor(&sensor3);
motor3.init();
// motor.initFOC(5.48, CCW);
motor3.initFOC();
Serial.println("Motor 3 Done");
while(1);
motor[1].useMonitoring(Serial);
initMotor(2);
initMotor(1);
initMotor(0);
}
void loop()
{
serialLoop();
motor1.move(target);
motor1.loopFOC();
motor1.monitor();
int len = Serial.available();
if (len > 0)
{
str = Serial.readStringUntil('\n'); // get new targets from serial (target are from 0 to 1000)
char axis = str[0];
str.remove(0, 1);
str.remove(len,1);
switch (axis)
{
case 'X':
target[1] = str.toFloat();
// Serial.println("Target X: " + String(target[1]));
target[1] = mapfloat(target[1], 0.0, 1000.0, 0.0, linearSensor[1].getMaxAngle()); // remap targets to motor angles
break;
case 'Y':
target[0] = str.toFloat();
// Serial.println("Target Y: " + String(target[0]));
target[0] = mapfloat(target[0], 0.0, 1000.0, 0.0, linearSensor[0].getMaxAngle()); // remap targets to motor angles
break;
case 'Z':
target[2] = str.toFloat();
// Serial.println("Target Z: " + String(target[2]));
target[2] = mapfloat(target[2], 0.0, 1000.0, 0.0, linearSensor[2].getMaxAngle()); // remap targets to motor angles
break;
default:
break;
}
}
motor2.move(target);
motor2.loopFOC();
motor2.monitor();
for (u_int8_t i = 0; i < 3; i++) // update motor target and Run FOC
{
// serialLoop();
// Serial.println("Target" + String(i) + ": " + String(target[i]));
motor[i].move(target[i]);
motor[i].loopFOC();
// motor[i].monitor();
}
}

8
src/pinout.h
View File

@@ -5,9 +5,9 @@
// PCB board : STM32 DJI Gimbal V1.0
#define M1_PWM1 PA8
#define M1_PWM1 PA10
#define M1_PWM2 PA9
#define M1_PWM3 PA10
#define M1_PWM3 PA8
#define M1_Fault PB12
#define M1_Hall1 PB1
#define M1_Hall2 PA5
@@ -20,8 +20,8 @@
#define M2_Hall2 PA3
#define M3_PWM1 PB0
#define M3_PWM2 PA6
#define M3_PWM3 PA7
#define M3_PWM2 PA7
#define M3_PWM3 PA6
#define M3_Fault PA0
#define M3_Hall1 PA2
#define M3_Hall2 PA1