From 77896cfc559033aca4182482b8bd4ee7add96cb4 Mon Sep 17 00:00:00 2001 From: Lucien Renaud Date: Tue, 23 Aug 2022 13:45:39 +0200 Subject: [PATCH] Change pins --- src/main.cpp | 130 +++++++++++++++++++++++++++++---------------------- src/pinout.h | 29 ++++++++++++ 2 files changed, 104 insertions(+), 55 deletions(-) create mode 100644 src/pinout.h diff --git a/src/main.cpp b/src/main.cpp index e646aea..f060e1a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,20 +1,21 @@ #include #include #include +#include BLDCMotor motor = BLDCMotor(4); -BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 9); +BLDCDriver3PWM driver = BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3); -LinearHallSensor linearSensor = LinearHallSensor(A0, A1, -0.75, 2.35); +LinearHallSensor linearSensor = LinearHallSensor(M2_Hall1, M2_Hall2, -0.75, 2.35); void initSensor() { - linearSensor.init(motor); + linearSensor.init(motor); } float callback() { - return linearSensor.readSensorCallback(); + return linearSensor.readSensorCallback(); } GenericSensor sensor = GenericSensor(callback, initSensor); @@ -26,73 +27,92 @@ float target = 0.0; void serialLoop() { - static String received_chars; - while (Serial.available()) - { - char inChar = (char)Serial.read(); - received_chars += inChar; - if (inChar == '\n') + static String received_chars; + while (Serial.available()) { - target = received_chars.toFloat(); - Serial.print("Target = "); - Serial.print(target); - received_chars = ""; + char inChar = (char)Serial.read(); + received_chars += inChar; + if (inChar == '\n') + { + target = received_chars.toFloat(); + Serial.print("Target = "); + Serial.print(target); + received_chars = ""; + } } - } } 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; + return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min; } void setup() { - Serial.begin(115200); - _delay(100); - Serial.println("INIT"); - pinMode(3, OUTPUT); - pinMode(5, OUTPUT); - pinMode(6, OUTPUT); - digitalWrite(3, LOW); - digitalWrite(5, LOW); - digitalWrite(6, LOW); - driver.voltage_power_supply = 6.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; + Serial.begin(115200); + delay(2000); + Serial.println("INIT"); - motor.useMonitoring(Serial); + // pinMode(M1_PWM1, OUTPUT); + // pinMode(M1_PWM2, OUTPUT); + // pinMode(M1_PWM3, OUTPUT); + // digitalWrite(M1_PWM1, LOW); + // digitalWrite(M1_PWM2, LOW); + // digitalWrite(M1_PWM3, LOW); - // Init sensor - motor.init(); - Serial.println("calibrating sensor in open loop..."); - sensor.init(); - Serial.println("Done"); - delay(1000); + // pinMode(M2_PWM1, OUTPUT); + // pinMode(M2_PWM2, OUTPUT); + // pinMode(M2_PWM3, OUTPUT); + // digitalWrite(M2_PWM1, LOW); + // digitalWrite(M2_PWM2, LOW); + // digitalWrite(M2_PWM3, LOW); - motor.linkSensor(&sensor); - motor.init(); - // motor.initFOC(5.48, CCW); - motor.initFOC(); + // 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); + + // Init sensor + motor.init(); + Serial.println("calibrating sensor in open loop..."); + sensor.init(); + Serial.println("Done"); + delay(1000); + + motor.linkSensor(&sensor); + motor.init(); + // motor.initFOC(5.48, CCW); + motor.initFOC(); } void loop() { - serialLoop(); - motor.move(target); - motor.loopFOC(); - motor.monitor(); + serialLoop(); + motor.move(target); + motor.loopFOC(); + motor.monitor(); } \ No newline at end of file diff --git a/src/pinout.h b/src/pinout.h new file mode 100644 index 0000000..c28e5c7 --- /dev/null +++ b/src/pinout.h @@ -0,0 +1,29 @@ +#ifndef pinout_h +#define pinout_h + +#include "Arduino.h" + +// PCB board : STM32 DJI Gimbal V1.0 + +#define M1_PWM1 PB13 +#define M1_PWM2 PB14 +#define M1_PWM3 PB15 +#define M1_Fault PB12 +#define M1_Hall1 PB1 +#define M1_Hall2 PB0 + +#define M2_PWM1 PB9 +#define M2_PWM2 PB8 +#define M2_PWM3 PB5 +#define M2_Fault PB10 +#define M2_Hall1 PA4 +#define M2_Hall2 PA3 + +#define M3_PWM1 PA5 +#define M3_PWM2 PA6 +#define M3_PWM3 PA7 +#define M3_Fault PA0 +#define M3_Hall1 PA2 +#define M3_Hall2 PA1 + +#endif \ No newline at end of file