Dear FOC-Community,
First of thank you all for the great library and also for the great website with lots of information!
Side node: I am building a robotic dog and I am more familiar with the mechanical side of things than the software side, so this is all quite new for me.
My goal is to have position/angle motion control. To achieve this, I used the following setup:
Hardware setup:
- Microcontroller: Nucleo 64 or Arduino Uno
- BLDC Driver: DRV8302
- Encoder: AS5600 Magnetic encoder
-Motor: Tarot 4108 ( Internal resistence : 135mĪ©)
With this setup, I achieved the following things:
- Working Open loop velocity control
- Working magnetic sensor
So the following step after this was achieved was closed loop voltage control and here is were I am stuckā¦
To make sure I am not making mistakes, I used the Arduino Uno instead of the Nucleo board just as the example here: DRV8302 example | Arduino-FOC (simplefoc.com).
I changed the encoder to the magnetic sensor so that the following code is created:
#include <SimpleFOC.h>
// DRV8302 pins connections
// don't forget to connect the common ground pin
#define INH_A 9
#define INH_B 10
#define INH_C 11
#define EN_GATE 8
#define M_PWM 6
#define M_OC 5
#define OC_ADJ 7
// motor instance
BLDCMotor motor = BLDCMotor(11);
// driver instance
BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE);
// Changed encoder to magnetic encoder
MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4);
void setup() {
// initialize magnetic sensor instead of regular encoder
sensor.init();
motor.linkSensor(&sensor);
// DRV8302 specific code
// M_OC - enable over-current protection
pinMode(M_OC,OUTPUT);
digitalWrite(M_OC,LOW);
// M_PWM - enable 3pwm mode
pinMode(M_PWM,OUTPUT);
digitalWrite(M_PWM,HIGH);
// OD_ADJ - set the maximum over-current limit possible
// Better option would be to use voltage divisor to set exact value
pinMode(OC_ADJ,OUTPUT);
digitalWrite(OC_ADJ,HIGH);
// configure driver
driver.voltage_power_supply = 10; //Using exernal power supply set on 10 volt with current limit set to 1 amps.
driver.init();
motor.linkDriver(&driver);
// choose FOC modulation
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// set control loop type to be used
motor.controller = ControlType::voltage;
motor.useMonitoring(Serial);
// controller configuration based on the control type
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 20;
// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.01;
// angle loop controller
motor.P_angle.P = 20;
// angle loop velocity limit
motor.velocity_limit = 50;
// default voltage_power_supply
motor.voltage_limit = 1; //lowered the motor voltage due to the low internal resistance of the motor
// use monitoring with serial for motor init
// monitoring port
Serial.begin(115200);
// comment out if not needed
motor.useMonitoring(Serial);
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();
// set the initial target value
motor.target = 2;
_delay(1000);
}
void loop() {
// iterative setting FOC phase voltage
motor.loopFOC();
// iterative function setting the outer loop target
// velocity, position or voltage
// if target not set in parameter uses motor.target variable
motor.move();
motor.monitor();
// user communication
motor.command(serialReceiveUserCommand());
}
// utility function enabling serial communication the user
String serialReceiveUserCommand() {
// a string to hold incoming data
static String received_chars;
String command = "";
while (Serial.available()) {
// get the new byte:
char inChar = (char)Serial.read();
// add it to the string buffer:
received_chars += inChar;
// end of user input
if (inChar == '\n') {
// execute the user command
command = received_chars;
// reset the command buffer
received_chars = "";
}
}
return command;
}
When this code is run the motor acts very weird. It does not move and makes lots of noice. Sometimes it will begin to vibrate very heavily. So my question is: How can I solve this? Did I make beginner mistakes somewhere?
Possible beginner mistakes that I already have looked at:
- Connected wires wrong: I have checked it lots of times, also in open loop velocity it works.
- Not working sensor: Sensor is working because the data from the serial monitor looks correct and is not acting strange.
- I also tried different types of motors (including gimbal motor) and they all act the same.
- I also watched and tried the different codes and watched the tuning guide on youtube.
- I added pullup resistors.
This is the output of the log when starting the arduino:
MOT: Monitor enabled!
MOT: Initialise variables.MOT: Monitor enabled!
MOT: Initialise variables.
MOT: Enable driver.
MOT: Align sensor.
MOT: natural_direction==CCW
MOT: Absolute zero align.
MOT: Success!
MOT: Motor ready.
I hope that I provided enough information and I am looking forward to a reply.
Greetings,
Wittecactus