Closed loop 3 axis OK !

docs/LinearHallEncoder.md

@@ -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;
-}
-
-``` 
-

platformio.ini

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

pythonGUI/gimbal_gui.py

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

pythonGUI/joystickTest.py

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

src/linearHallSensor.cpp

@@ -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;
+    const float step = 0.005;
-    uint8_t currentCheck = 0;  // current check number
+    const uint8_t epsilon = 3;
-    const uint8_t epsilon = 2;
     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
+    // Recovering previous controller type and go to 0
-    for (float i = _maxPositionEndValue; i >= (_maxPositionEndValue + _minPositionEndValue); i = i - 0.005)
+    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;
+}

src/linearHallSensor.h

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

src/main.cpp

@@ -3,53 +3,60 @@
 #include <linearHallSensor.h>
 #include <pinout.h>
 
-BLDCMotor motor1 = BLDCMotor(4);
+BLDCMotor motor[3] =
-BLDCDriver3PWM driver1 = BLDCDriver3PWM(M1_PWM1, M1_PWM2, M1_PWM3);
+    {
-LinearHallSensor linearSensor1 = LinearHallSensor(M1_Hall1, M1_Hall2);
+        BLDCMotor(4),
+        BLDCMotor(4),
+        BLDCMotor(4)};
 
-BLDCMotor motor2 = BLDCMotor(4);
+BLDCDriver3PWM driver[3] =
-BLDCDriver3PWM driver2 = BLDCDriver3PWM(M2_PWM2, M2_PWM1, M2_PWM3);
+    {
-LinearHallSensor linearSensor2 = LinearHallSensor(M2_Hall1, M2_Hall2);
+        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);
+LinearHallSensor linearSensor[3] =
-BLDCDriver3PWM driver3 = BLDCDriver3PWM(M3_PWM2, M3_PWM1, M3_PWM3);
+    {
-LinearHallSensor linearSensor3 = LinearHallSensor(M3_Hall1, M3_Hall2);
+        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()
+GenericSensor sensor[3] =
-{
+    {
-    linearSensor3.init(motor3);
+        GenericSensor(callback0, initSensor0),
-}
+        GenericSensor(callback1, initSensor1),
-float callback3()
+        GenericSensor(callback2, initSensor2)};
-{
-    return linearSensor3.readSensorCallback();
-}
 
-GenericSensor sensor1 = GenericSensor(callback1, initSensor1);
+float target[3] = {0.5, 0.5, 0.5};
-GenericSensor sensor2 = GenericSensor(callback2, initSensor2);
-GenericSensor sensor3 = GenericSensor(callback3, initSensor3);
 
-float targetX = 0.0;
+String str;
-float targetY = 0.0;
-
-float target = 0.0;
 
 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 ###
+    motor[1].useMonitoring(Serial);
-    Serial.println("\n\t\t### MOTOR 1 ###\n");
+    initMotor(2);
-    driver1.voltage_power_supply = 7.0;
+    initMotor(1);
-    Serial.println("Driver1 init: " + String(driver1.init()));
+    initMotor(0);
-    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);
 }
 
 void loop()
 {
-    serialLoop();
+    int len = Serial.available();
-    motor1.move(target);
+    if (len > 0)
-    motor1.loopFOC();
+    {
-    motor1.monitor();
+        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);
+    for (u_int8_t i = 0; i < 3; i++) // update motor target and Run FOC
-    motor2.loopFOC();
+    {
-    motor2.monitor();
+        
+        // serialLoop();
+        // Serial.println("Target" + String(i) + ": " + String(target[i]));
+        motor[i].move(target[i]);
+        motor[i].loopFOC();
+        // motor[i].monitor();
+    }
 }

src/pinout.h

@@ -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_PWM2 PA7
-#define M3_PWM3 PA7
+#define M3_PWM3 PA6
 #define M3_Fault PA0
 #define M3_Hall1 PA2
 #define M3_Hall2 PA1