Hi,
I am trying to do close loop control in some DC motors, but it appears that the motor is not receiving the data from the encoder.
I am testing with the example codes.
I already tested the motor and the encoder separately and they work.
For testing the encoder, I used this code:
#include <SimpleFOC.h>
Encoder encoder = Encoder(4, 5, 1680);
// interrupt routine initialization
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}
void setup() {
// monitoring port
Serial.begin(115200);
// enable/disable quadrature mode
encoder.quadrature = Quadrature::OFF;
// initialize encoder hardware
encoder.init();
// hardware interrupt enable
encoder.enableInterrupts(doA, doB);
Serial.println("Encoder ready");
_delay(1000);
}
void loop() {
// IMPORTANT - call as frequently as possible
// update the sensor values
encoder.update();
// display the angle and the angular velocity to the terminal
Serial.print(encoder.getAngle());
Serial.print("\t");
Serial.println(encoder.getVelocity());
}
It prints the right values.
Then I try the following code for velocity control:
#include <Arduino.h>
#include "SimpleFOC.h"
#include "SimpleFOCDrivers.h"
#include "SimpleDCMotor.h"
// DCMotor object
DCMotor motor = DCMotor();
// DCDriver object
DCDriver2PWM driver = DCDriver2PWM(8, 9);
//Encoder
Encoder encoder = Encoder(4, 5, 1680);
// interrupt routine initialization
void doA(){encoder.handleA();}
void doB(){encoder.handleB();}
// Commander object, used for serial control
Commander commander = Commander(Serial);
// motor control function - this is needed to link the incoming commands
// to the motor object
void onMotor(char* cmd){ commander.motor(&motor, cmd); }
void setup() {
// monitoring port
Serial.begin(115200);
while (!Serial) {}; // wait for serial connection
// enable debug output to the serial port
SimpleFOCDebug::enable();
// enable/disable quadrature mode
encoder.quadrature = Quadrature::OFF;
// basic driver setup - set power supply voltage
driver.voltage_power_supply = 6.0f;
driver.voltage_limit = 6.0f;
// Optionally set the PWM frequency.
driver.pwm_frequency = 5000;
// init driver
driver.init();
// initialize encoder hardware
encoder.init();
// hardware interrupt enable
encoder.enableInterrupts(doA, doB);
// link motor to driver
motor.linkDriver(&driver);
// link sensor to motor
motor.linkSensor(&encoder);
// set a voltage limit on the motor, optional. The value set here
// has to be lower than the power supply voltage.
motor.voltage_limit = 6.0f;
motor.velocity_limit = 500.0f;
// control type - for this example we use velocity mode.
motor.controller = MotionControlType::velocity;
motor.torque_controller = TorqueControlType::voltage;
// init motor
motor.init();
// set the PID parameters for velocity control.
motor.PID_velocity.P = 0.05f;
motor.PID_velocity.I = 0.02f;
motor.PID_velocity.D = 0.0f;
// output ramp limits the rate of change of the velocity, e.g. limits the
// accelleration.
motor.PID_velocity.output_ramp = 200.0f;
// low pass filter time constant. higher values smooth the velocity measured
// by the sensor, at the cost of latency and control responsiveness.
motor.LPF_velocity.Tf = 0.01f;
// set the target velocity to 0, we use the commander to set it later
motor.target = 0.0f;
// enable motor
motor.enable();
// add the motor and its control function to the commander
commander.add('M', onMotor, "dc motor");
// enable monitoring on Serial port
motor.useMonitoring(Serial);
Serial.println("Initialization complete.");
}
void loop() {
motor.move(); // target speed can be set via commander input
// call commander.run() once per loop iteration, it will process incoming commands
commander.run();
// call motor.monitor() once per loop iteration, it will print the motor state
motor.monitor();
}
When I set a target for the motor to reach, the motor never stops ramping up the velocity. I am pretty sure this is caused for the “I” term in the PID. I think this because the monitor does not print the position neither the velocity.
This is my first time using this library so is very probable that it is user error, but I don’t have any clue what it is.
I will be incredibly grateful if someone could help me.
