From 635997806158b335e33e18757f53128d536b59d0 Mon Sep 17 00:00:00 2001 From: Lucien RENAUD Date: Wed, 5 Oct 2022 18:27:39 +0200 Subject: [PATCH] motor1 ok --- src/linearHallSensor.cpp | 21 ++++++------ src/main.cpp | 69 ++++++++++++++++++++++++++++++++++++---- 2 files changed, 73 insertions(+), 17 deletions(-) diff --git a/src/linearHallSensor.cpp b/src/linearHallSensor.cpp index c574239..0202af4 100644 --- a/src/linearHallSensor.cpp +++ b/src/linearHallSensor.cpp @@ -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); diff --git a/src/main.cpp b/src/main.cpp index 6ab9c20..f82fb70 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -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(); } \ No newline at end of file