Mar 8

We surprisingly made more progress during the demo

•  Got the motor to spin in the correct direction and "attempt" a self balance using IMU readings (no video, unfortunately) 
• Was very jittery and heated the motor
• Eventually the motor stopped running (we may have fried it)

#include <SimpleFOC.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

// Motor setup
BLDCMotor motor = BLDCMotor(7, 16.4, 90);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);

// IMU setup
Adafruit_MPU6050 mpu;

// PID variables
//float Kp = 15.0;   // Proportional gain
//float Ki = 0.1;    // Integral gain
//float Kd = 0.8;    // Derivative gain

float Kp = 10.0;   // Start lower
float Ki = 0.05;   // Reduce integral action
float Kd = 2.5;    // Increase damping

float previous_error = 0;
float integral = 0;
float setpoint = 0;  // Desired angle (upright position)
float target_velocity = 0;

// Time tracking
unsigned long lastTime = 0;
const float dt = 0.01;  // Time step in seconds
//const float dt = 0.005;

void setup() {
    Serial.begin(115200);
    Wire.begin();

    // Initialize MPU6050
    if (!mpu.begin()) {
        Serial.println("Failed to find MPU6050!");
        while (1) delay(10);
    }
    mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

    // Initialize motor driver
    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;
    }
    
    Serial.println("System ready!");
    delay(1000);
}

void loop() {
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    // Calculate angle using complementary filter
    float accel_angle = atan2(a.acceleration.x, a.acceleration.z) * 180 / PI;
    static float angle = 0;
    float gyro_rate = g.gyro.y * 180 / PI;
    angle = 0.98 * (angle + gyro_rate * dt) + 0.02 * accel_angle;  // Complementary filter

    // PID control
    float error = setpoint - angle;
    integral += error * dt;
    float derivative = (error - previous_error) / dt;
    target_velocity = Kp * error + Ki * integral + Kd * derivative;
    previous_error = error;

    // Apply velocity to motor
    motor.move(target_velocity);

    // Debug output
    Serial.print("Angle: ");
    Serial.print(angle);
    Serial.print(" | Target Velocity: ");
    Serial.println(target_velocity);

    delay(10);  // Run loop at ~100 Hz
}