File content as of revision 18:3b1aa67e11ca:

/*
    Boulus Abu Joudom
    Cube Mini - Prototype 1 V1
    Mbed Version: May 28, 2020. 18:00
    ZHAW - IMS

    Cubiod Balancing Experiment - Control Lab
    Template Version: 1.2
    Seeed Tiny Microcontroller on Cuboid 1.0
*/

/*

    // Physical Model of Cuboid 2.0
    
    % Mass Parameters
    Cube Mini - Mass = 0.6 + 0.170 + 0.015 = 0.7850 Kg, Mass of the Full Cuboid with Motor and Flywheel
    Cube Mini - Flywheel Mass = 0.170 kg
    Cube Mini - Without Flywheel Mass = 0.6150 kg
    
    % Dimensions Parameters
    % Cube Mini Dimensions (10x10x10) cm
    Cube Mini - Length = 0.90*100e-3 m (The multiplication by 0.9 is to compensate for the rounded edges)
    Cube Mini - CoM = Cube Mini - Length * 0.5,  Center of Mass Location 
    % Inertia Parameters
    Cube Mini - Inertia (J) of the Body from the edge Edge = 0.003044 Kg.m^2,  Inertia about the edge
    Cube Mini - Inertia (J) of Flyhweel and Rotor from Center = (0.0002104064 + 0.0000181) kg.m^2,
    
    % Motor Parameters
    % Motor: Maxon Flat EC45, Part number: 411812, Specs: 24V, Km 36.9e-3
    Cube Mini - Motor.Km = 36.9e-3 % Torque Constant % Unit: Nm/A
    Cube Mini - Motor.Resistance = 0.608 % Terminal Resistance % Unit: Ohm
    Cube Mini - Motor.Inductance = 0.463e-3 % Unit: H, Henry
    
    % PI Controller Escon 
    Cube Mini - Escon.PI_Tn = 500e-6; %945e-6
    Cube Mini - Escon.PI_Kp = 100*(10/2^11);%576*(10/2^11)
    
    % RC Physical Velocity Voltage Filter
    Filter.Velocity.R = 1e3 Ohm
    Filter.Velocity.C = 10e-6 F

    Settings for Maxon ESCON controller (upload via ESCON Studio)
    Escon Studio file Location: ...

    Hardware Connections
        Serial PC Communication
            UART_TX     p9
            UART_RX     p11
        IMU SDA and SCL
            MPU6050_SDA p12
            MPU6050_SCL p13
        Velocity Input as Analogue Signal from Escon
            Pin 5
        Button Pin
            Pin 6
        Current output to Escon occurs through I2C connection to a DAC (Digital to Analogue Converter). DAC outputs Analogue voltage to Escon
            Pin SDA p3
            Pin SCL P4

    Escon Mapping of voltage to current and rotational velocity to voltage
        Escon Linear Mapping of input current from voltage is 0V is -15A and 5V is 15A, hence 2.5V is 0A !!!
        Escon Linear Mapping of output rotational velocity (in rpm) to voltage is -4000rpm is 0V and 4000rpm is 3.0V, hence 1.5V is 0 RPM !!!

    Notes
        The Maximum output Current allowed is 13A and the Minimum is -13A !!!
        Escon Linear Mapping of input current from voltage is 0V is -15A and 5V is 15A, hence 2.5V is 0A !!!
        All needed Libraries for the IMU (Accelerometer and Gyroscope), DAC are included.
        The PID is to be written by students. Use PID_Cntrl.cpp and PID_Cntrl.h as templates
        ...


*/
// Libraries
#include <stdbool.h>
#include "mbed.h"
#include "MPU6050.h"                // IMU Library
#include "mcp4725.h"                // DAC Library
#include "IIR_filter.h"             // Filter Library (Please go to the Filter Library and add in your Code)
#include "LinearCharacteristics.h"  // Linear Scaling Library
#include "PID_Cntrl.h"              // PID Library (Please go to PID Library and add in your PID Code)


