File content as of revision 3:1a714eccfe9f:

/*
    Boulus Abu Joudom
    Version: May 5, 2020. 17:00
    ZHAW - IMS

    Cubiod Balancing Experiment - Control Lab
    Seeed Tiny Microcontroller on Cuboid 1.0
*/
// Libraries
#include "mbed.h"
#include "math.h"
#include "MPU6050.h"
#include "IIR_filter.h"
#include "mcp4725.h"
#include <stdbool.h>
#include "LinearCharacteristics.h"
#include "PID_Cntrl.h"
#include "PID_Cntrl_2.h"

// Serial PC Communication
#define UART_TX     p9
#define UART_RX     p11

// IMU SDA and SCL
#define MPU6050_SDA p12
#define MPU6050_SCL p13

// PI Value
#define PI 3.1415927f
#define pi 3.1415927f

// Analog/Digitsl I/O Definitions
AnalogIn Velocity_Voltage_Input(p5);
DigitalOut Pin_3V3(p30);

// PC Serial
Serial pc(UART_TX, UART_RX);

//-------- USER INPUT ( Desired Values) -------
// User Input
float Desired_input = 0.0f;
// Initial System input in Amperes
float Sys_input_Amps = 0.0f;
//--------------------------------------------

// ------------- Variables -------------------

// Sample time of main loops
float Ts = 0.015;

// MPU 6050 Variables - Acceleration and Gyroscope Raw and Converted Data Variables
int16_t AccX_Raw, AccY_Raw, AccZ_Raw;
int16_t GyroX_Raw, GyroY_Raw, GyroZ_Raw;

double AccX_g, AccY_g, AccZ_g;
double GyroX_Degrees, GyroY_Degrees, GyroZ_Degrees, GyroZ_RadiansPerSecond;

int16_t IMU_Temperature;
//            ----------------

// Angle Variables
double Cuboid_Angle_Radians, Cuboid_Angle_Degrees;
double Cuboid_Angle_Speed_Degrees = 0.0;

// Low pass filter variables
float t = 0.5f;

// printf Variable
int k = 0;
int LoopCounter = 0;
float LoopTime = 0.0;

// Flywheel Position and Velocity variables
double Velocity_Input_Voltage = 0.0f;
double Velocity, Velocity_Voltage, Velocity_rpm, Velocity_Voltage_Read;

//              ------------------

//----------------- Controller VARIABLES------------------
// Variables concerning the Controller, the Design is in reference to the Matlab Simulink .............. and Variables are in reference to the ............. File

// Sate Space Controller Values
float K_SS_Controller [2] = {-57.1176, -2.6398}; // From Matlab

// Controller Variables
float Loop1_output; // Loop 1 controller output
float Loop2_output;  // Loop 2 controller output
float PID_Input, PID_Output;

// Saturation Parameters
// PI Controller Limits
const float uMin1 = -15.0f;
const float uMax1= 15.0f;

// Cuboid Driver Input Limits
const float uMin2 = -15.0f;
const float uMax2= 15.0f;

// Controller Loop 2 (PI-Part)
float Kp_1 = -0.09;
float Ki_1 = -0.09;
float Kd_1 = 0; // No D-Part
float Tf_1 = 1; // No D-Part

// Controller Loop (PI-Part) in Case 2 (breaking case)
float Kp_2 = 4;
float Ki_2 = 80;
float Kd_2 = 0; // No D-Part
float Tf_2 = 1; // No D-Part

// ---------- Functions -------------

// Interrupts
Ticker  ControllerLoopTimer;            // Interrupt for control loop

// DAC
MCP4725 VoltageOut(p3, p4, MCP4725::Fast400kHz, 0);

// IMU - MPU6050 Functions and I2C Functions
I2C    i2c(MPU6050_SDA, MPU6050_SCL);
MPU6050 mpu(i2c);

// Accelerometer and Gyroscope Filters
IIR_filter FilterAccX(t, Ts, 1.0f);
IIR_filter FilterAccY(t, Ts, 1.0f);
IIR_filter FilterGyro(t, Ts, t);

// Linear Scaler
LinearCharacteristics CurrentToVoltage(-15.0f, 15.0f, 0.0f, 5.0f);
LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -4000.0f, 4000.0f);

