Current Limits, Current Control, and High Power BLDCs

hardware support
1

Added context first, for questions/issues go to +++++++++++++++++
TL;DR:

  • How to limit current when using powering with a battery (but still have holding torque)?
  • Is it possible to read the current accurately even when using voltage control?
  • Once voltage control works how to start with foc current control?

Context/Background
I am hoping to move to SimpleFOC based motor controllers for the robot that I am working on. We have a couple of motors that are used in various parts of the system, name this 150kV motor from ODrive and this 350kV Turnigy motor.

Currently, our system uses ODrives, but it would be really nice to be able to make/have/use a board that can drive a single motor and has a better command interface. With these goals in mind, we came across the SimpleFOC PowerShield and immediately had our attention piqued. After reading through the post on the shield it seemed like it fit our use-case well as we generally have gearing so the motors can turn quickly. Decided to take a shot at testing it out using some BTN chips from China, knowing full well they may cause problems. The first round of tests went poorly but figured out it was because we didn’t have a good handle on the software and were using batteries for power which allowed huge current spikes to blow out the chips/traces.

We grabbed an Infineon BLDC_Shield that uses the IFX007 chips as they appeared to be drop-in replacements for the BTN so it should allow us to test some likely working hardware while we got our footing with the software. Further, we switched to a desktop power supply so we could limit the current effectively.

We now have pretty good control over the motors within the TorqueControlType::voltage regime, both MotionControlType::velocity and MotionControlType::angle which are the two that we need for the robot. This is true for both the Infineon and SimpleFOC boards.

+++++++++++++++++
Questions/Problems

  1. Voltage/Current/Velocity limits do not seem to behave as I would expect
    1.1 I have mostly been setting the phase resistance of the motor so that things can be done in current, but even when using voltage limits the behavior seems the same.
    1.2 In order to reach high speeds ~2000RPM (210rad/s) the limits have to be set very high, yet according to both the readout on the desktop power supply and an amp clamp the chosen limit is never reached. For example, with the settings shown below (1.2 Settings), the motor peaks at ~60 rad/s and an amp draw of ~0.75A even when the target velocity is >100. I am fairly sure I need to keep fiddling with PID gains, but my understanding is that should mostly affect how quickly and smoothly desired speeds are reached rather than what the cap is. Upping the current so motor.current_limit = 50, while changing nothing else, leads to a peak speed of ~90rad/s and an amp draw of ~0.82A.
    1.3 I am further puzzled because during the encoder index search and pole pair tests the motor moves fairly smoothly but it uses the full 5A that I have allowed it to have on the desktop power supply.
    1.4 The goal of these limits in our system is to ensure that the load drawn from the battery and through various wiring harnesses is not too high, but is high enough to allow for holding torque in specific locations.

  2. Current Sensing - Voltage Control
    2.1 In a potentially related issue, that may stem from my lack of deeper knowledge about FOC algorithms, trying to read the current sensor(s) in voltage mode is giving issues. Printing out current_sensor.getDCCurrent(), when running the same test from 1.2, gives highly volatile readings between 6-12A, with occasional extreme values like 1A or 28A.
    2.2 The goal of reading current in this mode is that – as I will outline below, we have not been able to get any control modes using the current working – but we try to monitor what the current draw is in order to predict/optimize battery life on the robot. Plus it seems any oddities here may be the cause of our current control issues.

  3. TorqueControlType::foc_current not working at all
    3.1 Trying to run in foc_current mode with the default gains leads to the motor spinning crazily even at torque or velocity targets of 0. And it is using the whole 5A available to it from the power supply.
    3.2 Using the gains found in this post (3.2 settings) that are for the ODrive motor we have done testing with, does make it so that the motor doesn’t move with velocity or torque targets of 0, but it still draws the full 5A of the power supply (or more if allowed) to sit still.
    3.3. Testing the current sensor on its own by attaching a resistor to the motor phase associated with it, either with the benchtop power supply or with a battery seems to give the right values; both when read directly from the ADC and when read through the current_sensor.getDCCurrent() function of the SimpleFOC library.
    3.4 In searching through the documentation and various issues here I found various tests for how to see if the current sensors and/or foc algorithm is working and none of them seem to behave as desired. One such test is from the getting started doc and says that when the motor is held current_d should be 0 (doesn’t seem to be true in my case) and current_q should increase as the setpoint is increased. Similarly, when a torque target of 0 is sent the motor doesn’t seem to spin freely as is mention here

  4. Units - Generally speaking it is a bit hard to figure out what units various settings and values are in. Is the whole library in standard units (e.g. V, A, rad/s) or are there times where mA or degrees are used? For all the limits and ramp values in the setup phase? What about the output from the monitor?

