Diff: main.cpp

Revision:

0:8e87cdf07037

Child:

1:d3406369c297

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jan 31 17:59:21 2020 +0000
@@ -0,0 +1,340 @@
+/*
+    Boulus Abu Joudom
+    Version: January 28, 2020. 12: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);
+
+// 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.01;
+
+// 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;
+
+// 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] = {-33.5, -1.856}; // Currently for Cuboid 1.0 - UPDATE LATER for Cuboid 2.0 -
+
+// 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 = -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, 4.94f);
+LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -2000.0f, 2000.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);
+
+    // 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);
+
+}
+
+void updateControllers(void)
+{
+    // Counter 
+    LoopCounter++;
+    Loop.start();
+
+    // 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);
+        */
+
+    // System input
+    Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
+    // Sys_input_Amps = - 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 --> 1 V
+    VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
+
+            //1LoopCounter, 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 \n\r", LoopCounter, Sys_input_Amps, Cuboid_Angle_Degrees, GyroZ_RadiansPerSecond, Velocity, Velocity_Voltage, AccX_g, AccY_g, PID_Input, PID_Output, (Loop1_output+Loop2_output), Loop.read());
+      // pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
+    Loop.reset();
+   
+    //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));
+    }
+*/
\ No newline at end of file