File content as of revision 13:41edfb8e3b3c:

/*
    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.0
    Seeed Tiny Microcontroller on Cuboid 1.0
*/

/*
    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
        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
            
    Notes
        The Maximum output Current allowed is 13A and the Minimum is -13A !!!
        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.

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


// Low pass filter variables


// Flywheel Position and Velocity variables


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

// Sate Space Controller Values


// Controller Variables


// PID (PI Parameters)


// Saturation Parameters


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

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

// Accelerometer and Gyroscope Filters

// PID (PI) Controller


// ----- User defined functions -----------

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

// Linear Scaler - Follow the mapping mentioned in the Notes above

//******************************************************************************
//---------- Main Loop -------------
//******************************************************************************
int main()
{
    // 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();
    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);
    
    VoltageOut.write(2.5); // Output Zero Current to the Motor

    // Reset Filters and PID Controller

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

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


    // 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 - (2^15/1000 = 32.768)
    GyroX_Degrees = GyroX_Raw / 32.768f;  GyroY_Degrees = GyroY_Raw / 32.768f;  GyroZ_Degrees = GyroZ_Raw / 32.768f;  

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

    


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

    // ......

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


}