1.2 Settings

phase_resistance = 0.047858;
driver.voltage_power_supply = 24;
driver.voltage_limit = 24;
motor.current_limit = 30;
motor.velocity_limit = 220;

3.2 Settings

  motor.PID_current_q.P = 0.5;
  motor.PID_current_q.I = 0.1;
  motor.PID_current_q.D = 0;
  motor.PID_current_q.limit = 1;
  motor.PID_current_q.output_ramp = 50;
  motor.LPF_current_q.Tf = 0.1;

  motor.PID_current_d.P = 1;
  motor.PID_current_d.I = 0.5;
  motor.PID_current_d.D = 0.01;
  motor.PID_current_d.limit = 2;
  motor.PID_current_d.output_ramp = 100;
  motor.LPF_current_d.Tf = 0.0;

Also, I am quite new to the forum and to FOC algorithms in general so please just point me elsewhere if the answers I am seeking already exist. And I am more than happy to answer any questions or give more details about the hardware or software in use or the steps attempted so far.

2

Hey @NNelsonwood,

So many good questions.

So, when using the voltage control mode. You will be setting the voltgae_limit or if you’ve set the motor’s phase resistance phase_resistance you will have to set the current_limit. Both of these limits in the voltage mode will be limiting the voltage, since there si no current measurement no real current limiting is possible. Aslo if phase resistance set the and provuded the current_limit the equivalent voltage_limit is

motor.voltage_limit = motor.phase_resistance*motor.current_limit

Or in the other words:

motor.foc_controller = TorqueControlType::voltage;
motor.voltage_limit = 10; \\ V
\\ efectively the same thing as
motor.phase_resitance = 10; \\ Ohm
motor.current_limit = 1; \\Am

Now in terms of expected behavior. When you limit the voltage that the motor can receive ( both by isiong voltge_limit or phase_resistance and current_limit) the motor final velocity will be impacted.
The electric motors have an effect that they induce electric voltage proportional to their velocity, often called back-emf voltage.

V_bemf = k_e*vel

The electric motor torque however is propotional to the current passing through it

T = k_i*I

Now if we write the equation of the electical circuit of a el. motor you would have:

V_in - V_bemf  - I*R = 0

Which basically means that any voltage that you set as the input (V_in) of your motor will be reduced by this induced bas-emf voltage (V_bemf) and the voltage that remains will create the electric current (I) and torque.

So based on this relationship you can write the velocity equation of the motor:

vel = ( V_in - I*R ) / k_e

Which basically shows that the final velocity of your motor will be propotional to the input voltage substracted by the I*R which is proportional to the torque your motor is required to generate in order to counteract the friction, motor inertia and possible the inertia of the mechanism that is attached to the motor.

From this equation you can see that if we limit the voltage V_in we directly limit the velocity the motor can achieve.

You might ask now, whay are we limiting the voltage then. It is actually to limit the worst case current in the motor. The worst case current draw of the motor is achieved if the motor is not turning and there is no back-emf V_bemf=0:

I = V_in/R

So setting the voltage_limit is intended to limit this maximal (worst case) current. But it also has the effect that it reduces the maximal velocity of the motor.

One quick fix for this might be to raise the voltage limit value based on the current valocity of the motor. When the velocity raises you could augment the velocity_limit and when velocity drops you could lower it. For no no such thing is implemented in the simplefoc and we dont have near future plans to support this, but we would be in to collaborate over it if someone is interested in contributing.

On the other hand, if you are using the foc_current mode then the voltage_limit and current_limit are two separate limits and will impact the motors behavior in different ways. Voltag_limit will ave the same impact as described before, but the current_limit will directly limit the I current in the motor and in that way yhe motor’s toque. This is a much safer option and we recommend not setting the voltage limit in the foc_current mode, but using only current_limit.

On a side note, if you use foc_current mode, setting the phase_resistance value will only facilitate changes in between the voltage more and the foc_current mode, it will not impct the foc_current mode in any way. So this variable is intended mostly for the voltage mode.

