FOC V2 - Code

25/05/2024 | Views: 6103 | Arduino | by: ELECTRONOOBS

I have two boards, one with the AS 5600 sensor and the other with the AS5048. Even if the AS5048 is a lot more expensive, for some reason I only was able to make it work in PWM mode and PWM mode is not as precise as i2c. That’s why I’ve decided to continue with the 5600. So first test is to download the amd 56 00 library and run the read angle code. Upload the code and open the monitor. As you can see, I can now see the angle when rotating the motor so the sensor is working.

For the next part we have to remember the open loop control first. If you remember, this was the code and I was using SPWM signals using this lookup table. I’ve used the same approach as in my sinusoidal inverter project using SPWM.

Check the closed loop code with PID control that I’ve made. First we read the potentiometer for the setpoint. Then we read the real angle from the magnetic sensor. Then we divide the angle for the electrical angle. You see, is important to know the amount of poles pairs our motor has. As I’ve told you, my motor has 22 magnets so a total of 11 pole pairs. So in order to make a full real rotation, we are making 11 electrical rotations inside of the coils.

That’s why here in the code we map the real shaft positions according to the electrical rotations. Then we calculate the PID error. Depending on this error, we first decide if we will rotate clockwise or counterclockwise.
Then we make the PID code, which is actually just PD, I haven’t used the I variable. The P value is proportional so just the error multiplied by the P constant. The D value needs the elapsed time and the error difference and finally, we sum up the total PID output as this PID torque variable. I’ve placed the PWM signals in a separate function, and here, depending on the position, we calculate the SPWM signal using again the SINE function. Then apply the values to the output pins and that's it.

×

```
/********************************************************************************************************
* Title: ELECTRONOOBS open source FOC controller.
* Date: 25/05/2024
* Version: 2.5
* Author: http://electronoobs.com
* Based on tutorial: https://www.electronoobs.com/eng_arduino_tut198.php
* This is a sensored ESC with FOC implemented based on Arduino with the ATmega328 chip.
* It uses SPWM signals and PID to control the position of brushless motors.
* Subscribe: https://youtu.be/pzGRKyHlXsM
********************************************************************************************************/
#include <Wire.h>
#include <AS5600.h>
#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
#define SERIAL SerialUSB
#define SYS_VOL 3.3
#else
#define SERIAL Serial
#define SYS_VOL 5
#endif
//Variables for the hall effect angle sensor
AMS_5600 ams5600;
int ang, lang = 0;
//Inputs or outputs
const int potentiometer = A3; // pot controls the RPM speed
const int MotorPinPhase_A =9; //Driver's pin for coil A
const int MotorPinPhase_B =10; //Driver's pin for coil B
const int MotorPinPhase_C =11; //Driver's pin for coil C
const int buzzer = A2; //Pin for the on board buzzer
const int fanPin = 5; //Pin for fan MOSFET
const int supplyVoltagePin = A1; //Pin for reading the supply voltage
const int temperaturePin = A0; //Pin for reading the temperature with the NTC
const int PWMSensor = 2; //PWM output from the magnetic encoder sensor (not used in this code...)
//The coils have a phase difference of 120 degrees so between each coil we have 120
//Here is where we will store the SPWM value for each coil
int SPWMValueA=0;
int SPWMValueB=120;
int SPWMValueC=240;
//Variables the user could change depending on teh used motor ...
const int motorPoles = 11; //PP value is pole pair (S and N) of the motor. (22 magnets = 11 pp)
int TorqueValue=100; //Set torque value from 0 to 100%;
/*The motor has ¡the real position in degrees from 0 to 360 but also the electrical position
* The electrical position is affected by the amount of "pp" or pole pairs. In this case my motor
* has 11 pairs, so for 360 degrees of real shaft rotation, we need 11 electrical rotations */
int RealShaftPosition=0; //Here we store the read position of shaft from the sensor
int ElectricalPosition=0; //Here we store the electrical position of shaft
int RotationDirection=0; //Magnetic field orientation vector so we know if we rotate CW or CCW
//PID Variables
float PID_P = 0; //Here we stopre the P gain
float PID_D = 0; //Here we stopre the D gain
float Kp = 25; //P is the proportional gain for the PID control (no I yet...)
float Kd = 32; //D is the derivative gain for the PID control (no I yet...)
float PID_setpoint=0; //Here we store the setpoint for the PID calculations
float PID_error=0; //Here we store the error for the PID calculations (error = real_value - setpoint)
float previousPID_error=0; //This is used for the D part of the PID calculations
float elapsedTime, currentTime, previousTime; //Variables for time control
int PID_Torque=0; //Here we will store the tortal PID torque affected by the PID code
void setup() {
Serial.begin(115200);
Wire.begin();
TCCR1B = TCCR1B & 0b11111000 | 0x01; // fix PWM frequency at 31250 Hz for Pins 9 and 10
TCCR2B = TCCR2B & 0b11111000 | 0x01; // fix PWM frequency at 31250 Hz for Pins 11 and 3
ICR1 = 255 ; // 8 bit resolution
pinMode(potentiometer, INPUT); //Set pot pin as input
pinMode(supplyVoltagePin, INPUT); //Set voltage in pin as input
pinMode(temperaturePin, INPUT); //Set temperature read pin as input
pinMode(MotorPinPhase_A, OUTPUT); //Set driver pins as outputs
pinMode(MotorPinPhase_B, OUTPUT); //Set driver pins as outputs
pinMode(MotorPinPhase_C, OUTPUT); //Set driver pins as outputs
pinMode(buzzer, OUTPUT); //Set buzzer as output
pinMode(fanPin, OUTPUT); //Set fanPin as output
digitalWrite(buzzer, LOW); //Start with the buzzer off
digitalWrite(fanPin, LOW); //Start with the fan off (temperature not yet implemented)
//Start the magnetic sensor and check for magnet...
if(ams5600.detectMagnet() == 0 ){
while(1){
if(ams5600.detectMagnet() == 1 ){
SERIAL.print("Current Magnitude: ");
SERIAL.println(ams5600.getMagnitude());
break;
}
else{
SERIAL.println("Can not detect magnet");
}
delay(200);
}
}//End of setting the magnet sensor
currentTime = millis();
//tone(buzzer, 2000, 1000);
}//End of SETUP LOOP
void loop() {
//Read the setpoint from the potentiometer
PID_setpoint = analogRead(potentiometer); //Read the analog value of the potentiometer
PID_setpoint = map(PID_setpoint,0,1023,0,360); //Map that value to a range of 0 to 360º
//Read the real angle from the sensor
float rawangle = ams5600.getRawAngle(); //RAW read from the sensor (not in degrees yet)
float realAngle = rawangle * 0.087890625; //Value from sensor datasheet
realAngle = constrain(realAngle, 0, 360); //Constraint the values to a range of 0 to 360º
ElectricalPosition=map(realAngle,0,360,0,(360*motorPoles)); //We need to scale the electrical position depending on the amount of pole pairs
ElectricalPosition=constrain(ElectricalPosition, 0, 360*motorPoles); //Constraint the values once again
RealShaftPosition=map(ElectricalPosition,0,(360*motorPoles),0,360);
// We first calculate the PID error between the value we want and the value we have (in degrees)
PID_error=RealShaftPosition-PID_setpoint; //First as always, wec alculate teh PID error
//Decide if we go CW or CCW
if (PID_error<0){RotationDirection = 90;} //Magnetic field vector is now 90 degrees ahead so CW
if (PID_error>0){RotationDirection = 270;} //Magnetic field vector is now 90 degrees behind so CCW
////////////PID Calculations///////////////
//Calculate the P gain
PID_P = Kp * PID_error;
//Calculate the D gain
currentTime = millis();
elapsedTime = currentTime - previousTime;
PID_D = Kd*((PID_error - previousPID_error)/elapsedTime);
previousTime = currentTime;
previousPID_error = PID_error;
//Sum up P and D gains
PID_Torque=abs(PID_P + PID_D); //Define the gain of the PID system.
PID_Torque=constrain(PID_Torque,0,TorqueValue); //Limit the torque 0-100%. The driver might get HOT so lower the value to maybe 50% or so...
rotate(); //Run the function that will apply the SPWM values to the driver
}//End of VOID LOOP
void rotate(){
const int SyncOffset=21; //You might neet do adjust this value (offset between where the real angle starts and the electrical angle starts)
SPWMValueA = ElectricalPosition + RotationDirection + SyncOffset; //We add the position and direction so it will rotate CW or CW
SPWMValueB = SPWMValueA + 120; //120 degrees of phase difference between coils A and B
SPWMValueC = SPWMValueB + 120; //120 degrees of phase difference between coils B and C
SPWMValueA = SPWMValueA%360; //Keep the values between 0 and 360 degrees
SPWMValueB = SPWMValueB%360;
SPWMValueC = SPWMValueC%360;
//Calculate the PWM values for creating a sine signal (SPWM)
int SINE_A_PWM = sin((double)SPWMValueA*PI/360)*127.5+127.5; //Multiply by PI and divide by 180 in order to pass from degrees to radians
int SINE_B_PWM = sin((double)SPWMValueB*PI/360)*127.5+127.5; //Multiply by 127.5 and add 127.5 in order to keep the range between 0-255 (8 bit PWM signal)
int SINE_C_PWM = sin((double)SPWMValueC*PI/360)*127.5+127.5; //SINE values between -1 and 1 are placed between 0-255 for PWM.
//Write SPWM values to each pin of the driver
analogWrite(MotorPinPhase_A, SINE_A_PWM*PID_Torque/100);
analogWrite(MotorPinPhase_B, SINE_B_PWM*PID_Torque/100);
analogWrite(MotorPinPhase_C, SINE_C_PWM*PID_Torque/100);
}//End of moving function
```

>