Diff: main.cpp

Revision:

16:762867ba6aaa

Parent:

15:3395ad948f44

--- a/main.cpp	Fri May 29 11:34:21 2020 +0000
+++ b/main.cpp	Fri May 29 15:41:58 2020 +0000
@@ -10,9 +10,39 @@
 */
 
 /*
+
+    // 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
@@ -27,13 +57,13 @@
         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
@@ -105,7 +135,7 @@
 // -------------------------------
 //  Controller Variables: Such as SS Controller Values and Saturation Limits
 // -------------------------------
-// Variables concerning the Controller Design is in reference to the Matlab and Simulink Files .............. 
+// Variables concerning the Controller Design is in reference to the Matlab and Simulink Files ..............
 
 // Sate Space Controller Values
 
@@ -154,7 +184,7 @@
 // ----- User defined functions -----------
 
 // Controller loop (via interrupt)
-void updateControllers(void);   
+void updateControllers(void);
 
 // Linear Scaler - Follow the mapping mentioned in the Notes above
 
@@ -171,10 +201,10 @@
     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
@@ -188,7 +218,7 @@
 //******************************************************************************
 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
@@ -200,22 +230,26 @@
     // -------------- 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 - (2^15/1000 = 32.768)
-    GyroX_Degrees = GyroX_Raw / 32.768f;  GyroY_Degrees = GyroY_Raw / 32.768f;  GyroZ_Degrees = GyroZ_Raw / 32.768f;  
+    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 -----------------------------
 
     // Switch Statement Maybe?......
-    
+
     // ----------------
 
     // Print Data