// 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 Connection
Serial pc(UART_TX, UART_RX);        // serial connection via USB - programmer

// -------------------------------
// Analog/Digitsl I/O Definitions
// -------------------------------
AnalogIn Velocity_Voltage_Input(p5); // Velocity Input as Analogue Signal from Escon
DigitalOut Pin_3V3(p30);             // Defining P30 as 3V3 Pin for internal use. Please do not modify.
InterruptIn Button(p6);        // User Button Interrput

// -------------------------------
// Initialization of Values and Variables
// -------------------------------

// Sample time of main loops
float Ts = 0.007; // Around 143 Hz. Do not change. This is the lowest value possible if printing to a serial connection is needed.

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

// printf Variable
int k = 0;


// -------------------------------
//  Variables: Here define variables such as gains etc.
// -------------------------------

// Accelerometer and Gyroscope


// Cube Angle and Speed Variables
double Cuboid_Angle_Radians, Cuboid_Angle_Degrees;
double Cuboid_Angle_Speed_Degrees = 0.0;

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

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

// User Button
int Button_Status = 0;                  // User Button Status
Timer T_Button;                         // define timer for button

// -------------------------------
//  Controller Variables: Such as SS Controller Values and Saturation Limits
// -------------------------------
// Variables concerning the Controller Design is in reference to the Matlab and Simulink Files ..............

// User Input
float Desired_input = 0.0f;
// Initial System input in Amperes
float Sys_input_Amps = 0.0f;

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

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

// PID (PI Parameters)

// PID 1 - Velocity control after lift-up
float Kp_1 = -0.09;
float Ki_1 = -0.09*0.5*0.5;
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 = 0.25/4.0;
float Ki_2 = 0.25/4.0;
float Kd_2 = 0; // No D-Part
float Tf_2 = 1; // No D-Part

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

// Cuboid Escon Input Limits in Amps
const float uMin = -13.0f;        // Minimum Current Allowed
const float uMax =  13.0f;        // Maximum Current Allowe


// -------------------------------
//  User-Defined Functions
// -------------------------------

// USer Button
void pressed(void);                     // user Button pressed
void released(void);                    // user Button released

// Controller loop (via interrupt)
void updateControllers(void);           // controller loop (via interrupt)

// Linear Scaler - Follow the mapping mentioned in the Notes above
// Linear Scaler
LinearCharacteristics CurrentToVoltage(-15.0f, 15.0f, 0.0f, 5.0f);
LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -4000.0f, 4000.0f);

// PID (PI) Controller (My PID Controller is fine but needs clarity updates)
PID_Cntrl  C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1);   // Defining the 1st Loop Controller (PI-Part)
PID_Cntrl  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
PID_Cntrl  C3(Kp_1*2.5,Ki_1*1.5,Kd_2,Tf_2,Ts,uMin1,uMax1); // Safety Implementation in Case 4 PID

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

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


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

// DAC - For Current output
MCP4725 VoltageOut(p3, p4, MCP4725::Fast400kHz, 0);

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

// Timers
Timer Loop;

