Hello,
I am trying to implement a code that would run my BLDC motor using Low-side current sensing only. However, it seems like the FOC functions do not work unless I am using a sensor (hall effect / magnetic / encoder). Does anyone know if there is a way around this?
This is the code I have right now, it runs when I uncomment the hall effect sensors code but if I remove that and try to only use Low-side current sensing it prints the following.
Code:
#include <SimpleFOC.h>
unsigned long startMillis; //some global variables available anywhere in the program
unsigned long currentMillis;
const unsigned long period = 2000; //the value is a number of milliseconds
#define CSA 22
#define CSB 14
#define CSC 15
// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver6PWM driver = BLDCDriver6PWM(6, 9, 2, 3, 8, 7, 17);
// HallSensor sensor = HallSensor(23, 21, 20, 11);
// Interrupt routine intialisation
// channel A and B callbacks
// void doA(){sensor.handleA();}
// void doB(){sensor.handleB();}
// void doC(){sensor.handleC();}
// If no available hadware interrupt pins use the software interrupt
// PciListenerImp listenC(sensor.pinC, doC);
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&motor.target, cmd); }
void d_P(char* cmd) { command.scalar(&motor.PID_current_d.P, cmd); }
void d_I(char* cmd) { command.scalar(&motor.PID_current_d.I, cmd); }
void d_D(char* cmd) { command.scalar(&motor.PID_current_d.D, cmd); }
void q_P(char* cmd) { command.scalar(&motor.PID_current_q.P, cmd); }
void q_I(char* cmd) { command.scalar(&motor.PID_current_q.I, cmd); }
void q_D(char* cmd) { command.scalar(&motor.PID_current_q.D, cmd); }
void q_F(char* cmd) { command.scalar(&motor.LPF_current_q.Tf, cmd); }
void d_F(char* cmd) { command.scalar(&motor.LPF_current_d.Tf, cmd); }
// current sensor
// shunt resistor value
// gain value
// pins phase A,B, (C optional)
// InlineCurrentSense current_sense = InlineCurrentSense(0.0001f, 20.0f, CSA, CSB, CSC);
LowsideCurrentSense current_sense = LowsideCurrentSense(0.2f, 20, CSA, CSB, CSC);
// PhaseCurrent_s readCurrentSense(){
// PhaseCurrent_s c;
// // dummy example only reading analog pins
// c.a = analogRead(CSA);
// c.b = analogRead(CSB);
// c.c = analogRead(CSC); // if no 3rd current sense set it to 0
// return(c);
// }
// void initCurrentSense(){
// pinMode(CSA,INPUT);
// pinMode(CSB,INPUT);
// pinMode(CSC,INPUT);
// }
// GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCurrentSense);
void setup() {
// driver init
driver.init();
// link the driver to the current sense
current_sense.linkDriver(&driver);
// for SimpleFOCShield v2.01/v2.0.2
// current_sense.gain_b *= -1;
Serial.begin(115200);
Serial.println(“Current sense ready.”);\
// initialize sensor hardware
// sensor.init();
// sensor.enableInterrupts(doA, doB, doC);
// software interrupts
// PciManager.registerListener(&listenC);
// link the motor to the sensor
// motor.linkSensor(&sensor);
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 12;
motor.voltage_limit = 12;
// link driver
motor.linkDriver(&driver);
driver.pwm_frequency = 20000;
// link the current sense to the motor
motor.linkCurrentSense(¤t_sense);
// set torque mode:
motor.torque_controller = TorqueControlType::foc_current;
// set motion control loop to be used
motor.controller = MotionControlType::torque;
// foc current control parameters
// Q axis
// PID parameters for Teensy 4.0
motor.PID_current_q.P = 0.2f; // Adjusted for Teensy 4.0
motor.PID_current_q.I = 10; // Adjusted for Teensy 4.0
motor.PID_current_q.D = 0;
motor.PID_current_q.limit = motor.voltage_limit;
// motor.PID_current_q.ramp = 1000; // Adjusted for Teensy 4.0
// Low pass filtering for Teensy 4.0
// LPF_current_q.Tf= 0.01; // Adjusted for Teensy 4.0
// D axis
// PID parameters for Teensy 4.0
motor.PID_current_d.P = 0.2f; // Adjusted for Teensy 4.0
motor.PID_current_d.I = 10; // Adjusted for Teensy 4.0
motor.PID_current_d.D = 0;
motor.PID_current_d.limit = motor.voltage_limit;
// motor.PID_current_d.ramp = 1000; // Adjusted for Teensy 4.0
// Low pass filtering for Teensy 4.0
motor.LPF_current_q.Tf = 0.01;
motor.LPF_current_d.Tf = 0.01;
// use monitoring with serial
Serial.begin(115200);
// comment out if not needed
motor.useMonitoring(Serial);
// initialize motor
motor.init();
current_sense.init();
// skip alignment procedure
// current_sense.skip_align = true;
// align sensor and start FOC
motor.initFOC();
// add target command T
command.add(‘T’, doTarget, “target current”);
command.add(‘P’, d_P, “d__P”);
command.add(‘I’, d_I, “d__I”);
command.add(‘D’, d_D, “d__D”);
command.add(‘J’, q_P, “q__P”);
command.add(‘K’, q_I, “q__I”);
command.add(‘L’, q_D, “q__D”);
command.add(‘F’, q_F, “q__F”);
command.add(‘G’, d_F, “d__F”);
startMillis = millis(); //initial start time
Serial.println(F(“Motor ready.”));
Serial.println(F(“Set the target current using serial terminal:”));
motor.useMonitoring(Serial);
_delay(1000);
motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D | _MON_VEL | _MON_ANGLE;
motor.monitor_downsample = 10000; // default 10
motor.monitor();
}
void loop() {
PhaseCurrent_s currents = current_sense.getPhaseCurrents();
float current_magnitude = current_sense.getDCCurrent();
DQCurrent_s current = current_sense.getFOCCurrents(_electricalAngle(motor.shaft_angle, motor.pole_pairs));
currentMillis = millis(); //get the current "time" (actually the number of milliseconds since the program started)
// if (currentMillis - startMillis >= period) //test whether the period has elapsed
// {
// Serial.print(current.d*1000); // milli Amps
// Serial.print("\t");
// Serial.println(current.q*1000); // milli Amps
// // Serial.print("\t");
// // Serial.println(current_magnitude*1000); // milli Amps
// startMillis = currentMillis; //IMPORTANT to save the start time of the current LED state.
// }
// main FOC algorithm function
motor.loopFOC();
// Motion control function
motor.move();
// user communication
command.run();
// motor.monitor();
}
Serial monitor:
11:54:57.735 → Current sense ready.
11:54:57.735 → MOT: Monitor enabled!
11:54:57.735 → MOT: Init
11:54:58.200 → MOT: Enable driver.
11:54:58.731 → TEENSY-CS: Configuring low side current sense!
11:54:58.731 → TEENSY-CS: Syncing low side current sense!
11:54:58.731 → TEENSY-CS: Syncing to FlexPWM: 2, Submodule: 2
11:55:00.730 → MOT: No sensor.
11:55:00.730 → MOT: Init FOC failed.
11:55:00.730 → Motor ready.
11:55:00.730 → Set the target current using serial terminal:
11:55:00.730 → MOT: Monitor enabled!