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

motor1 ok

Browse Source
This commit is contained in:
Lucien RENAUD 2022-10-05 18:27:39 +02:00
parent 3b206b7e3f
commit 6359978061
2 changed files with 73 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/linearHallSensor.cpp
View File

@ -67,19 +67,19 @@ void LinearHallSensor::init(BLDCMotor motor)
MotionControlType prevController = motor.controller;
motor.controller = MotionControlType::angle_openloop;
float prevVoltageLimit = motor.voltage_limit;
motor.voltage_limit = 0.8;
motor.voltage_limit = 1.5;
// 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 = 2;
float currentPosition = 0.0;
uint32_t senseA[30];
uint32_t senseB[30];
int N = 30;
const uint8_t N = 40;
uint32_t senseA[N];
uint32_t senseB[N];
int ptr = 0;
init_arr(senseA, N, 0);
@ -117,9 +117,8 @@ void LinearHallSensor::init(BLDCMotor motor)
// Move to new position
currentPosition += step;
motor.move(currentPosition);
delay(2);
delay(1);
}
// _maxPositionEndValue = currentPosition - (step * nCheck);
_maxPositionEndValue = currentPosition - (step * (ptr > N ? N : ptr));
currentPosition = _maxPositionEndValue - M_PI / 8;
delay(100);
@ -158,7 +157,7 @@ void LinearHallSensor::init(BLDCMotor motor)
// Move to new position
currentPosition -= step;
motor.move(currentPosition);
delay(2);
delay(1);
}
_minPositionEndValue = currentPosition + (step * (ptr > N ? N : ptr));
Serial.println("\t- Found second end stop : Min position = " + String(_minPositionEndValue));
@ -175,7 +174,7 @@ void LinearHallSensor::init(BLDCMotor motor)
Serial.println("\t- Offset: " + String(_offset));
// Read max angle
for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.005)
for (float i = _minPositionEndValue; i <= _maxPositionEndValue; i = i + 0.0025)
{
motor.move(i);
delay(1);
@ -189,10 +188,10 @@ void LinearHallSensor::init(BLDCMotor motor)
sum += readSensorCallback();
_maxSensor = sum / 50.0;
Serial.println("\t- Max sensor: " + String(_maxSensor));
Serial.println("\t- Max angle: " + String(_maxSensor));
// Recovering previous controller type and go in the middle
for (float i = _maxPositionEndValue; i >= (_maxPositionEndValue + _minPositionEndValue) / 2.0; i = i - 0.01)
for (float i = _maxPositionEndValue; i >= (_maxPositionEndValue + _minPositionEndValue); i = i - 0.005)
{
motor.move(i);
delay(1);

69
src/main.cpp
View File

@ -7,6 +7,10 @@ BLDCMotor motor1 = BLDCMotor(4);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3);
LinearHallSensor linearSensor1 = LinearHallSensor(M3_Hall1, M3_Hall2);
BLDCMotor motor2 = BLDCMotor(4);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(M1_PWM2, M1_PWM1, M1_PWM3);
LinearHallSensor linearSensor2 = LinearHallSensor(M1_Hall1, M1_Hall2);
void initSensor1()
{
linearSensor1.init(motor1);
@ -16,7 +20,17 @@ float callback1()
return linearSensor1.readSensorCallback();
}
void initSensor2()
{
linearSensor2.init(motor2);
}
float callback2()
{
return linearSensor2.readSensorCallback();
}
GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
GenericSensor sensor2 = GenericSensor(callback2, initSensor2);
float targetX = 0.0;
float targetY = 0.0;
@ -49,14 +63,17 @@ void setup()
{
Serial.begin(115200);
delay(5000);
delay(3000);
Serial.println("INIT");
pinMode(LED_BUILTIN, OUTPUT); // Lightup LED
digitalWrite(LED_BUILTIN, LOW);
driver1.voltage_power_supply = 7.1;
driver1.init();
// ### MOTOR 1 ###
Serial.println("\n\t\t### MOTOR 1 ###\n");
driver1.voltage_power_supply = 7.0;
Serial.println("Driver1 init: " + String(driver1.init()));
Serial.println("Driver2 init: " + String(driver2.init()));
motor1.linkDriver(&driver1);
motor1.useMonitoring(Serial);
motor1.controller = MotionControlType::angle;
@ -71,17 +88,53 @@ void setup()
motor1.P_angle.I = 10.0;
motor1.velocity_limit = 25;
// Init sensor
// Init sensor 1
motor1.init();
Serial.println("calibrating sensor in open loop...");
sensor1.init();
Serial.println("Done");
Serial.println("Sensor 1 done");
delay(1000);
motor1.linkSensor(&sensor1);
motor1.init();
motor1.initFOC(2.98, CW);
// motor1.initFOC();
Serial.println("Motor 1 Done");
motor1.disable();
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);
motor1.initFOC();
motor2.initFOC();
Serial.println("Motor 2 Done");
}
void loop()
@ -90,4 +143,8 @@ void loop()
motor1.move(target);
motor1.loopFOC();
motor1.monitor();
motor2.move(target);
motor2.loopFOC();
motor2.monitor();
}