It is hard to say what the problem is here, we would need to go a bit more in detail.
Which board are you using, is it the powershiled or the Infineons board?
Infineon’s board has a high-side current sesning circuit which we do not support in the simplefoc.
We only support inline current sensing and low-side sensing for esp32 boards.
What is the mcu you are using?

There is definietely an issue there. I don’t think this is the gain problem and I’d suggest you to only test the torque control mode until you have good behavior. (That for current target equal to 0 you have no resistance in your motor).
Can you show us the output of your serial terminal while the motor is initialising?
Once again, to go deeper we will need the mcu and driver specs :smiley:
But yeah, once you have the current control working you will be able to limit (or a better word might be to allocate) the torque your motor is allowed to generate. You will be able to reach much higher velocity values.

All the units in the library are Volts, Amps, radian and radian per second.
The only exception is in the monitor where the current outupurs are saled to miliamps for easier visibility.

1 Like
3

@Antun_Skuric thanks so much for the detailed answer!

It really helped clarify the difference between the voltage control mode and foc_current modes. And it is good to hear I should be working towards getting current control to work as that was the original goal.

Sorry that I was unclear on the hardware details. I am doing all the foc_current control with the SimpleFOC PowerShield, populated with BTN8982s. And using a Sparkfun SADM21 Dev Breakout board. It has the same footprint as an Arduino Uno R3 but, as the name suggests, uses a SAMD21G instead of an ATmega328.

As such I am using inline current sensing with the 0.001Ω resistor and the INA240 current sensor with a gain of 50.

Here is a screenshot of the serial when initializing the motor, carried out when with motor.torque_controller = TorqueControlType::foc_current, motor.foc_modulation = FOCModulationType::SpaceVectorPWM, and motor.controller = MotionControlType::torque. It also shows that at the start, even with a target torque of zero, the motor initially spins (monitor showing target, velocity, angle):

Screen Shot 2022-01-18 at 11.08.57
Screen Shot 2022-01-18 at 11.08.57832Ă—1162 136 KB

This makes a lot of sense, but I think it would be really useful for it to be mentioned in the docs of the monitor that it doesn’t use base units as it was a little confusing to figure out what was going on. Just adding units in the comments of each monitor output here would probably be sufficient. For example

  • current.q - [mA] measured current in q direction ( if current sense available )

I looked to see if there was an easy way for me to facilitate the change but doesn’t appear I have access (which is understandable :laughing:).

Let me know if there is any other info you need to help diagnose the issue, as well as what I should be doing for next steps.

4

hey @NNelsonwood,

I’ll make sure to update it in the docs, if you have some time you can make a github issue for it, so it will go faster.

I see in your serial output that the aligning the current sense has returned 1, which means that no chnages have been made for the alignment . PowerShiled should return either Success: 2 or Success: 3 because there is an inversion of one of the current sense direction in the hardware.
Success: 1 is either an error of calibration or you maybe used motor.skip_align=true; Is that the case?

If you do not have this line of code than there is an issue with the alignment of the current sense. The good thing is that you can skip it. Please use the following lines in between current_sense.init() and motor.linkCurrentSense(..);

// init current sense
current_sense.init();
...
// invert phase b gain
current_sense.gain_b *=-1;
// skip alignment
current_sense.skip_align = true;
...
// link motor and current sense
motor.linkCurrentSense(&current_sense);
5

Thanks for the quick response @Antun_Skuric.

I did accidentally have current_sensor.skip_align = true in the code. After running with it set to false the return of the check is now MOT: Success: 3 and the motor seems to behave better.

Though when perturbed it still starts to spin when the target is 0. That said, seems like there is a chance it is due to the PID gains? Or is there something else that I should check first?

Also, created a git issue for the units update.

6

Hey @NNelsonwood,
Ok, cool, that was probably the main issue.

The running away might be the PID gain issue that’s true. Try using the commander raise a bit the filtering values of the d and q current. Then try lowering all the P and I gains and try fiddling with them.
D gain you will most probably not need, the same goes for the ramp.

7

Hello @Antun_Skuric,

I think I just made a pull request for the changes to the documentation.

And tuning the filtering and PI gains sounds like a good plan of action. And, just to clarify, are you saying the ramp and D gain should be 0 or should remain at their library defaults?