//******************************************************************************
//----------------------------- Main Loop --------------------------------------
//******************************************************************************
int main()
{

    VoltageOut.write(2.5); // Output Zero Current to the Motor

    // Microcontroller initialization
    Pin_3V3.write(1);
    *((unsigned int *)0x40007504) = 0x400A; // Configure the ADC to the internel reference of 1.2V.
    // IMU - MPU6050 initialization - Do NOT Modify
    mpu.initialize();
    bool mpu6050TestResult = mpu.testConnection();
    if(mpu6050TestResult) {
        pc.printf("MPU6050 test passed \n\r");
    } else {
        pc.printf("MPU6050 test failed \n\r");
    }
    mpu.setDLPFMode(MPU6050_DLPF_BW_42); // Set Low Pass Filter Bandwidth to 44Hz (4.9ms) for the Acc and 42Hz (4.8ms) 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

    // Serial Communication
    pc.baud(115200);



    // Reset Filters and PID Controller
    FilterAccX.reset(0.0f);
    FilterAccY.reset(0.0f);
    FilterGyro.reset(0.0f);
    C1.reset(0.0f);
    C2.reset(0.0f);
    C3.reset(0.0f);

    pc.printf("Hello World!\n\r");
    wait(5);
    // Control Loop Interrupt at Ts, 200Hz Rate
    ControllerLoopTimer.attach(&updateControllers, Ts);
    Button.fall(&pressed);          // attach key pressed function
    Button.rise(&released);         // attach key pressed function


}
//******************************************************************************
//------------------ Control Loop (called via interrupt) -----------------------
//******************************************************************************
void updateControllers(void)
{

    // Acquire Velocity
    Velocity_Voltage = 3.0*1.1978917*(Velocity_Voltage_Input.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 = .... Refer to mapping
    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);
    Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;


    // ------------------------- Controller -----------------------------

    // Switch Statement Maybe?......
    switch(Button_Status) {

        case 0: // Output of 0 Amps
            Sys_input_Amps = 0.0;
            break;

        case 1: // Breaking

            // PI Controller
            PID_Input = 0.0f - Velocity;
            PID_Output = C2.update(PID_Input);

            // System input
            Sys_input_Amps = PID_Output;
            break;

        case 2: // Balancing and lift-up

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

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


            // Do Not Modify - This is implemented to prevent the Cube from continuously speeding up while being still for after falling due to the user interrupting its movement
            if ( abs(Velocity) > 250.0f && (abs(Cuboid_Angle_Degrees)<50.0f && abs(Cuboid_Angle_Degrees)>40.0f) ) {
                Button_Status = 4;
                break;
            }
            break;

        case 3: // Fall
        
            Sys_input_Amps = 0.0;
            // Not implemented Yet :)
            break;

        case 4: // Lift up when stuck
            // Do Not Modify - This is implemented to prevent the Cube from continuously speeding up while being still for after falling due to the user interrupting its movement

            PID_Input2 = 0.0f - Velocity;
            PID_Output2 = C3.update(-1*PID_Input2);

            // System input
            Sys_input_Amps = PID_Output2;
            if (Sys_input_Amps > 1.0f) {
                Sys_input_Amps = 1.0f;
            }
            if (Sys_input_Amps < -1.0f) {
                Sys_input_Amps = -1.0f;
            }
            VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
            //pc.printf("%0.7f, %0.7f, \n\r", abs(Cuboid_Angle_Degrees), abs(Velocity));

            if (abs(Velocity) < 15.0f) {
                Sys_input_Amps = 0.0;
                Button_Status = 2;
                break;
            }
            break;
    }
    // End of Switch Statement

    if (Sys_input_Amps > uMax) {
        Sys_input_Amps = uMax;
    }
    if (Sys_input_Amps < uMin) {
        Sys_input_Amps = uMin;
    }
    if (Cuboid_Angle_Degrees > -50.0f && Cuboid_Angle_Degrees < 50.0f) {
    // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
    VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
    }
    else {
        Sys_input_Amps = 0.0f;
        VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
        }

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

    // Print Data // Printing Rate (in this case) is every Ts*100 = 0.007*100 = every 0.7 seconds
    if(++k >= 100) {
        k = 0;
        pc.printf("Some Outputs: %0.6f, %0.6f, %i\n\r", Sys_input_Amps, Cuboid_Angle_Degrees, Button_Status);
    }


}
//******************************************************************************
//------------------ User functions like buttens handle etc. -------------------
//******************************************************************************

//...
// start timer as soon as Button is pressed
void pressed()
{
    T_Button.start();
}
// Falling edge of button: enable/disable controller
void released()
{
    // readout, stop and reset timer
    float ButtonTime = T_Button.read();
    T_Button.stop();
    T_Button.reset();
    if(ButtonTime > 0.05f) {
        Button_Status = Button_Status + 1;
        if (Button_Status > 3.0f) {
            Button_Status = 1.0;
        }
        //pc.printf("Button Status: %i\r\n", Button_Status);
    }
}