@runger @fdurante as the SimpleFOC library is still not working I use the following code (hard way) that control the motor in position using as input the IMU. It is a PID that compare and correct the pitch to stay horizontal. But the problem now is that the Shield switch on and off when the code is running, I have the feedback of both encoder and IMU but the motor just move as impulse (blue light of the simpleFOC shield switch on and off). I am using a 12V power supply connected to the shield. The code :
#include “I2Cdev.h”
#include “MPU6050_6Axis_MotionApps20.h”
#include <Wire.h>
#include <SimpleFOC.h>
MPU6050 mpu;
bool dmpReady = false;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
float ypr[3];
const int CS_PIN = 10;
const int MOSI_PIN = PA7;
const int MISO_PIN = PA6;
const int SCK_PIN = PA5;
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
float target_pitch = 0;
float Kp = 0.0001, Ki = 0.0002, Kd = 0;
float integral = 0, previous_error = 0;
Commander command = Commander(Serial);
void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); }
const int printInterval = 100000; // 100 ms in microseconds
unsigned long lastPrintTime = 0;
// SPI bit-banging functions for encoder reading
uint8_t SPIBitBangTransfer(uint8_t dataOut) {
uint8_t dataIn = 0;
for (int i = 7; i >= 0; i–) {
digitalWrite(MOSI_PIN, (dataOut >> i) & 1);
digitalWrite(SCK_PIN, HIGH);
delayMicroseconds(1); // Ideally replace this with non-blocking timer logic if necessary
dataIn <<= 1;
dataIn |= digitalRead(MISO_PIN);
digitalWrite(SCK_PIN, LOW);
delayMicroseconds(1);
}
return dataIn;
}
uint16_t readEncoderAngleBitBang() {
uint16_t angle = 0;
digitalWrite(CS_PIN, LOW);
delayMicroseconds(5);
uint8_t highByte = SPIBitBangTransfer(0xFF);
uint8_t lowByte = SPIBitBangTransfer(0xFF);
digitalWrite(CS_PIN, HIGH);
angle = ((highByte << 8) | lowByte) & 0x3FFF;
return angle;
}
void setup() {
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000);
Serial.println("Initializing MPU6050...");
mpu.initialize();
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
if (devStatus == 0) {
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
dmpReady = true;
Serial.println("DMP ready!");
} else {
Serial.print("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println(")");
}
pinMode(CS_PIN, OUTPUT);
pinMode(MOSI_PIN, OUTPUT);
pinMode(MISO_PIN, INPUT);
pinMode(SCK_PIN, OUTPUT);
digitalWrite(CS_PIN, HIGH);
digitalWrite(SCK_PIN, LOW);
driver.voltage_power_supply = 12;
driver.voltage_limit = 12;
if (!driver.init()) {
Serial.println("Driver init failed!");
return;
}
motor.linkDriver(&driver);
motor.voltage_limit = 12;
motor.controller = MotionControlType::velocity_openloop;
if (!motor.init()) {
Serial.println("Motor init failed!");
return;
}
command.add('L', doLimit, "voltage limit");
Serial.println("Setup complete!");
delay(1000);
}
void loop() {
if (!dmpReady) return;
unsigned long currentMicros = micros(); // Capture micros once for consistency
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024) {
mpu.resetFIFO();
Serial.println("FIFO overflow!");
} else if (fifoCount >= packetSize) {
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
float pitch = radiansToDegrees(ypr[1]); // Convert only once with helper function
float error = target_pitch - pitch;
integral += error;
float derivative = error - previous_error;
float velocity_command = calculateVelocityCommand(error, integral, derivative);
motor.move(velocity_command); // Move motor with calculated command
previous_error = error;
// Read Encoder Position
uint16_t encoder_angle = readEncoderAngleBitBang();
float current_position = (float)encoder_angle * 360.0 / 16383.0; // Convert to degrees
// Print at interval
if (currentMicros - lastPrintTime > printInterval) {
printStatus(currentMicros - lastPrintTime, pitch, velocity_command, current_position);
lastPrintTime = currentMicros;
}
mpu.resetFIFO();
}
command.run();
}
// Helper functions
float radiansToDegrees(float rad) {
return rad * 180.0 / M_PI;
}
float calculateVelocityCommand(float error, float& integral, float derivative) {
float velocity_command = Kp * error + Ki * integral + Kd * derivative;
if (velocity_command > 2 || velocity_command < -2) {
velocity_command = 0;
integral = 0; // Reset integral if command is out of bounds
}
return velocity_command;
}
void printStatus(unsigned long loopTime, float pitch, float velocity_command, float current_position) {
char buffer[10];
Serial.print("Loop Time: “);
Serial.print(loopTime);
Serial.print(” us | Pitch: “);
Serial.print(dtostrf(pitch, 6, 1, buffer));
Serial.print(” | Velocity command: “);
Serial.print(dtostrf(velocity_command, 6, 1, buffer));
Serial.print(” | Encoder Position: ");
Serial.print(dtostrf(current_position, 6, 1, buffer));
Serial.println(“°”);
}