And, just to clarify, are you saying the ramp and D gain should be 0 or should remain at their library defaults?

I now realize these ^ are the same thing

1 Like
8

Also, should the current_q and current_d gains/filters be the same? Are there specific use cases when they should be different? Should they always be tuned separately?

10

Hey @NNelsonwood,

Yeah, I’d keep them the same.
Personally, I am not aware of any situation where you would need to use different values, at least for BLDC motors.

FOC is more general approach that is used for AC motors as well, which have a bit more complex dynamics and might have some scenarios where that is necessary.

11

Thanks @Antun_Skuric, makes total sense that the AC motor use-case would be different.

After playing with PID values a bunch to try and get the motor to not turn/vibrate with a target torque of 0 I was getting very low PID gains (e.g. 0.008, 0.02, 0.0, Tf = 0.001) which then meant it was super weird when responding to non-zero inputs.

This felt wrong so after some experimenting, with the gains raised back up a bit, it seems like the issue is that the “neutral” point is around -3, meaning that if I set the target in torque mode to -3 the motor happily stays in place and values of 2 and -8 seems to give the same speeds but in opposite directions.

Is there some way to affect what the center point/zero point is?

12

Hey @NNelsonwood,

Did you try to raise the filtering? I think at the beginning the filtering time constants are very low and might let pass a lot of noise.
Try raising the LPF_current_q.Tf and LPF_current_d.Tf to 0.01 or so.

Huh, this is weird!
So 3 amps is as if you have set 0?
This means that there is a huge misalignment which happens during the initialization. Do you see the same behavior in the voltage mode?

13

@Antun_Skuric thanks for the quick response!

I have been playing with the filtering, and it does might quite a big difference. It seems that between 0.005 and 0.05 is a good zone but haven’t narrowed it down further due to the other issue mentioned.

If the motor.target is setting a current value, then yes, I was seeing that it seemed to be centered around (-)3 instead of 0. I have never noticed this happening when using TorqueControlType::voltage, and was able to get the PID values for both velocity and angle control relatively well in that mode.

However, it seems my issue may stem from the fact that sometimes it get MOT: Success: 4 and others MOT: Success: 3 as the response to MOT: Align current sense. Specifically, I get 3 when I limit the current to 5A with my power supply and 4 when the power supply current limit is 10A. In both cases, all of the available current is used and it doesn’t seem to matter whether the motor.current_limit is set to 5 or 10, only what the power supply is providing.

14

Yes that is definitely an issue.

Try avoiding the current sense alignment. This should give you the properly aligned current sense and avoid the alignment procedure.

// init current sense
current_sense.init();
...
// invert phase b gain
current_sense.gain_b *=-1;
// skip alignment
current_sense.skip_align = true;
...
// link motor and current sense
motor.linkCurrentSense(&current_sense);
15

@Antun_Skuric

Another update after a lot more testing of PID values. I did what you recommended for the current sensor alignment and it seems to have helped in a way.

The whole system is generally a whole lot more stable - kind of…I have been able to get to work okay with just P controllers for both current_q and current_d. But adding any I terms always seemed to mess things up very quickly. Further, I once again ended up in the state where the center point seemed to not be 0 anymore but using the commander to play around when in that state it seemed to only happen when the I gain was non-zero. So my hypothesis was that the error buildup was causing the I value to want to spin the motor even when at a target of 0 if it had previously been spinning. This led me down a rabbit hole of paying closer attention to what the I gains were doing. And I had the weird discovery that a negative I value for current_d seemed to work better. Then I had to try negative values for various gains and it seems that the sign of the P and I gains on current_q and current_d want to be opposite each other. For example:

  motor.PID_current_q.P = 0.25;
  motor.PID_current_q.I = 0.01;

  motor.PID_current_d.P = -0.25;
  motor.PID_current_d.I = -0.1;

The system was far from fully stable with these values, but it seemed to hold the zero point much better with opposite P gains. And when looking at a graph with current_p and current_q it made it so the resting value of current_d with a target of zero was centered around a number that was ~0A instead of centering around -2.5A like it would with a positive gain. The_negative_ I gain for current_q kept the motor from almost immediately taking all available current for any target with magnitude greater than 2, as would happen when the gain was positive.

