Closed loop 3 axis OK !
This commit is contained in:
Lucien RENAUD
parent
413894315a
commit
0d7631cfd8
|
|
@ -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.
|
||||
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
We can see that in the first movement (positive rotation), the green is out of phase of π/2.`
|
||||
|
||||
|
||||
### 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;
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -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()
|
||||
Binary file not shown.
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
253
src/main.cpp
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue