Hello,
I have written a code (in C language) to control two SparkFun 12V 3-Phase BLDC motors with encoders AS5048A Gimbal Motor with Encoder - 12V, 467RPM - SparkFun Electronics via two SparkFun 11V motor-drivers SparkFun Brushless Motor Driver - 3-Phase (TMC6300) - SparkFun Electronics that were connected to the SparkFun RedBoard Artemis ATP microcontroller board. After compiling and uploading the code successfully, the motors did not move. All the LED of the microcontroller board and motor-drivers were on. A battery was charged up to 7.91V. Could someone please specify what is wrong with the code? Here is my code:
/*
The following code was built based on the example SimpleFOC control
code published on this webpage " Position Control example | Arduino-FOC ",
and other information published on these webpages " Velocity Open-Loop | Arduino-FOC " ,
" Library Source | Arduino-FOC " by editing the example SimpleFOC control code.
The short paper about SimpleFOC was published:
SimpleFOC: A Field Oriented Control (FOC) Library for Controlling
Brushless Direct Current (BLDC) and Stepper Motors. A. Skuric, HS. Bank, R. Unger,
O. Williams, D. González-Reyes Journal of Open Source Software, 7(74), 4232,
*/
#include <SimpleFOC.h>
#include <Arduino.h>
// Setting Motor #1 and Motor #2 having 11 coil pairs
BLDCMotor motor1 = BLDCMotor(11);
BLDCMotor motor2 = BLDCMotor(11);
// Initialize two 6PWM motor-drivers by assigning HIGH_Side pins and LOW_Side pins in sequence
// ending with the motor-drivers En/VIO/STANDBY pins D17 and D1 appropriately
const int VIO_PIN1 = D17;
const int VIO_PIN2 = D1;
BLDCDriver6PWM driver1 = BLDCDriver6PWM(D26, D19, D22, D4, D27, D23, VIO_PIN1);
BLDCDriver6PWM driver2 = BLDCDriver6PWM(D37, D18, D45, D44, D42, D43, VIO_PIN2);
//Initialize variables for the PWM pins of the PWM wires of the encoders
const int PWM_pin_encoder1 = A12; // Connect to the AS5048A encoder #1 PWM pin
const int PWM_pin_encoder2 = A13; // Connect to the AS5048A encoder #2 PWM pin
//Initialize variables for encoder #1 and encoder #2 by MagenticSensorPWM
//MagneticSensorPWM is applied because the encoders PWM pins are used
MagneticSensorPWM encoder1 = MagneticSensorPWM(PWM_pin_encoder1, 14, 936);
MagneticSensorPWM encoder2 = MagneticSensorPWM(PWM_pin_encoder2, 14, 876);
// Build Setup() block
void setup(){
Serial.begin(115200);
//Setting drivers
driver1.voltage_power_supply = 7.4;
driver2.voltage_power_supply = 7.4;
// initiate drivers
driver1.init();
driver1.init();
// link motors to drivers
motor1.linkDriver(&driver1);
motor2.linkDriver(&driver2);
// state voltage limit for each motor
motor1.voltage_limit = 8.4;
motor2.voltage_limit = 8.4;
//setting control mode without all PID loops
motor1.controller = MotionControlType::velocity_openloop;
motor2.controller = MotionControlType::velocity_openloop;
// Initialize motors
motor1.init();
motor2.init();
//Initialize encoder #1 and encoder #2
encoder1.init();
encoder2.init();
//delay(??);
}
//SimpleFOC requires to declare loop() to state all other functions:
void loop(){
// Setting both rotors at their initial positions
encoder1.update();
encoder2.update();
// the .getAngle() function should output angles in radians
float initial_angle_rotor1 = encoder1.getAngle();
float initial_angle_rotor2 = encoder2.getAngle();
// Build if-statements to set both rotors at their initial positions
float updated_initial_angle_rotor1;
float updated_initial_angle_rotor2;
// build the if-statement to get Rotor #1 at its initial position
if (initial_angle_rotor1 != 0){
updated_initial_angle_rotor1 = initial_angle_rotor1;
while(updated_initial_angle_rotor1 > 0){
motor1.move(2); // value of the motor #1 velocity is chosen to be positive
delay(1);
encoder1.update();
updated_initial_angle_rotor1 = encoder1.getAngle();
}
// encoder #1 default zero position (zero radian) is Rotor #1 initial position
if (updated_initial_angle_rotor1 == 0){
motor1.move(0);
}
}
// build the if-statement to get Rotor #2 at its initial position
if (initial_angle_rotor2 != (PI/2)){
updated_initial_angle_rotor2 = initial_angle_rotor2;
while(updated_initial_angle_rotor2 > (PI/2) || updated_initial_angle_rotor2 < (PI/2)){
motor2.move(-2); // value of the motor #2 velocity is chosen to be negative
delay(1);
encoder2.update();
updated_initial_angle_rotor2 = encoder2.getAngle();
}
// encoder #2 default zero position (zero radian) is not the Rotor #1 initial position
// Rotor #/2 initial position is (0 + pi/2) radians
if (updated_initial_angle_rotor2 == (0 + PI/2)){
motor2.move(0);
}
}
delay(20000);
// Initialize a variable for a number of cycles (one cycle is 360 degrees)
int num_cycles = 3; // number of cycles is set arbitrarily
// Build a loop to iterate a number of cycles
for (int c = 0; c < num_cycles; c++){
float i = 0;
float angle1 = 0;
float angle2 = 0;
float speed1 = 0;
float speed2 = 0;
// Build loop to update encoder1 input angle and calculate a total angle passed by Rotor #1
// This loop also moves both rotors for one cycle
if (updated_initial_angle_rotor1 == 0 && updated_initial_angle_rotor2 == (0 + PI/2)){
for (i = 0; i <= PI;){
//update input values of encoders
encoder1.update();
encoder2.update();
angle1 = encoder1.getAngle();
angle2 = encoder2.getAngle();
//Make i as the average angle of two angles. If both rotors move almost synchronically, it should work out,
i = (angle1 + (angle2 - PI/2))/2;
// Make speed functions for two 3-Phase BLDC motors so that their speeds increase incrementally
// 250 RPM (Rotations Per Minute) is about 26.18 Radians Per Second (Rad/s)
// about 120 milliseconds needed to pass 180 degrees (pi radians) at the speed 26 rad/s
// rotors require more time to pass 180 degrees because they reach about 26.18 rad/s incrementally
speed1 = (26.18 / PI); // every iteration set constant speed value to prevent excessively fast growth of speed1
speed2 = (26.18 / PI); // every iteration set constant speed value to prevent excessively fast growth of speed2
speed1 = speed1 + (2/3) * (i * 26.18 / PI); // Rotor #1 speed increases incrementally
speed2 = speed2 + (2/3) * (i * 26.18 / PI); // Rotor #2 speed increases incrementally
motor1.move(speed1); // value of the motor #1 velocity is chosen to be positive
motor2.move(-speed2); // value of the motor #2 velocity is chosen to be negative
//delay(1); // delay is set to give motors some time to accelerate
//command.run();
delay(1); // delay is set to give motors some time to accelerate and to iterate the loop
}
}
// Build a loop to move Rotor #1 and Rotor #2 through their inertial zones to their initial positions
float inertial_zone_angle1 = PI;
float inertial_zone_angle2 = (3*PI/2);
if (inertial_zone_angle1 < (2 * PI) && inertial_zone_angle1 > ( PI) && inertial_zone_angle2 < (PI / 2) && inertial_zone_angle2 > (3*PI / 2)){
for (float n = PI; n <= (2 * PI);){
encoder1.update();
encoder2.update();
inertial_zone_angle1 = encoder1.getAngle();
inertial_zone_angle2 = encoder2.getAngle();
//Make i as the average angle of two angles. If both rotors move almost synchronically, it should work out,
n = (inertial_zone_angle1 + (inertial_zone_angle2 - PI/2))/2;
motor1.move(0.01); // slowly move Rotor #1 to its initial position through inertial zone
motor2.move(-0.01); // slowly move Rotor #2 to its initial position through inertial zone
delay(1); // delay is
encoder1.update();
encoder2.update();
inertial_zone_angle1 = encoder1.getAngle();
inertial_zone_angle2 = encoder2.getAngle();
if (inertial_zone_angle1 == 0){
motor1.move(0);
delay(10);
}
if (inertial_zone_angle2 == (0 + PI/2)){
motor2.move(0);
delay(5);
}
}
}
//delay(10); // this delay is between cycles
// buld a loop in case one updated_initial\_
//if (updated_initial_angle_rotor1 == 0 && updated_initial_angle_rotor2 == 0)
}
while(1){delay(100);} //forever being executed without starting loop() again
//return; // start loop() again
}
Thank you.