My (somewhat limited) knowledge of PID controllers tells me that negative gains are not supposed to happen, so I am fairly sure this is indicative of a larger issue. However, what exactly that might be is unclear to me. Something about how current_q and current_d are being calculated? Improper signs on the gain for the current sensors? Need more/less filtering?

1 Like
16

After a bit more playing around I have yet to get it working well.

I took a step back to ensure the current sensors were working well - so I tried allowing the current sense alignment to go forward. What I found curious was that no matter what I did to the current gains before the alignment happened I always got a MOT: Success: 3 return status from the alignment step.

I then set the current gains to print after alignment, and it gives phase A’s as negative every time. However, it still returns Success: 3 even when I tell it to flip gain_a before calling motor.initFOC(). Is this expected behavior?

Secondly, knowing that it is the A gain that should be flipped in my setup, I tried using the following instead of what was proposed ahead (difference being that the flipped gain is a rather than b).

// init current sense
current_sense.init();
...
// invert phase b gain
current_sense.gain_a *=-1;
// skip alignment
current_sense.skip_align = true;
...
// link motor and current sense
motor.linkCurrentSense(&current_sense);

It seems like this fixes the previous issue of the current PID gains needing to have opposite signs, however it doesn’t seem to have been the last of my problems as I still can get a good PID tuning. I’m still at the point where it works okay with just a P term, though it either vibrates at a target torque of 0 or is somewhat slow/diminished when reacting to non-zero targets - depending on both the P and Tf terms.

But any addition of an I term, even 0.005, seems to cause long-term instability. A related symptom that I find odd is that once it starts to get confused due to the I term, if I set it back to zero via the commander for both current_q and current_d the poor behavior stays rather than returning to the generally well-behaved system when beginning with only a P term.

17

Hey @NNelsonwood,

Could you share your code here so that I can verify which one of the phases needs to be inverted.

Also which microcontroller are you using?

18

@Antun_Skuric, thanks again for all your help. I am using the SAMD21 Dev Breakout from SparkFun.

My code in its entirety:

#include <SimpleFOC.h>

/***********************************
 *********** CONSTANTS *************
 ***********************************/
// Encoder constants
#define ENC_PIN_A 10  // Pin connected to encoder A
#define ENC_PIN_B 11  // Pin connected to encoder B
#define ENC_PIN_I 12  // Pin connected to encoder I/Z
#define ENC_CPR 2000  // Total number of edges in one rotation

// Phase A = BTN1, Phase B = BTN3, Phase C = BTN2 - For current sensing reasons
#define DRV_PIN_PHA 9 // Pin connected to Phase A
#define DRV_PIN_PHB 5 // Pin connected to Phase B
#define DRV_PIN_PHC 6 // Pin connected to Phase C
#define DRV_PIN_ENA 8 // Pin connected to Enable A
#define DRV_PIN_ENB 4 // Pin connected to Enable B
#define DRV_PIN_ENC 7 // Pin connected to Enable C
#define DRV_FRQ     1000 // PWM Frequency [Hz]
#define DRV_V_SUP   24  // Supply voltage [V]
#define DRV_V_MAX   24  // Maximum voltage allowed [V]
#define DRV_C_MAX   10  // Maximum current allowed [A]
#define DRV_VEL_MAX 220  // Maximum velocity allowed [rad/s]

#define CUR_RES  0.001 // Shunt resistor [Ω]
#define CUR_GAIN 50    // Current sensor gain (from part number)
#define CUR_PIN_PHA A0 // Pin connected to current sensor 1
#define CUR_PIN_PHB A1 // Pin connected to current sensor 2

#define MOT_POL_PR 7          // Number of pole pairs in motor
#define MOT_PHS_RES 0.047858  // Phase resistance of turnigy motor [Ω]
#define MOT_PHS_IND 2.9971390e-05 // Phase inductance of turnigy motor [H]
//#define MOT_PHS_RES 0.0335543 // Phase of oDrive Motor [Ω]

#define MONITOR_PERIOD 1 // [ms] time between montor printing

/***********************************
 ********** GLOBAL VARS ************
 ***********************************/
long timer_monitor = millis();

Encoder encoder = Encoder(ENC_PIN_A, ENC_PIN_B, ENC_CPR / 4, ENC_PIN_I);
// interrupt routine intialisation
void doA() {encoder.handleA();}
void doB() {encoder.handleB();}
void doIndex() {encoder.handleIndex();}