// PID Controllers
PID_Cntrl_2 C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1);  // Defining the 1st Loop Controller (PI-Part)
PID_Cntrl_2 C2(Kp_2,Ki_2,Kd_2,Tf_2,Ts,uMin1,uMax1);  // Defining the PI Controller for Chase (State 2) to keep motor velocity at zero

// Timers
Timer Loop;

// ----- User defined functions -----------
void updateControllers(void);   // speed controller loop (via interrupt)

// -------------- MAIN LOOP ----------------
int main()
{

    pc.baud(115200);
    Pin_3V3.write(1);
    
    Loop.start();

    // Reset Filters
    FilterAccX.reset(0.0f);
    FilterAccY.reset(0.0f);
    FilterGyro.reset(0.0f);

    //--------------------------------- IMU - MPU6050 initialize -------------------------------------
    pc.printf("MPU6050 initialize \n\r");
    mpu.initialize();
    pc.printf("MPU6050 testConnection \n\r");

    bool mpu6050TestResult = mpu.testConnection();
    if(mpu6050TestResult) {
        pc.printf("MPU6050 test passed \n\r");
    } else {
        pc.printf("MPU6050 test failed \n\r");
    }

    // Set Low Pass Filter Bandwidth to 44Hz (4.9ms) for the Acc and 42Hz (4.8ms) for the Gyroscope
    //mpu.setDLPFMode(MPU6050_DLPF_BW_42);

    // Set Low Pass Filter Bandwidth to 21Hz (8.5ms) for the Acc and 20Hz (8.3ms) for the Gyroscope
    mpu.setDLPFMode(MPU6050_DLPF_BW_20);

    // Change the scale of the Gyroscope to +/- 1000 degrees/sec
    mpu.setFullScaleGyroRange(2u);

    // Change the scale of the Accelerometer to +/- 4g - Sensitivity: 4096 LSB/mg
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);

    //----------------------------------------------------------------------------------------------
    
    // Reset PID
    C1.reset();

    pc.printf("Hello World!\n\r");
    // ControllerLoopTimer.attach(&updateControllers, Ts);

wait(1);

while(1)
{
    Loop.reset();
    
    // Counter 
    LoopCounter++;

    // Acquire Velocity
    Velocity_Voltage_Read = Velocity_Voltage_Input.read();
    Velocity_Voltage = 3.25*(Velocity_Voltage_Read);
    Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
    Velocity = Velocity_rpm*2.0*pi/60.0;

    // Aquire Raw Acceleration and Gyro Data form the IMU
    mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);

    // -------------- Convert Raw data to SI Units --------------------

    //Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
    AccX_g = AccX_Raw / 8192.0f;
    AccY_g = AccY_Raw / 8192.0f;
    AccZ_g = AccZ_Raw / 8192.0f;

    //Convert Gyroscope Raw Data to Degrees per second
    GyroX_Degrees = GyroX_Raw / 32.768f;  // (2^15/1000 = 32.768)
    GyroY_Degrees = GyroY_Raw / 32.768f;  // (2^15/1000 = 32.768)
    GyroZ_Degrees = GyroZ_Raw / 32.768f;  // (2^15/1000 = 32.768)

    //Convert Gyroscope Raw Data to Degrees per second
    GyroZ_RadiansPerSecond = (GyroZ_Raw / 32.768f)* pi/180.0f;

    // ----- Combine Accelerometer Data and Gyro Data to Get Angle ------

    Cuboid_Angle_Radians =  -1*atan2(-FilterAccX(AccX_g), FilterAccY(AccY_g)) + 0.7854f + FilterGyro(GyroZ_RadiansPerSecond);  // Check later if fast enough!!
    Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;


    // ------------------------- Controller -----------------------------
    // Current Input Updater - Amperes
    // Loop 1
    Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
    // Loop 2
    Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
    // PI Controller
    PID_Input = Desired_input - Velocity;
    PID_Output = C1.update(PID_Input);
        /*
        //PID_Input = 20.0 - Velocity; 
        //PID_Output = C1.update(-1*PID_Input);
        */


