Diff: main.cpp

Revision:

13:41edfb8e3b3c

Parent:

12:ba4ce3fa4f53

Child:

14:fe003a27018d

--- a/main.cpp	Thu May 28 16:39:03 2020 +0000
+++ b/main.cpp	Thu May 28 18:14:34 2020 +0000
@@ -1,21 +1,48 @@
 /*
     Boulus Abu Joudom
-    Version: May 21, 2020. 17:00
+    Cube Mini - Prototype 1 V1
+    Mbed Version: May 28, 2020. 18:00
     ZHAW - IMS
 
-    Cubiod (Cube Mini) Balancing Experiment - Control Lab
-    Using Seeed Tiny Microcontroller
+    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 "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"
+#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
@@ -29,121 +56,104 @@
 #define PI 3.1415927f
 #define pi 3.1415927f
 
-// PC Serial
-Serial pc(UART_TX, UART_RX);
+// PC Serial Connection
+Serial pc(UART_TX, UART_RX);        // serial connection via USB - programmer
 
 // -------------------------------
 // Analog/Digitsl I/O Definitions
 // -------------------------------
-AnalogIn Velocity_Voltage_Input(p5);
-DigitalOut Pin_3V3(p30);
+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.
 
-// ------------------------------------
-// USER INPUT ( Desired Values)
-// ------------------------------------
-// User Input
-float Desired_input = 0.0f;
-// Initial System input in Amperes
-float Sys_input_Amps = 0.0f;
-
-// ------------------------------------
-//  Variables
-// ------------------------------------
+// -------------------------------
+// Initialization of Values and Variables
+// -------------------------------
 
 // Sample time of main loops
-float Ts = 0.005;
+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;
 
-// ------------------------------------
-
-// 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;
+
+
+// -------------------------------
+//  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
-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
+// -------------------------------
+//  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
-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;
+
+
+// PID (PI Parameters)
+
 
 // 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
+// Cuboid Escon Input Limits in Amps
+const float uMin = -13.0f;        // Minimum Current Allowed
+const float uMax =  13.0f;        // Maximum Current Allowe
 
-// 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 -------------
+// -------------------------------
+//  User-Defined Functions
+// -------------------------------
 
 // Interrupts
 Ticker  ControllerLoopTimer;            // Interrupt for control loop
 
-// DAC
+
+// -------------------------------
+//  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);
 
-// 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;
 
+// Accelerometer and Gyroscope Filters
+
+// PID (PI) Controller
+
+
 // ----- User defined functions -----------
-void updateControllers(void);   // speed controller loop (via interrupt)
+
+// Controller loop (via interrupt)
+void updateControllers(void);   
+
+// Linear Scaler - Follow the mapping mentioned in the Notes above
 
 //******************************************************************************
 //---------- Main Loop -------------
@@ -151,121 +161,65 @@
 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
+    // IMU - MPU6050 initialization - Do NOT Modify
     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.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
-    FilterAccX.reset(0.0f);
-    FilterAccY.reset(0.0f);
-    FilterGyro.reset(0.0f);
-
-    // Reset PID
-    C1.reset(0.0f);
+    // Reset Filters and PID Controller
 
     // Control Loop Interrupt at Ts, 200Hz Rate
     ControllerLoopTimer.attach(&updateControllers, Ts);
 
 }
 //******************************************************************************
-//---------- control loop (called via interrupt) -------------
+//---------- 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);
 
-    // 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;
+    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 - (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 ------
 
-    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) {
+    if(++k >= 100) {
         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);
+
 }