Diff: main.cpp

Revision:

12:ba4ce3fa4f53

Parent:

11:af811e6f9a04

Child:

13:41edfb8e3b3c

--- a/main.cpp	Tue May 19 17:50:52 2020 +0000
+++ b/main.cpp	Thu May 28 16:39:03 2020 +0000
@@ -1,10 +1,10 @@
 /*
     Boulus Abu Joudom
-    Version: May 5, 2020. 17:00
+    Version: May 21, 2020. 17:00
     ZHAW - IMS
 
-    Cubiod Balancing Experiment - Control Lab
-    Seeed Tiny Microcontroller on Cuboid 1.0
+    Cubiod (Cube Mini) Balancing Experiment - Control Lab
+    Using Seeed Tiny Microcontroller
 */
 // Libraries
 #include "mbed.h"
@@ -25,25 +25,30 @@
 #define MPU6050_SDA p12
 #define MPU6050_SCL p13
 
-// PI Value
+// PI Values
 #define PI 3.1415927f
 #define pi 3.1415927f
 
-// Analog/Digitsl I/O Definitions
-AnalogIn Velocity_Voltage_Input(p5);
-DigitalOut Pin_3V3(p30);
-
 // PC Serial
 Serial pc(UART_TX, UART_RX);
 
-//-------- USER INPUT ( Desired Values) -------
+// -------------------------------
+// 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 -------------------
+// ------------------------------------
+//  Variables
+// ------------------------------------
 
 // Sample time of main loops
 float Ts = 0.005;
@@ -55,8 +60,7 @@
 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;
@@ -67,15 +71,13 @@
 
 // printf Variable
 int k = 0;
-int LoopCounter = 0;
-float LoopTime = 0.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
@@ -143,74 +145,42 @@
 // ----- User defined functions -----------
 void updateControllers(void);   // speed controller loop (via interrupt)
 
-// -------------- MAIN LOOP ----------------
+//******************************************************************************
+//---------- Main Loop -------------
+//******************************************************************************
 int main()
 {
-    VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
-    Button_Status = 0;
-
+    // Microcontroller initialization
     pc.baud(115200);
+    VoltageOut.write(CurrentToVoltage(0));
     Pin_3V3.write(1);
-
-    //Loop.start();
+    *((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);
 
-    //--------------------------------- 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(0.0f);
 
-    // Wait
-    wait(10);
-
-    pc.printf("Hello World!\n\r");
-    //wait(15);
-    Loop.start();
-    Button_Status = 0;
+    // Control Loop Interrupt at Ts, 200Hz Rate
     ControllerLoopTimer.attach(&updateControllers, Ts);
-    
-   
-   // Configure the ADC to the internel reference of 1.2V. ADC -> Congif Reg 0x40007504 // Multiply the ADC output (PIN input) by 1.2 to get the actual input relative to 3.0V. Then multiply by 3.0 to get the actual voltage
-   *((unsigned int *)0x40007504) = 0x400A;
 
 }
-
-// Loop.reset();
-
+//******************************************************************************
+//---------- control loop (called via interrupt) -------------
+//******************************************************************************
 void updateControllers(void)
 {
 
-
-    // Counter
-    //LoopCounter++;
+    // 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();
@@ -218,10 +188,6 @@
     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)
@@ -274,12 +240,13 @@
             //PID_Input = Desired_input - Velocity;
             //PID_Output = C1.update(PID_Input);
 
-            PID_Input = 52.36f - Velocity;
-            PID_Output = C1.update(-1*PID_Input);
+            // Constant Speed Control
+            //PID_Input = 52.36f - Velocity;
+            //PID_Output = C1.update(-1*PID_Input);
 
             // System input
-            //Sys_input_Amps = 2.0f; //PID_Output; //- Loop1_output - Loop2_output;
-            VoltageOut.write(0.5f);
+            Sys_input_Amps = 0.5f; //PID_Output; //- Loop1_output - Loop2_output;
+            //VoltageOut.write(0.5f);
 
             if (Sys_input_Amps > uMax2) {
                 Sys_input_Amps = uMax2;
@@ -289,123 +256,16 @@
             }
 
             // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
-            //VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
-
-            // Print Data
-
-            if(++k >= 200) {
-                k = 0;
-
-                //1LoopCounter,2.1CurrentToVoltage, 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, ", LoopCounter, CurrentToVoltage(Sys_input_Amps), Sys_input_Amps, Cuboid_Angle_Degrees, GyroZ_RadiansPerSecond, Velocity, Velocity_Voltage, AccX_g, AccY_g, PID_Input, PID_Output, (Loop1_output+Loop2_output));
-                pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
-                // Printing Register Values
-                //printf("ADC : 0x%x\r\n", *((unsigned int *)0x40007504));
-                //printf("ADC : %0.5f\n", Velocity_Voltage_Input.read());
-            }
-
-            //pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
-
-            //
-
-            // wait(0.0025);
-            // pc.printf(" %0.5f \n\r", Loop.read());
-
-            /*
-             LoopTime = Loop.read();
-             while(LoopTime < Ts) {
-                 LoopTime = Loop.read();
-             }
-             */
-
-            // pc.printf(" %0.5f \n\r", LoopTime);
-
-            //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);
-
-                    }
-            */
+            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);
 }
 
-
-
-
-
-//-------------------------------------------------------------------
-//--------------------------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