Hello,
I am currently trying to implement current sensing into my closed loop angle control project.
The current sensing part itself is working I think but as soon as I am trying to either change the foc_modulation or the torque_controller the system does not work when I enter a target angle via Serial.
Here is the code that I am using.
/**
Code for adaptive Steering Wheel EMC
-Get string from Serialport with desired angle command of BLDC inside the steering wheel / Format: “”
-closed loop control of BLDC with SimpleFOC library
-current sensing not implemented yet
/
#define B_G431
#include <SimpleFOC.h>
//Char for SimpleFOC command
char* testangle;
float maxv;
// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(4,0.35,601);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
//test
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
// encoder instance
Encoder encoder = Encoder(PB6, PB7, 2048);
// 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();
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
//variables for Serial Communication(get Angle command as string)
const byte numChars = 32;
char receivedChars[numChars];
boolean newData = false;
char* cangle; //Angle of clockspring
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();
//test
currentSense.linkDriver(&driver);
// link the motor and the driver
motor.linkDriver(&driver);
//Here is the problem
motor.foc_modulation = FOCModulationType::SinePWM;
motor.torque_controller = TorqueControlType::foc_current;
// aligning voltage [V]
motor.voltage_sensor_align = 3;
// set motion control loop to be used
motor.controller = MotionControlType::angle;
// contoller configuration
// default parameters in defaults.h
// velocity PI controller parameters
motor.PID_velocity.P = 0.2f;
motor.PID_velocity.I = 20;
motor.PID_velocity.D = 0;
// default voltage_power_supply
//motor.voltage_limit = 11;
motor.current_limit = 6;
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
motor.PID_velocity.output_ramp = 1000;
// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.01f;
// angle P controller
motor.P_angle.P = 20;
//maximal velocity of the position control
motor.velocity_limit = 500;
// use monitoring with serial
//Serial2.begin(115200);
Serial.begin(115200);
// comment out if not needed
// motor.useMonitoring(Serial);
// initialize motor
motor.init();
//test
currentSense.init();
if (currentSense.init()) Serial.println(“Current sense init success!”);
else{
Serial.println(“Current sense init failed!”);
return;
}
//currentSense.skip_align = true;
motor.linkCurrentSense(¤tSense);
// 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);
maxv = 0;
}
void loop() {
// 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
//Recieve angle over Serial
recvWithStartEndMarkers();
showNewData();
cangle = receivedChars;
//Serial.println(cangle);
command.run(cangle);
if(abs(encoder.getVelocity())>maxv)
{
maxv = abs(encoder.getVelocity());
Serial.println(maxv);
}
}
//function for recieving string
void recvWithStartEndMarkers() {
static boolean recvInProgress = false;
static byte ndx = 0;
char startMarker = ‘<’;
char endMarker = ‘>’;
char rc;
while (Serial.available() > 0 && newData == false) {
analogWrite(LED_BUILTIN,175);
rc = Serial.read();
if (recvInProgress == true) {
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
recvInProgress = false;
ndx = 0;
newData = true;
}
}
else if (rc == startMarker) {
recvInProgress = true;
}
}
}
//complimentary function for void recvWithStartEndMarkers()
void showNewData() {
if (newData == true) {
//Serial.print("This just in … ");
//Serial.println(receivedChars);
newData = false;
}
}
Am I missing some configurations or is the code in the wrong place ?
I already tried to look into some threads in the community but I can’t seem to find where my error is comming from.