PID_Output = 0; // CHANGE LATER - This is for testing purposes


    // System input
    Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;


    if (Sys_input_Amps > uMax2) {
        Sys_input_Amps = uMax2;
    }
    if (Sys_input_Amps < uMin2) {
        Sys_input_Amps = uMin2;
    }

    // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
    VoltageOut.write(CurrentToVoltage(Sys_input_Amps));

            //1LoopCounter,2.1CurrentToVoltage, 2Sys_input_Amps, 3Cuboid_Angle_Degrees, 4GyroZ_RadiansPerSecond, 5Velocity, 6Velocity_Voltage, 7AccX_g, 8AccY_g, 9PID_Input, 10PID_Output, (Loop1_output+Loop2_output), Loop.read())
    pc.printf(" %i, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, ", LoopCounter, CurrentToVoltage(Sys_input_Amps), Sys_input_Amps, Cuboid_Angle_Degrees, GyroZ_RadiansPerSecond, Velocity, Velocity_Voltage, AccX_g, AccY_g, PID_Input, PID_Output, (Loop1_output+Loop2_output));
    
      // pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
    
    // 
    
    // wait(0.0025);
    // pc.printf(" %0.5f \n\r", Loop.read());
    
    LoopTime = Loop.read();
    while(LoopTime < Ts) {
        LoopTime = Loop.read();   
    } 
   
    pc.printf(" %0.5f \n\r", LoopTime);
   
    //pc.printf("Angle = %0.2f,  Current = %0.2f, VoltageOut = %0.3f, Velocity = %0.2f, VoltageIn = %0.3f\n\r", Cuboid_Angle_Degrees, Sys_input_Amps, CurrentToVoltage(Sys_input_Amps), Velocity, Velocity_Voltage);
    
/*
        // Print Data
        if(++k >= 300) {
            k = 0;
            //pc.printf("%0.8f\n\r", Loop.read());
            pc.printf("Angle = %0.2f,  Current = %0.2f, VoltageOut = %0.3f, Velocity = %0.2f, VoltageIn = %0.3f\n\r", Cuboid_Angle_Degrees, Sys_input_Amps, CurrentToVoltage(Sys_input_Amps), Velocity, Velocity_Voltage);

        }
*/

}
}





//-------------------------------------------------------------------
//--------------------------Example Codes-----------------------------

// This code demonstrates how to change the Low Pass Filter Bandwidth
/*
pc.printf("Digital Low Pass Filter Bandwidth: %u \n\r", mpu.getDLPFMode());

mpu.setDLPFMode(MPU6050_DLPF_BW_42);
wait(0.1);
pc.printf("Digital Low Pass Filter Bandwidth: %u \n\r", mpu.getDLPFMode());
*/
//                          ----------------

// This code demonstrates how to change the scale settings for the Gyro scope
/*
pc.printf("Gyro Range Scale: %u \n\r", mpu.getFullScaleGyroRange());
int Temp123 = 0;
while(Temp123 < 5) {

    wait(0.4);
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    //writing current accelerometer and gyro position
    pc.printf(" AccX: %d;  AccY: %d;  AccZ: %d;  GyroX: %d;  GyroY: %d;  GyroZ: %d;\n\r",ax,ay,az,gx,gy,gz);
    Temp123 = Temp123 + 1;
}
Temp123 = 0;

mpu.setFullScaleGyroRange(3u);
wait(0.1);
pc.printf("Gyro Range Scale: %u \n\r", mpu.getFullScaleGyroRange());
while(Temp123 < 5) {

    wait(0.4);
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    //writing current accelerometer and gyro position
    pc.printf(" AccX: %d;  AccY: %d;  AccZ: %d;  GyroX: %d;  GyroY: %d;  GyroZ: %d;\n\r",ax,ay,az,gx,gy,gz);
    Temp123 = Temp123 + 1;
}
Temp123 = 0;
*/

/*
    while(1) {
        wait(0.4);
        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        //writing current accelerometer and gyro position
        pc.printf(" AccX: %d;  AccY: %d;  AccZ: %d;  GyroX: %d;  GyroY: %d;  GyroZ: %d;\n\r",ax,ay,az,gx,gy,gz);
    }
*/
//                          ------------------


// Printing Register Values
//printf("i2c SCL : 0x%x\r\n", *((unsigned int *)0x40004508));

//                          ------------------

// Velocity Measurement Code for Designing a Filter
/*
    if (aaa < (600*3)) {
        if (++aaa <= 900) {
            VoltageOut.write(CurrentToVoltage(1.0f));
        } else {
            VoltageOut.write(CurrentToVoltage(-1.0f));
        }
        pc.printf("%0.8f\n\r", Velocity_Voltage_Read);
    } else {
        VoltageOut.write(CurrentToVoltage(0.0f));
    }
*/