diff --git a/docs/LinearHallEncoder.md b/docs/LinearHallEncoder.md deleted file mode 100644 index 9cb8fbe..0000000 --- a/docs/LinearHallEncoder.md +++ /dev/null @@ -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; -} - -``` - diff --git a/platformio.ini b/platformio.ini index fd5d195..bc1a7dd 100644 --- a/platformio.ini +++ b/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 \ No newline at end of file + ; -D SIMPLEFOC_STM32_DEBUG +monitor_dtr = 1 diff --git a/pythonGUI/gimbal_gui.py b/pythonGUI/gimbal_gui.py new file mode 100644 index 0000000..7876ba1 --- /dev/null +++ b/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) diff --git a/pythonGUI/joystickTest.py b/pythonGUI/joystickTest.py new file mode 100644 index 0000000..e04bc4e --- /dev/null +++ b/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() \ No newline at end of file diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..ce3d581 Binary files /dev/null and b/requirements.txt differ diff --git a/src/linearHallSensor.cpp b/src/linearHallSensor.cpp index 0202af4..4928279 100644 --- a/src/linearHallSensor.cpp +++ b/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; - 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; +} \ No newline at end of file diff --git a/src/linearHallSensor.h b/src/linearHallSensor.h index 5af1225..89d5d03 100644 --- a/src/linearHallSensor.h +++ b/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; diff --git a/src/main.cpp b/src/main.cpp index d1a1cfb..c5ed8e1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,53 +3,60 @@ #include #include -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(); + } } \ No newline at end of file diff --git a/src/pinout.h b/src/pinout.h index 347b84e..4d6e6ef 100644 --- a/src/pinout.h +++ b/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_PWM3 PA7 +#define M3_PWM2 PA7 +#define M3_PWM3 PA6 #define M3_Fault PA0 #define M3_Hall1 PA2 #define M3_Hall2 PA1