Below is the code I am using (MOT1, ENC1 and MOT2, ENC2 are used for two shields)
#include <SimpleFOC.h>
// MOT1 Pinout test (PASS)
#define MOT1_A 16 //5
#define MOT1_B 13 //9
#define MOT1_C 27 //6
#define MOT1_EN 12 //8
#define ENC1_A 19 //12 (change to 12->19)
#define ENC1_B 26 //2
// MOT2 Pinout test (PASS)
#define MOT2_A 25 //3
#define MOT2_B 5 //10
#define MOT2_C 23 //11
#define MOT2_EN 14 //7
#define ENC2_A 39 // (12->19), (A5->39), (3->25)
#define ENC2_B 36 // (A4->36), (2->26)
// Motor instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(MOT1_A, MOT1_B, MOT1_C, MOT1_EN);
// BLDCDriver3PWM driver = BLDCDriver3PWM(MOT2_A, MOT2_B, MOT2_C, MOT2_EN);
// encoder instance
Encoder encoder = Encoder(ENC1_A, ENC1_B, 500); // changing from 2048 to 500 helps reducing motor jitter by a lot !
// Encoder encoder = Encoder(ENC2_A, ENC2_B, 500); // changing from 2048 to 500 helps reducing motor jitter by a lot !
// Interrupt routine intialisation
// channel A and B callbacks
void doA() {
encoder.handleA();
}
void doB() {
encoder.handleB();
}
// angle set point variable
float target_angle = 0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) {
command.scalar(&target_angle, cmd);
}
void setup() {
// initialize encoder sensor hardware
encoder.init();
encoder.enableInterrupts(doA, doB);
// link the motor to the sensor
motor.linkSensor(&encoder);
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 12;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
// aligning voltage [V]
motor.voltage_sensor_align = 12;
// index search velocity [rad/s]
motor.velocity_index_search = 3;
// set motion control loop to be used
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
motor.controller = MotionControlType::angle;
// contoller configuration
// default parameters in defaults.h
// velocity PI controller parameters
motor.PID_velocity.P = 0.3f;
motor.PID_velocity.I = 0.7f;
// default voltage_power_supply
motor.voltage_limit = 12;
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
motor.PID_velocity.output_ramp = 10000;
// velocity low pass filtering time constant, lower value means less filtering
motor.LPF_velocity.Tf = 0.1f; // ! very important to filter more
// angle P controller
motor.P_angle.P = 20;
// motor.P_angle.I = 0.5f;
// maximal velocity of the position control
// motor.velocity_limit = 10;
// For many motion control applications it will make sense run multiple torque control loops for each motion control loop. This can have a great impact on the smoothness and can provide better high-speed performance.
// Therefore this library enables a very simple downsampling strategy for the move() function which is set using the parameter motor.motion_downsample:
motor.motion_downsample = 5; // - times (default 0 - disabled)
// use monitoring with serial
Serial.begin(115200);
// comment out if not needed
motor.useMonitoring(Serial);
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();
// add target command T
command.add(‘T’, doTarget, “target angle”);
Serial.println(F(“Motor ready.”));
Serial.println(F(“Set the target angle using serial terminal:”));
_delay(1000);
}
void loop() {
// Serial.println(encoder.getAngle());
// main FOC algorithm function
// the faster you run this function the better
// Arduino UNO loop ~1kHz
// Bluepill loop ~10kHz
motor.loopFOC();
// Motion control function
// velocity, position or voltage (defined in motor.controller)
// this function can be run at much lower frequency than loopFOC() function
// You can also use motor.move() and set the motor.target in the code
motor.move(target_angle);
// function intended to be used with serial plotter to monitor motor variables
// significantly slowing the execution down!!!
// motor.monitor();
// user communication
command.run();
}
