File content as of revision 12:ba4ce3fa4f53:

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

    Cubiod (Cube Mini) Balancing Experiment - Control Lab
    Using Seeed Tiny Microcontroller
*/
// 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_Old.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 Values
#define PI 3.1415927f
#define pi 3.1415927f

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

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

// ------------------------------------
// 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.005;

// 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;

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

// 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 Case1_Counter = 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*0.3, -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 = -5.0f;
const float uMax1= 5.0f;

// Cuboid Driver Input Limits
const float uMin2 = -13.0f;
const float uMax2= 13.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

// User Button
int Button_Status = 0;

// ---------- 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_Old C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1);  // Defining the 1st Loop Controller (PI-Part)
PID_Cntrl_Old 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()
{
    // Microcontroller initialization
    pc.baud(115200);
    VoltageOut.write(CurrentToVoltage(0));
    Pin_3V3.write(1);
    *((unsigned int *)0x40007504) = 0x400A; // Configure the ADC to the internel reference of 1.2V.
    // IMU - MPU6050 initialization
    mpu.initialize();
    mpu.setDLPFMode(MPU6050_DLPF_BW_20); // Sets Low Pass Filter Bandwidth to 21Hz (8.5ms) for the Acc and 20Hz (8.3ms) for the Gyroscope
    mpu.setFullScaleGyroRange(2u);  // Change the scale of the Gyroscope to +/- 1000 degrees/sec
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // Change the scale of the Accelerometer to +/- 4g - Sensitivity: 4096 LSB/mg

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

    // Reset PID
    C1.reset(0.0f);

    // Control Loop Interrupt at Ts, 200Hz Rate
    ControllerLoopTimer.attach(&updateControllers, Ts);

}
//******************************************************************************
//---------- control loop (called via interrupt) -------------
//******************************************************************************
void updateControllers(void)
{

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

    // Acquire Velocity
    Velocity_Voltage_Read = Velocity_Voltage_Input.read();
    Velocity_Voltage = 3.0*1.1978917*(Velocity_Voltage_Read); // *1.2V because the Vref is 1.2V // *3 because the prescaling is 1/3 // *1.1978917 is for Calibration purposes instead of 1.2
    Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
    Velocity = Velocity_rpm*2.0*pi/60.0;

    // -------------- 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 -----------------------------

    switch(Button_Status) {

        case 0:

            // PI Controller
            PID_Input = Desired_input - Velocity;
            PID_Output = C1.update(PID_Input);

            // System input
            Sys_input_Amps = PID_Output;
            VoltageOut.write(CurrentToVoltage(Sys_input_Amps));

            // Counter of 10 seconds then go to Case 1
            Case1_Counter++;
            if (Case1_Counter > 2000) {
                Button_Status = 1;
            }

        case 1:

            // 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);

            // Constant Speed Control
            //PID_Input = 52.36f - Velocity;
            //PID_Output = C1.update(-1*PID_Input);

            // System input
            Sys_input_Amps = 0.5f; //PID_Output; //- Loop1_output - Loop2_output;
            //VoltageOut.write(0.5f);

            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));

    }

    // Print Data
    if(++k >= 10) {
        k = 0;
        pc.printf("Some Outputs: %0.6f, %0.6f\n\r", Sys_input_Amps, Velocity);
    }

    pc.printf("%0.6f, %0.6f\n\r", Sys_input_Amps, Velocity);
}