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

Change pins

Browse Source
This commit is contained in:
Lucien Renaud 2022-08-23 13:45:39 +02:00
parent ba62b3987b
commit 77896cfc55
2 changed files with 104 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
src/main.cpp
View File

@ -1,20 +1,21 @@
#include <Arduino.h> #include <Arduino.h>
#include <SimpleFOC.h> #include <SimpleFOC.h>
#include <linearHallSensor.h> #include <linearHallSensor.h>
#include <pinout.h>
BLDCMotor motor = BLDCMotor(4); 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() void initSensor()
{ {
linearSensor.init(motor); linearSensor.init(motor);
} }
float callback() float callback()
{ {
return linearSensor.readSensorCallback(); return linearSensor.readSensorCallback();
} }
GenericSensor sensor = GenericSensor(callback, initSensor); GenericSensor sensor = GenericSensor(callback, initSensor);
@ -26,73 +27,92 @@ float target = 0.0;
void serialLoop() void serialLoop()
{ {
static String received_chars; static String received_chars;
while (Serial.available()) while (Serial.available())
{
char inChar = (char)Serial.read();
received_chars += inChar;
if (inChar == '\n')
{ {
target = received_chars.toFloat(); char inChar = (char)Serial.read();
Serial.print("Target = "); received_chars += inChar;
Serial.print(target); if (inChar == '\n')
received_chars = ""; {
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) 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() 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; Serial.begin(115200);
driver.init(); delay(2000);
motor.linkDriver(&driver); Serial.println("INIT");
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); // 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 // pinMode(M2_PWM1, OUTPUT);
motor.init(); // pinMode(M2_PWM2, OUTPUT);
Serial.println("calibrating sensor in open loop..."); // pinMode(M2_PWM3, OUTPUT);
sensor.init(); // digitalWrite(M2_PWM1, LOW);
Serial.println("Done"); // digitalWrite(M2_PWM2, LOW);
delay(1000); // digitalWrite(M2_PWM3, LOW);
motor.linkSensor(&sensor); // pinMode(M3_PWM1, OUTPUT);
motor.init(); // pinMode(M3_PWM2, OUTPUT);
// motor.initFOC(5.48, CCW); // pinMode(M3_PWM3, OUTPUT);
motor.initFOC(); // 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() void loop()
{ {
serialLoop(); serialLoop();
motor.move(target); motor.move(target);
motor.loopFOC(); motor.loopFOC();
motor.monitor(); motor.monitor();
} }

29
src/pinout.h Normal file
View File

@ -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