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

3 motors working

Browse Source
This commit is contained in:
Lucien RENAUD 2022-10-06 17:33:11 +02:00
parent 74a3c59dbd
commit c3022bbe5f
1 changed files with 54 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
src/main.cpp
View File

@ -4,12 +4,16 @@
#include <pinout.h> #include <pinout.h>
BLDCMotor motor1 = BLDCMotor(4); BLDCMotor motor1 = BLDCMotor(4);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3); BLDCDriver3PWM driver1 = BLDCDriver3PWM(M1_PWM1, M1_PWM2, M1_PWM3);
LinearHallSensor linearSensor1 = LinearHallSensor(M3_Hall1, M3_Hall2); LinearHallSensor linearSensor1 = LinearHallSensor(M1_Hall1, M1_Hall2);
BLDCMotor motor2 = BLDCMotor(4); BLDCMotor motor2 = BLDCMotor(4);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(M1_PWM2, M1_PWM1, M1_PWM3); BLDCDriver3PWM driver2 = BLDCDriver3PWM(M2_PWM2, M2_PWM1, M2_PWM3);
LinearHallSensor linearSensor2 = LinearHallSensor(M1_Hall1, M1_Hall2); LinearHallSensor linearSensor2 = LinearHallSensor(M2_Hall1, M2_Hall2);
BLDCMotor motor3 = BLDCMotor(4);
BLDCDriver3PWM driver3 = BLDCDriver3PWM(M3_PWM2, M3_PWM1, M3_PWM3);
LinearHallSensor linearSensor3 = LinearHallSensor(M3_Hall1, M3_Hall2);
void initSensor1() void initSensor1()
{ {
@ -29,8 +33,18 @@ float callback2()
return linearSensor2.readSensorCallback(); return linearSensor2.readSensorCallback();
} }
void initSensor3()
{
linearSensor3.init(motor3);
}
float callback3()
{
return linearSensor3.readSensorCallback();
}
GenericSensor sensor1 = GenericSensor(callback1, initSensor1); GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
GenericSensor sensor2 = GenericSensor(callback2, initSensor2); GenericSensor sensor2 = GenericSensor(callback2, initSensor2);
GenericSensor sensor3 = GenericSensor(callback3, initSensor3);
float targetX = 0.0; float targetX = 0.0;
float targetY = 0.0; float targetY = 0.0;
@ -73,7 +87,6 @@ void setup()
Serial.println("\n\t\t### MOTOR 1 ###\n"); Serial.println("\n\t\t### MOTOR 1 ###\n");
driver1.voltage_power_supply = 7.0; driver1.voltage_power_supply = 7.0;
Serial.println("Driver1 init: " + String(driver1.init())); Serial.println("Driver1 init: " + String(driver1.init()));
Serial.println("Driver2 init: " + String(driver2.init()));
motor1.linkDriver(&driver1); motor1.linkDriver(&driver1);
motor1.useMonitoring(Serial); motor1.useMonitoring(Serial);
motor1.controller = MotionControlType::angle; motor1.controller = MotionControlType::angle;
@ -100,7 +113,6 @@ void setup()
motor1.initFOC(2.98, CW); motor1.initFOC(2.98, CW);
// motor1.initFOC(); // motor1.initFOC();
Serial.println("Motor 1 Done"); Serial.println("Motor 1 Done");
motor1.disable();
delay(1000); delay(1000);
@ -108,7 +120,7 @@ void setup()
Serial.println("\n\t\t### MOTOR 2 ###\n"); Serial.println("\n\t\t### MOTOR 2 ###\n");
motor2.useMonitoring(Serial); motor2.useMonitoring(Serial);
driver2.voltage_power_supply = 7.0; driver2.voltage_power_supply = 7.0;
// Serial.println("Driver init: " + String(driver2.init())); Serial.println("Driver init: " + String(driver2.init()));
motor2.linkDriver(&driver2); motor2.linkDriver(&driver2);
motor2.controller = MotionControlType::angle; motor2.controller = MotionControlType::angle;
@ -135,6 +147,41 @@ void setup()
// motor.initFOC(5.48, CCW); // motor.initFOC(5.48, CCW);
motor2.initFOC(); motor2.initFOC();
Serial.println("Motor 2 Done"); 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);
} }
void loop() void loop()