BLDCDriver3PWM driver = BLDCDriver3PWM(DRV_PIN_PHA, DRV_PIN_PHB, DRV_PIN_PHC,
                                       DRV_PIN_ENA, DRV_PIN_ENB, DRV_PIN_ENC);

InlineCurrentSense current_sensor = InlineCurrentSense(CUR_RES, CUR_GAIN, CUR_PIN_PHA, CUR_PIN_PHB);

BLDCMotor motor = BLDCMotor(MOT_POL_PR, MOT_PHS_RES);

Commander commander = Commander(SerialUSB);
void doMotor(char* cmd) {commander.motor(&motor, cmd);}
void doTarget(char* cmd) {commander.scalar(&motor.target, cmd);}

/***********************************
 ************* SETUP ***************
 ***********************************/
void setup() {
  // Monitoring port
  SerialUSB.begin(115200);

  _delay(5000); // Delay that doesn't block interrupts

  // Encoder setup
//  encoder.quadrature = Quadrature::ON;          // Enable/Disable quadrature mode
//  encoder.pullup = Pullup::USE_EXTERN;          // Internal/External pullup resistors
  encoder.init();                               // Initialize encoder
  encoder.enableInterrupts(doA, doB, doIndex);  // Add hardware interrupts to encoder

  _delay(250);

  // Driver setup
  driver.pwm_frequency = DRV_FRQ;           // PWM frequency [Hz]
  driver.voltage_power_supply = DRV_V_SUP;  // Supply voltage [V], real or theoretical??
//  driver.voltage_limit = DRV_V_MAX;       // Maximum voltage [V]
  driver.init();                            // Initialize driver
  driver.enable();

  _delay(250);

  // Current sensing setup
  current_sensor.init();
  current_sensor.gain_a *= -1; // Flip value on phase a
//  current_sensor.gain_b *= -1; // Flip value on phase b
//  current_sensor.gain_c *= -1; // Flip value on phase a
//  current_sensor.skip_align = true; // Don't need as gains set above

//  SerialUSB.println("Current sensor ready");
  _delay(250);

  // Motor setup
  motor.linkSensor(&encoder);
  motor.linkDriver(&driver);
  motor.linkCurrentSense(&current_sensor);
//  motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // One of four modulation types
//  motor.voltage_sensor_align = 8;                           // Voltage used during alignment [V]
//  motor.velocity_index_search = PI;                         // Velocity used during index search [rad/s]
//  motor.sensor_offset = 0;                                  // Can offset the sensor 0 [rad]
  motor.torque_controller = TorqueControlType::foc_current; // One of three torque control types
  motor.controller = MotionControlType::torque;             // One of five motion control types

  // PID calculations as used by oDrive
  float current_p =  DRV_FRQ * MOT_PHS_IND;
  float plant_pole = MOT_PHS_RES / MOT_PHS_IND;
  float current_i = plant_pole * current_p;

  // Current PID controllers
//  motor.PID_current_q.P = current_p;
//  motor.PID_current_q.I = current_i;
  motor.PID_current_q.P = 0.25;
  motor.PID_current_q.I = 0.0;
  motor.PID_current_q.D = 0.0;
//  motor.PID_current_q.limit = motor.voltage_limit;
//  motor.PID_current_q.output_ramp = 1000;
  motor.LPF_current_q.Tf = 0.08;

//  motor.PID_current_d.P = current_p;
//  motor.PID_current_d.I = current_i;
  motor.PID_current_d.P = 0.25;
  motor.PID_current_d.I = 0.0;
  motor.PID_current_d.D = 0.0;
//  motor.PID_current_d.limit = 2;
//  motor.PID_current_d.output_ramp = 1000;
  motor.LPF_current_d.Tf = 0.08;

  // Motion PID controllers
//  motor.PID_velocity.P = 0.2;
//  motor.PID_velocity.I = 5;
//  motor.PID_velocity.D = 0.001;
//  motor.PID_velocity.output_ramp = 1e6; // jerk limit, rarely nead to change
//  motor.LPF_velocity.Tf = 0.01; // Velocity low pass filter time constant
//
//  motor.P_angle.P = 25;
//  motor.P_angle.I = 0;
//  motor.P_angle.D = 0;
//  motor.P_angle.output_ramp = 1e6; // Acceleration limit

  motor.velocity_limit = DRV_VEL_MAX;
//  motor.voltage_limit = 24;           // [V] defaults to driver.voltage_limit
  motor.current_limit = DRV_C_MAX;      // [A] defaults to 0.5

//  motor.motion_downsample = 5; // Run once for every _ motor.motion() calls, can effect motion tuning

  motor.useMonitoring(SerialUSB); // Enabel real-time monitoring and status messages
  // Choose what to monitor real time from: _TARGET, _VOLT_Q, _VOLT_D, _CURR_Q, _CURR_D, _VEL, _ANGLE
//  motor.monitor_variables = _MON_TARGET | _MON_VEL;// | _MON_ANGLE;
//  motor.monitor_variables = _MON_TARGET | _MON_CURR_Q | _MON_CURR_D;
  motor.monitor_variables = 0;// 
  motor.monitor_downsample = 100; // Output once for every _ motor.monitor() calls

  motor.init();
  motor.initFOC(); // Align sensor and start FOC

  commander.decimal_places = 4; // default 3
  commander.verbose = VerboseMode::user_friendly; // One of three output modes
//  commander.verbose = VerboseMode::nothing;
  commander.add('M', doMotor, "motor");
  commander.add('T', doTarget, "target");

  SerialUSB.println("Target,Current_q,Current_d");
  _delay(1000); // Delay that doesn't block interrupts
}

