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

Closed loop 3 axis OK !

Browse Source
This commit is contained in:
Lucien RENAUD
2022-11-07 11:09:23 +01:00
parent 413894315a
commit 0d7631cfd8
9 changed files with 347 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
docs/LinearHallEncoder.md
View File

@@ -1,98 +0,0 @@
# DJI Gimbal Retro-Engineering
The aim of this project is to be able to use the 3-axis DJI gimbal with a custom open source controller. This high quality gimbal is very tiny and easy to find as replacement part which makes it very suitable for DIY projects.
## Description
todo
## Pinout identification
The Gimbal is composed of a flex PCB with a main connector and 3 smaller for each motor. The main end connector is a 40-pin mezzanine board to board connectors. In order to work easily I have designed a breakout board which open to a 2.54" header.
Here is the strategy I followed to find the pinout:
1. Find all equipotential pins:
With a multimeter set to continuity tests, and test all the combinations
2. Group remaining pins by motor
With the multimeter find all the pins connected to the motor connector. (Reapeat 3 times)
3.
### Open-loop control
Each motor has its own drivers a MP6536. Which makes it easy as no additional hardware is necessary to drive the motors.
There are 4 pins from the MP6536:
1. PWM1
2. PWM2
3. PWM3
4. Fault : Output. When low, indicates overtemperature, over-current, or under-voltage.
Connected directly to a MCU and with the Simple FOC Library, open-loop control works quite well. However due to open-loop control, it cannot know when a "step" is missed so misalignment can occur. Also, the motor tends to become quite hot due to the continuous current sent to the coils.
## Position estimation with the integrated linear hall sensors
### 1. Setup
Each motor is composed of two ratiometric linear hall sensors. (Texas Instrument DRV5053 Analog-Bipolar Hall Effect Sensor) They are placed at around 120º from each other (eyes measured) and measure the magnetic field of the rotor.
![Photo of the stator](Hallmotor.jpg)
Ratiometric means that the output signal is proportional to the voltage supply to the sensor. In this setup, with 5V supply, the output measured is between 520mV and 1.5V, so a 1V amplitude.
### 2. Measures
These oscilloscope traces are the sensor output when rotating the rotor forth and back. (a bit less than 180º on the 3rd motor)
The channel 0 (Yellow) is the Hall 1 and the Channel 1 (Green) is the Hall 2
![hall sensors traces](courbes.png)
We can see that in the first movement (positive rotation), the green is out of phase of π/2.`
![Sinwave figure](cosSinEncoderDiagram.png)
### 3. Encoding the position
1. Get the absolute angle within a period
Since the 2 signals correspond to a cos and sin signals, it is possible to compute the angle inside the period using arctan2 function. However, we have more than one period, it is so necessary to increment a position.
$$\theta= atan2(a,b)$$
2. Incremental position
To increment the position, it is necessary to start from 0 at a known postion. For that the motor is moved in open loop to one end and the position is set to 0.
Then we need to sum all the delta of movement at each measure sample.
$$\phi_t=\phi_{t-1} + (\theta_t - \theta_{t-1})mod(-\pi;\pi)$$
## Coding the solution
1. Get the angle in the perdiod
In order to compute the angle from the cos and sin with atan, it is necessary to remap the values of the analog readings from -1 to 1.
Beforehand, the maximum and minimum peak of the signals need to be found. It can be done by swiping the motor on startup in open-loop mode.
Then the arctan function can be applied. It is preferable to use arctan2 as it will give an angle within the 4 quadrants (-π,π). Whereas arctan give an angle between (-π/2,π/2). [Wikipedia](https://en.wikipedia.org/wiki/Atan2)
```C++
float LinearHallSensor::Callback() // Return the estimated position of the sensor
{
A = norm(analogRead(CH1),minCh1, maxCh1); //read analog values and normalise between [-1;1]
B = norm(analogRead(CH2),minCh2, maxCh2);
theta = atan2(A,B); // Compute the absolute angle in the period
phi = phi + dist_angle(theta, theta_prev); // increment the difference
theta_prev = theta; // save fot nex time
return phi;
}
float norm(float x, float in_min, float in_max) //return the input value normalised between [-1;1]
{
return (float)(x + 1.0) * (2.0) / (float)(in_max - in_min) -1.0;
}
float dist_angle(float newAngle, float prevAngle) // return the difference modulo [-pi;pi]
{
float diff = newAngle - prevAngle;
while (diff < (-M_PI))
diff += 2 * M_PI;
while (diff > M_PI)
diff -= 2 * M_PI;
return diff;
}
```

5
platformio.ini
View File

@@ -27,9 +27,10 @@ lib_deps =
askuric/Simple FOC@^2.2.2
Wire
SPI
monitor_port = COM12
monitor_port = COM5
monitor_speed = 115200
build_flags =
-D PIO_FRAMEWORK_ARDUINO_ENABLE_CDC
-D USBCON
monitor_dtr = 1
; -D SIMPLEFOC_STM32_DEBUG
monitor_dtr = 1

43
pythonGUI/gimbal_gui.py Normal file
View File

@@ -0,0 +1,43 @@
import pygame
import serial
import time
ser = serial.Serial('COM5',115200, timeout=0.01)
print(ser.name)
pygame.init()
pygame.joystick.init()
pad = pygame.joystick.Joystick(0)
pad.init()
print(pad.get_name())
def remap(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
while(True):
pygame.event.pump()
X_axis = round(pad.get_axis(2),4)
Y_axis = round(pad.get_axis(3),4)
Z_axis = round(pad.get_axis(0),4)
# print(f'Axis 2: {X_axis}\tAxis 3: {Y_axis} \tAxis 0: {Z_axis}')
X_axis = round(remap(X_axis,-1,1,0,1000))
Y_axis = round(remap(Y_axis,-1,1,0,1000))
Z_axis = round(remap(Z_axis,-1,1,0,1000))
# print(f'Axis X: {X_axis}\tAxis Y: {Y_axis} \tAxis Z: {Z_axis}')
ser.write(('X'+str(X_axis)+'\n').encode())
ser.write(('Y'+str(Y_axis)+'\n').encode())
ser.write(('Z'+str(Z_axis)+'\n').encode())
income = ser.readline()
if (len(income)>0):
print(income)
# time.sleep(0.1)

149
pythonGUI/joystickTest.py Normal file
View File

@@ -0,0 +1,149 @@
import pygame
pygame.init()
# This is a simple class that will help us print to the screen.
# It has nothing to do with the joysticks, just outputting the
# information.
class TextPrint:
def __init__(self):
self.reset()
self.font = pygame.font.Font(None, 25)
def tprint(self, screen, text):
text_bitmap = self.font.render(text, True, (0, 0, 0))
screen.blit(text_bitmap, (self.x, self.y))
self.y += self.line_height
def reset(self):
self.x = 10
self.y = 10
self.line_height = 15
def indent(self):
self.x += 10
def unindent(self):
self.x -= 10
def main():
# Set the width and height of the screen (width, height), and name the window.
screen = pygame.display.set_mode((500, 700))
pygame.display.set_caption("Joystick example")
# Used to manage how fast the screen updates.
clock = pygame.time.Clock()
# Get ready to print.
text_print = TextPrint()
# This dict can be left as-is, since pygame will generate a
# pygame.JOYDEVICEADDED event for every joystick connected
# at the start of the program.
joysticks = {}
done = False
while not done:
# Event processing step.
# Possible joystick events: JOYAXISMOTION, JOYBALLMOTION, JOYBUTTONDOWN,
# JOYBUTTONUP, JOYHATMOTION, JOYDEVICEADDED, JOYDEVICEREMOVED
for event in pygame.event.get():
if event.type == pygame.QUIT:
done = True # Flag that we are done so we exit this loop.
if event.type == pygame.JOYBUTTONDOWN:
print("Joystick button pressed.")
if event.button == 0:
joystick = joysticks[event.instance_id]
if joystick.rumble(0, 0.7, 500):
print(f"Rumble effect played on joystick {event.instance_id}")
if event.type == pygame.JOYBUTTONUP:
print("Joystick button released.")
# Handle hotplugging
if event.type == pygame.JOYDEVICEADDED:
# This event will be generated when the program starts for every
# joystick, filling up the list without needing to create them manually.
joy = pygame.joystick.Joystick(event.device_index)
joysticks[joy.get_instance_id()] = joy
print(f"Joystick {joy.get_instance_id()} connencted")
if event.type == pygame.JOYDEVICEREMOVED:
del joysticks[event.instance_id]
print(f"Joystick {event.instance_id} disconnected")
# Drawing step
# First, clear the screen to white. Don't put other drawing commands
# above this, or they will be erased with this command.
screen.fill((255, 255, 255))
text_print.reset()
# Get count of joysticks.
joystick_count = pygame.joystick.get_count()
text_print.tprint(screen, f"Number of joysticks: {joystick_count}")
text_print.indent()
# For each joystick:
for joystick in joysticks.values():
jid = joystick.get_instance_id()
text_print.tprint(screen, f"Joystick {jid}")
text_print.indent()
# Get the name from the OS for the controller/joystick.
name = joystick.get_name()
text_print.tprint(screen, f"Joystick name: {name}")
guid = joystick.get_guid()
text_print.tprint(screen, f"GUID: {guid}")
power_level = joystick.get_power_level()
text_print.tprint(screen, f"Joystick's power level: {power_level}")
# Usually axis run in pairs, up/down for one, and left/right for
# the other. Triggers count as axes.
axes = joystick.get_numaxes()
text_print.tprint(screen, f"Number of axes: {axes}")
text_print.indent()
for i in range(axes):
axis = joystick.get_axis(i)
text_print.tprint(screen, f"Axis {i} value: {axis:>6.3f}")
text_print.unindent()
buttons = joystick.get_numbuttons()
text_print.tprint(screen, f"Number of buttons: {buttons}")
text_print.indent()
for i in range(buttons):
button = joystick.get_button(i)
text_print.tprint(screen, f"Button {i:>2} value: {button}")
text_print.unindent()
hats = joystick.get_numhats()
text_print.tprint(screen, f"Number of hats: {hats}")
text_print.indent()
# Hat position. All or nothing for direction, not a float like
# get_axis(). Position is a tuple of int values (x, y).
for i in range(hats):
hat = joystick.get_hat(i)
text_print.tprint(screen, f"Hat {i} value: {str(hat)}")
text_print.unindent()
text_print.unindent()
# Go ahead and update the screen with what we've drawn.
pygame.display.flip()
# Limit to 30 frames per second.
clock.tick(30)
if __name__ == "__main__":
main()
# If you forget this line, the program will 'hang'
# on exit if running from IDLE.
pygame.quit()

BIN
requirements.txt Normal file
View File

Binary file not shown.

41
src/linearHallSensor.cpp
View File

@@ -67,19 +67,18 @@ void LinearHallSensor::init(BLDCMotor motor)
MotionControlType prevController = motor.controller;
motor.controller = MotionControlType::angle_openloop;
float prevVoltageLimit = motor.voltage_limit;
motor.voltage_limit = 1.5;
motor.voltage_limit = 2.0;
// 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 epsilon = 2;
const float step = 0.005;
const uint8_t epsilon = 3;
float currentPosition = 0.0;
const uint8_t N = 40;
const uint8_t N = 20;
uint32_t senseA[N];
uint32_t senseB[N];
int ptr = 0;
init_arr(senseA, N, 0);
@@ -104,8 +103,9 @@ void LinearHallSensor::init(BLDCMotor motor)
_maxCh1 = senseA[ptr];
else if (senseA[ptr] < _minCh1)
_minCh1 = senseA[ptr];
if (senseB[ptr] > _maxCh1)
_maxCh1 = senseB[ptr];
if (senseB[ptr] > _maxCh2)
_maxCh2 = senseB[ptr];
else if (senseB[ptr] < _minCh2)
_minCh2 = senseB[ptr];
@@ -117,10 +117,10 @@ void LinearHallSensor::init(BLDCMotor motor)
// Move to new position
currentPosition += step;
motor.move(currentPosition);
delay(1);
delay(3);
}
_maxPositionEndValue = currentPosition - (step * (ptr > N ? N : ptr));
currentPosition = _maxPositionEndValue - M_PI / 8;
currentPosition = _maxPositionEndValue - M_PI / 16;
delay(100);
motor.move(currentPosition);
Serial.println("\t- Found first end stop : Max position = " + String(_maxPositionEndValue));
@@ -128,7 +128,6 @@ void LinearHallSensor::init(BLDCMotor motor)
// Swipe motor to search other hard end, and find eventually new max analog values of sensors
endFound = false;
currentCheck = 0;
init_arr(senseA, N, 0);
init_arr(senseB, N, 0);
@@ -144,8 +143,9 @@ void LinearHallSensor::init(BLDCMotor motor)
_maxCh1 = senseA[ptr];
else if (senseA[ptr] < _minCh1)
_minCh1 = senseA[ptr];
if (senseB[ptr] > _maxCh1)
_maxCh1 = senseB[ptr];
if (senseB[ptr] > _maxCh2)
_maxCh2 = senseB[ptr];
else if (senseB[ptr] < _minCh2)
_minCh2 = senseB[ptr];
@@ -157,7 +157,7 @@ void LinearHallSensor::init(BLDCMotor motor)
// Move to new position
currentPosition -= step;
motor.move(currentPosition);
delay(1);
delay(3);
}
_minPositionEndValue = currentPosition + (step * (ptr > N ? N : ptr));
Serial.println("\t- Found second end stop : Min position = " + String(_minPositionEndValue));
@@ -168,7 +168,7 @@ void LinearHallSensor::init(BLDCMotor motor)
_offset = 0.0;
for (uint8_t i = 0; i <= 49; i++)
sum += readSensorCallback();
_offset = -sum / 50.0;
_offset = sum / 50.0;
Serial.println("\t- maxA: " + String(_maxCh1) + "\tminA: " + String(_minCh1) + "\tmaxB: " + String(_maxCh2) + "\tminB: " + String(_minCh2));
Serial.println("\t- Offset: " + String(_offset));
@@ -190,8 +190,8 @@ void LinearHallSensor::init(BLDCMotor motor)
Serial.println("\t- Max angle: " + String(_maxSensor));
// Recovering previous controller type and go in the middle
for (float i = _maxPositionEndValue; i >= (_maxPositionEndValue + _minPositionEndValue); i = i - 0.005)
// Recovering previous controller type and go to 0
for (float i = _maxPositionEndValue; i >= (0); i = i - 0.005)
{
motor.move(i);
delay(1);
@@ -225,5 +225,10 @@ float LinearHallSensor::readSensorCallback()
_estimatePosition += dist_angle(_currentPosition, _prevPosition);
_prevPosition = _currentPosition;
return (_estimatePosition + _offset) / 4.0;
return (_estimatePosition / 4.0 - _offset);
}
float LinearHallSensor::getMaxAngle()
{
return _maxSensor;
}

10
src/linearHallSensor.h
View File

@@ -34,6 +34,14 @@ class LinearHallSensor
float readSensorCallbackStateMachine();
float readSensorCallback();
/**
* @brief retrun the max angle of the motor
*
* @return max angle in rad
*/
float getMaxAngle();
private:
uint32_t _analogPin1;
@@ -49,7 +57,7 @@ class LinearHallSensor
float _currentPosition;
float _estimatePosition; // current position + offset
float _offset; // offset angle measured at init
float _maxSensor; // Max angle value measured (so command from 0 to that value)
float _maxSensor; // Max range angle value measured (so command from 0 to that value)
float _minPositionEndValue; // min pos in open loop mode
float _maxPositionEndValue;

253
src/main.cpp
View File

@@ -3,53 +3,60 @@
#include <linearHallSensor.h>
#include <pinout.h>
BLDCMotor motor1 = BLDCMotor(4);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(M1_PWM1, M1_PWM2, M1_PWM3);
LinearHallSensor linearSensor1 = LinearHallSensor(M1_Hall1, M1_Hall2);
BLDCMotor motor[3] =
{
BLDCMotor(4),
BLDCMotor(4),
BLDCMotor(4)};
BLDCMotor motor2 = BLDCMotor(4);
BLDCDriver3PWM driver2 = BLDCDriver3PWM(M2_PWM2, M2_PWM1, M2_PWM3);
LinearHallSensor linearSensor2 = LinearHallSensor(M2_Hall1, M2_Hall2);
BLDCDriver3PWM driver[3] =
{
BLDCDriver3PWM(M1_PWM1, M1_PWM2, M1_PWM3),
BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3),
BLDCDriver3PWM(M3_PWM1, M3_PWM2, M3_PWM3)};
BLDCMotor motor3 = BLDCMotor(4);
BLDCDriver3PWM driver3 = BLDCDriver3PWM(M3_PWM2, M3_PWM1, M3_PWM3);
LinearHallSensor linearSensor3 = LinearHallSensor(M3_Hall1, M3_Hall2);
LinearHallSensor linearSensor[3] =
{
LinearHallSensor(M1_Hall1, M1_Hall2),
LinearHallSensor(M2_Hall1, M2_Hall2),
LinearHallSensor(M3_Hall1, M3_Hall2)};
void initSensor0()
{
linearSensor[0].init(motor[0]);
}
float callback0()
{
return linearSensor[0].readSensorCallback();
}
void initSensor1()
{
linearSensor1.init(motor1);
linearSensor[1].init(motor[1]);
}
float callback1()
{
return linearSensor1.readSensorCallback();
return linearSensor[1].readSensorCallback();
}
void initSensor2()
{
linearSensor2.init(motor2);
linearSensor[2].init(motor[2]);
}
float callback2()
{
return linearSensor2.readSensorCallback();
return linearSensor[2].readSensorCallback();
}
void initSensor3()
{
linearSensor3.init(motor3);
}
float callback3()
{
return linearSensor3.readSensorCallback();
}
GenericSensor sensor[3] =
{
GenericSensor(callback0, initSensor0),
GenericSensor(callback1, initSensor1),
GenericSensor(callback2, initSensor2)};
GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
GenericSensor sensor2 = GenericSensor(callback2, initSensor2);
GenericSensor sensor3 = GenericSensor(callback3, initSensor3);
float target[3] = {0.5, 0.5, 0.5};
float targetX = 0.0;
float targetY = 0.0;
float target = 0.0;
String str;
void serialLoop()
{
@@ -60,9 +67,9 @@ void serialLoop()
received_chars += inChar;
if (inChar == '\n')
{
target = received_chars.toFloat();
target[0] = received_chars.toFloat();
Serial.print("Target = ");
Serial.print(target);
Serial.print(target[0]);
received_chars = "";
}
}
@@ -73,9 +80,46 @@ 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;
}
void initMotor(u_int8_t m)
{
Serial.printf("\n\t\t### MOTOR %d ###\n\n", m + 1);
driver[m].voltage_power_supply = 7.0;
driver[m].pwm_frequency = 50000;
Serial.printf("Driver%d init: %d\n", m + 1, driver[m].init());
motor[m].linkDriver(&driver[m]);
motor[m].useMonitoring(Serial);
motor[m].controller = MotionControlType::angle;
motor[m].foc_modulation = FOCModulationType::SinePWM;
motor[m].voltage_limit = 2.0;
motor[m].voltage_sensor_align = 2.0;
motor[m].PID_velocity.P = 0.05f;
motor[m].PID_velocity.I = 0.008;
motor[m].PID_velocity.D = 0.0;
motor[m].LPF_velocity.Tf = 0.02f;
motor[m].P_angle.P = 150.0;
motor[m].P_angle.I = 5.0;
motor[m].velocity_limit = 20;
// Init sensor
motor[m].init();
Serial.println("calibrating sensor in open loop...");
sensor[m].init();
Serial.printf("Sensor %d done\n", m + 1);
motor[m].linkSensor(&sensor[m]);
motor[m].init();
if (m == 2)
motor[m].initFOC(3.0, CW);
else if (m == 1)
motor[m].initFOC(3.79, CW);
// motor[m].initFOC();
else
motor[m].initFOC(0.18, CW);
Serial.printf("Motor %d Done\n", m + 1);
}
void setup()
{
Serial.begin(115200);
delay(3000);
Serial.println("INIT");
@@ -83,115 +127,50 @@ void setup()
pinMode(LED_BUILTIN, OUTPUT); // Lightup LED
digitalWrite(LED_BUILTIN, LOW);
// ### MOTOR 1 ###
Serial.println("\n\t\t### MOTOR 1 ###\n");
driver1.voltage_power_supply = 7.0;
Serial.println("Driver1 init: " + String(driver1.init()));
motor1.linkDriver(&driver1);
motor1.useMonitoring(Serial);
motor1.controller = MotionControlType::angle;
motor1.foc_modulation = FOCModulationType::SinePWM;
motor1.voltage_limit = 1.0;
motor1.voltage_sensor_align = 1.0;
motor1.PID_velocity.P = 0.05f;
motor1.PID_velocity.I = 0.01;
motor1.PID_velocity.D = 0.0;
motor1.LPF_velocity.Tf = 0.01f;
motor1.P_angle.P = 150.0;
motor1.P_angle.I = 10.0;
motor1.velocity_limit = 25;
// Init sensor 1
motor1.init();
Serial.println("calibrating sensor in open loop...");
sensor1.init();
Serial.println("Sensor 1 done");
delay(1000);
motor1.linkSensor(&sensor1);
motor1.init();
motor1.initFOC(2.98, CW);
// motor1.initFOC();
Serial.println("Motor 1 Done");
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);
motor2.initFOC();
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);
motor[1].useMonitoring(Serial);
initMotor(2);
initMotor(1);
initMotor(0);
}
void loop()
{
serialLoop();
motor1.move(target);
motor1.loopFOC();
motor1.monitor();
int len = Serial.available();
if (len > 0)
{
str = Serial.readStringUntil('\n'); // get new targets from serial (target are from 0 to 1000)
char axis = str[0];
str.remove(0, 1);
str.remove(len,1);
switch (axis)
{
case 'X':
target[1] = str.toFloat();
// Serial.println("Target X: " + String(target[1]));
target[1] = mapfloat(target[1], 0.0, 1000.0, 0.0, linearSensor[1].getMaxAngle()); // remap targets to motor angles
break;
case 'Y':
target[0] = str.toFloat();
// Serial.println("Target Y: " + String(target[0]));
target[0] = mapfloat(target[0], 0.0, 1000.0, 0.0, linearSensor[0].getMaxAngle()); // remap targets to motor angles
break;
case 'Z':
target[2] = str.toFloat();
// Serial.println("Target Z: " + String(target[2]));
target[2] = mapfloat(target[2], 0.0, 1000.0, 0.0, linearSensor[2].getMaxAngle()); // remap targets to motor angles
break;
default:
break;
}
}
motor2.move(target);
motor2.loopFOC();
motor2.monitor();
for (u_int8_t i = 0; i < 3; i++) // update motor target and Run FOC
{
// serialLoop();
// Serial.println("Target" + String(i) + ": " + String(target[i]));
motor[i].move(target[i]);
motor[i].loopFOC();
// motor[i].monitor();
}
}

8
src/pinout.h
View File

@@ -5,9 +5,9 @@
// PCB board : STM32 DJI Gimbal V1.0
#define M1_PWM1 PA8
#define M1_PWM1 PA10
#define M1_PWM2 PA9
#define M1_PWM3 PA10
#define M1_PWM3 PA8
#define M1_Fault PB12
#define M1_Hall1 PB1
#define M1_Hall2 PA5
@@ -20,8 +20,8 @@
#define M2_Hall2 PA3
#define M3_PWM1 PB0
#define M3_PWM2 PA6
#define M3_PWM3 PA7
#define M3_PWM2 PA7
#define M3_PWM3 PA6
#define M3_Fault PA0
#define M3_Hall1 PA2
#define M3_Hall2 PA1