Hi Everyone! I could really use some help with this one…
I am trying to create a code to assign the motor a current based on the given position. At a “detent” the motor would have very little to no current and then as you move away from that detent, the current would increase to try and move the motor back. I do not want to use a PID controller because I need to be able to change the feel of each detent. For example I could make a linear equation or expontential pulling you back to the dentent position to create a differnent feel.
I am currently using torque motion control type and foc_current torque control type.
Here is my code:
#include <SimpleFOC.h>
///////////////////// DEFINE ENCODER VARIABLES /////////////////////
int ppm = 2048; // Enter the encoder pulse per revolution number
int indexA = 2; // Enter the encoder channel pin A
int indexB = 3; // Enter the encoder channel pin B
int indexI = 4; // Enter the encoder channel pin Index
int sensor_align_voltage = 5; // voltage used during sensor aligning
///////////////////// SETUP ENCODER VARIABLES /////////////////////
Encoder encoder = Encoder(indexA, indexB, ppm, indexI); // Encoder instance
void doA(){encoder.handleA();} // interrupt routine intialisation for A
void doB(){encoder.handleB();} // interrupt routine intialisation for B
//InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2); // current sensor, not sure how it gets the q and d currents though)
void doIndex(){encoder.handleIndex();} // interrupt routine intialisation for Index
///////////////////// DEFINE MOTOR VARIABLES /////////////////////
int pole_pairs = 1; // Enter the pole pair number for the motor
int pwmA = 9; // Enter the motor channel pin pwmA
int pwmB = 5; // Enter the motor channel pin pwmB
int pwmC = 6; // Enter the motor channel pin pwmC
///////////////////// SETUP MOTOR VARIABLES /////////////////////
BLDCMotor motor = BLDCMotor(pole_pairs); // BLDCMotor motor = BLDCMotor(pole pair number);
BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, 8); // BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
///////////////////// DETENT SETTINGS /////////////////////
const float deg_to_rad = PI / 180.0; // Conversion factor from degrees to radians
const float detent_positions_deg[] = {0, 45, 90, 135, 180, 225, 270, 315}; // Detent positions in degrees
const float detent_positions[] = {
detent_positions_deg[0] * deg_to_rad,
detent_positions_deg[1] * deg_to_rad,
detent_positions_deg[2] * deg_to_rad,
detent_positions_deg[3] * deg_to_rad,
detent_positions_deg[4] * deg_to_rad,
detent_positions_deg[5] * deg_to_rad,
detent_positions_deg[6] * deg_to_rad,
detent_positions_deg[7] * deg_to_rad
}; // Convert to radians
float tolerance = 5 * deg_to_rad; // 5 degrees tolerance in radians
float fixed_current = 1.5; // Fixed current to apply for correction
float current_position = 0.0; // Set first current position to 0;
void setup() {
Serial.begin(115200); // use monitoring with serial
SimpleFOCDebug::enable(&Serial); // enable more verbose output for debugging
///////////////////// ENCODER SETUP /////////////////////
encoder.quadrature = Quadrature::ON; // enable/disable quadrature mode
encoder.init(); // Initialize Encoder
encoder.enableInterrupts(doA, doB, doIndex); // enabling interupts for encoder pins
motor.linkSensor(&encoder); // Linking encoder to motor
Serial.println("Encoder Ready");
delay(1000);
///////////////////// DRIVER SETUP /////////////////////
driver.voltage_power_supply = 12; // power supply voltage [V]
driver.init(); // Initialize Driver
motor.linkDriver(&driver); // Linking driver to motor
current_sense.linkDriver(&driver); // Linking current sense to driver
current_sense.init(); // initializing current sense
motor.linkCurrentSense(¤t_sense); // linking current sense to motor
Serial.println("Driver Ready");
delay(1000);
///////////////////// MOTOR SETUP /////////////////////
motor.voltage_limit = 10; // maximum voltage that can be sent to motor [V]
motor.current_limit = 2.5; // maximum current that can be sent to motor [A]
motor.velocity_limit = 5*PI/180; // maxium velocity motor is allowed to go [deg/s] converted into [rad/s]
motor.voltage_sensor_align = sensor_align_voltage; // aligning voltage and sensor [V]
motor.init(); // Initialize Motor
Serial.println("Motor Ready");
delay(1000);
///////////////////// FOC CONTROL TYPE /////////////////////
motor.controller = MotionControlType::torque;
motor.torque_controller = TorqueControlType::foc_current; // options: voltage, dc_current, foc_current
motor.initFOC(); // Start FOC
Serial.println("FOC Ready");
delay(1000);
// Print First Angle //
Serial.println(current_position);
}
void loop() {
encoder.update(); // Updating the sensors internal variables
float current_position = encoder.getMechanicalAngle(); // Get the current position of the mechanical shaft angle in the range 0 to 2PI
// Determine the current to apply based on position
float current_target = 0;
for (int i = 0; i < sizeof(detent_positions)/sizeof(detent_positions[0]); i++) {
float detent_position = detent_positions[i];
if (current_position >= detent_position - tolerance && current_position <= detent_position + tolerance) {
// Within tolerance, set current to 0
current_target = 0;
break;
} else if (current_position > detent_position + tolerance) {
// Apply negative current to move back towards detent
current_target = -fixed_current;
} else if (current_position < detent_position - tolerance) {
// Apply positive current to move back towards detent
current_target = fixed_current;
}
}
// Apply the control current to the motor
motor.move(current_target);
Serial.print("Current Position:");
Serial.println(current_position*180/PI);
Serial.print("Assigned Current (amps):");
Serial.println(current_target);
// Add a small delay to allow for stable control
delay(100);
}