/***********************************
 ************** LOOP ***************
 ***********************************/
void loop() {
//  nonMotorTests();
//  printCurrentSensor();

  if (millis() - timer_monitor > MONITOR_PERIOD) {
//    printMonitoredValues();
    timer_monitor = millis();
  }

  motor.loopFOC();  // iterative setting of FOC phase voltage
  motor.move();     // iterative functiuon setting outer loop target
  motor.monitor();
  commander.run();    // User communications

}


/***********************************
 ************ FUNCTIONS ************
 ***********************************/

// Print chosen monitor values
void printMonitoredValues() {
  
  SerialUSB.print(motor.target);
  SerialUSB.print(",");
//  SerialUSB.print(motor.shaft_angle);
//  SerialUSB.print(",");
//  SerialUSB.print(motor.shaft_velocity);
//  SerialUSB.print(",");
//  SerialUSB.print(motor.voltage.q);
//  SerialUSB.print(",");
  SerialUSB.print(motor.current.q);
  SerialUSB.print(",");
//  SerialUSB.print(motor.voltage.d);
//  SerialUSB.print(",");
  SerialUSB.print(motor.current.d);
//  SerialUSB.print(",");
//  SerialUSB.print(current_sensor.getDCCurrent());
  SerialUSB.print("\n");
  
}

// Test everything without a motor
void nonMotorTests() {
  SerialUSB.println();

  // iterative function updating the sensor internal variables, usually called in motor.loopFOC()
  encoder.update();
  printEncoder();

  driver.setPwm(1.5, 5, 3.25); // Set PWM so phase A = 3V, phB = 5V, phC = 3.25V

  printCurrentSensor();

  _delay(100);
}

// Print current encoder angle and velocity
void printEncoder() {

  float current_angle = encoder.getAngle();
  current_angle = current_angle * 180 / PI;

  float current_vel = encoder.getVelocity();

  SerialUSB.print("Angle: ");
  SerialUSB.print(current_angle);
  SerialUSB.print(" [deg] \t Velocity: ");
  SerialUSB.print(current_vel);
  SerialUSB.println(" [rad/s]");
}


// Print readings from current sensors
void printCurrentSensor() {

  PhaseCurrent_s currents = current_sensor.getPhaseCurrents();
  float current_magnitude = current_sensor.getDCCurrent();

  SerialUSB.print("Phase A: ");
  SerialUSB.print(currents.a);
  SerialUSB.print(" [A] \t Phase B: ");
  SerialUSB.print(currents.b);
  SerialUSB.print(" [A] \t Phase C: ");
  SerialUSB.print(currents.c);
  SerialUSB.print(" [A] \t Magnitude: ");
  SerialUSB.print(current_magnitude);
  SerialUSB.println(" [A]");
}
19

Hey @NNelsonwood,
1khz pwm frequency is too low. Basically you need to have the pwm frequency at least few times higher than the loop execution time. I would guess loop execution time is around 200-500us (2-5khz). So I’d suggest using the pwm frequency of at least 5khz (more like 10khz).