Boulus Abu Joudom / Mbed 2 deprecated Cube_Mini_Solution

Cube Mini Solution

Dependencies:   mbed QEI MPU6050 BLE_API nRF51822 MCP4725 eMPL_MPU6050

main.cpp@12:ba4ce3fa4f53, 2020-05-28 (annotated)

Committer:
BoulusAJ
Date:
Thu May 28 16:39:03 2020 +0000
Revision:
12:ba4ce3fa4f53
Parent:
11:af811e6f9a04
Child:
13:41edfb8e3b3c
Template Version May 21

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BoulusAJ 0:8e87cdf07037 1 /*
BoulusAJ 0:8e87cdf07037 2 Boulus Abu Joudom
BoulusAJ 12:ba4ce3fa4f53 3 Version: May 21, 2020. 17:00
BoulusAJ 0:8e87cdf07037 4 ZHAW - IMS
BoulusAJ 0:8e87cdf07037 5
BoulusAJ 12:ba4ce3fa4f53 6 Cubiod (Cube Mini) Balancing Experiment - Control Lab
BoulusAJ 12:ba4ce3fa4f53 7 Using Seeed Tiny Microcontroller
BoulusAJ 0:8e87cdf07037 8 */
BoulusAJ 0:8e87cdf07037 9 // Libraries
BoulusAJ 0:8e87cdf07037 10 #include "mbed.h"
BoulusAJ 0:8e87cdf07037 11 #include "math.h"
BoulusAJ 0:8e87cdf07037 12 #include "MPU6050.h"
BoulusAJ 0:8e87cdf07037 13 #include "IIR_filter.h"
BoulusAJ 0:8e87cdf07037 14 #include "mcp4725.h"
BoulusAJ 0:8e87cdf07037 15 #include <stdbool.h>
BoulusAJ 0:8e87cdf07037 16 #include "LinearCharacteristics.h"
BoulusAJ 7:07c5b6709d87 17 #include "PID_Cntrl_Old.h"
BoulusAJ 0:8e87cdf07037 18 #include "PID_Cntrl_2.h"
BoulusAJ 0:8e87cdf07037 19
BoulusAJ 0:8e87cdf07037 20 // Serial PC Communication
BoulusAJ 0:8e87cdf07037 21 #define UART_TX p9
BoulusAJ 0:8e87cdf07037 22 #define UART_RX p11
BoulusAJ 0:8e87cdf07037 23
BoulusAJ 0:8e87cdf07037 24 // IMU SDA and SCL
BoulusAJ 0:8e87cdf07037 25 #define MPU6050_SDA p12
BoulusAJ 0:8e87cdf07037 26 #define MPU6050_SCL p13
BoulusAJ 0:8e87cdf07037 27
BoulusAJ 12:ba4ce3fa4f53 28 // PI Values
BoulusAJ 0:8e87cdf07037 29 #define PI 3.1415927f
BoulusAJ 0:8e87cdf07037 30 #define pi 3.1415927f
BoulusAJ 0:8e87cdf07037 31
BoulusAJ 0:8e87cdf07037 32 // PC Serial
BoulusAJ 0:8e87cdf07037 33 Serial pc(UART_TX, UART_RX);
BoulusAJ 0:8e87cdf07037 34
BoulusAJ 12:ba4ce3fa4f53 35 // -------------------------------
BoulusAJ 12:ba4ce3fa4f53 36 // Analog/Digitsl I/O Definitions
BoulusAJ 12:ba4ce3fa4f53 37 // -------------------------------
BoulusAJ 12:ba4ce3fa4f53 38 AnalogIn Velocity_Voltage_Input(p5);
BoulusAJ 12:ba4ce3fa4f53 39 DigitalOut Pin_3V3(p30);
BoulusAJ 12:ba4ce3fa4f53 40
BoulusAJ 12:ba4ce3fa4f53 41 // ------------------------------------
BoulusAJ 12:ba4ce3fa4f53 42 // USER INPUT ( Desired Values)
BoulusAJ 12:ba4ce3fa4f53 43 // ------------------------------------
BoulusAJ 0:8e87cdf07037 44 // User Input
BoulusAJ 0:8e87cdf07037 45 float Desired_input = 0.0f;
BoulusAJ 0:8e87cdf07037 46 // Initial System input in Amperes
BoulusAJ 0:8e87cdf07037 47 float Sys_input_Amps = 0.0f;
BoulusAJ 0:8e87cdf07037 48
BoulusAJ 12:ba4ce3fa4f53 49 // ------------------------------------
BoulusAJ 12:ba4ce3fa4f53 50 // Variables
BoulusAJ 12:ba4ce3fa4f53 51 // ------------------------------------
BoulusAJ 0:8e87cdf07037 52
BoulusAJ 0:8e87cdf07037 53 // Sample time of main loops
BoulusAJ 8:71babe904b92 54 float Ts = 0.005;
BoulusAJ 0:8e87cdf07037 55
BoulusAJ 0:8e87cdf07037 56 // MPU 6050 Variables - Acceleration and Gyroscope Raw and Converted Data Variables
BoulusAJ 0:8e87cdf07037 57 int16_t AccX_Raw, AccY_Raw, AccZ_Raw;
BoulusAJ 0:8e87cdf07037 58 int16_t GyroX_Raw, GyroY_Raw, GyroZ_Raw;
BoulusAJ 0:8e87cdf07037 59
BoulusAJ 0:8e87cdf07037 60 double AccX_g, AccY_g, AccZ_g;
BoulusAJ 0:8e87cdf07037 61 double GyroX_Degrees, GyroY_Degrees, GyroZ_Degrees, GyroZ_RadiansPerSecond;
BoulusAJ 0:8e87cdf07037 62
BoulusAJ 12:ba4ce3fa4f53 63 // ------------------------------------
BoulusAJ 0:8e87cdf07037 64
BoulusAJ 0:8e87cdf07037 65 // Angle Variables
BoulusAJ 0:8e87cdf07037 66 double Cuboid_Angle_Radians, Cuboid_Angle_Degrees;
BoulusAJ 0:8e87cdf07037 67 double Cuboid_Angle_Speed_Degrees = 0.0;
BoulusAJ 0:8e87cdf07037 68
BoulusAJ 0:8e87cdf07037 69 // Low pass filter variables
BoulusAJ 0:8e87cdf07037 70 float t = 0.5f;
BoulusAJ 0:8e87cdf07037 71
BoulusAJ 0:8e87cdf07037 72 // printf Variable
BoulusAJ 0:8e87cdf07037 73 int k = 0;
BoulusAJ 10:4e9899cef12c 74 int Case1_Counter = 0;
BoulusAJ 0:8e87cdf07037 75
BoulusAJ 0:8e87cdf07037 76 // Flywheel Position and Velocity variables
BoulusAJ 0:8e87cdf07037 77 double Velocity_Input_Voltage = 0.0f;
BoulusAJ 0:8e87cdf07037 78 double Velocity, Velocity_Voltage, Velocity_rpm, Velocity_Voltage_Read;
BoulusAJ 0:8e87cdf07037 79
BoulusAJ 12:ba4ce3fa4f53 80 // --------------------------------------
BoulusAJ 0:8e87cdf07037 81
BoulusAJ 0:8e87cdf07037 82 //----------------- Controller VARIABLES------------------
BoulusAJ 0:8e87cdf07037 83 // Variables concerning the Controller, the Design is in reference to the Matlab Simulink .............. and Variables are in reference to the ............. File
BoulusAJ 0:8e87cdf07037 84
BoulusAJ 0:8e87cdf07037 85 // Sate Space Controller Values
BoulusAJ 8:71babe904b92 86 float K_SS_Controller [2] = {-57.1176*0.3, -2.6398}; // From Matlab
BoulusAJ 0:8e87cdf07037 87
BoulusAJ 0:8e87cdf07037 88 // Controller Variables
BoulusAJ 0:8e87cdf07037 89 float Loop1_output; // Loop 1 controller output
BoulusAJ 0:8e87cdf07037 90 float Loop2_output; // Loop 2 controller output
BoulusAJ 0:8e87cdf07037 91 float PID_Input, PID_Output;
BoulusAJ 0:8e87cdf07037 92
BoulusAJ 0:8e87cdf07037 93 // Saturation Parameters
BoulusAJ 0:8e87cdf07037 94 // PI Controller Limits
BoulusAJ 6:122879c1503a 95 const float uMin1 = -5.0f;
BoulusAJ 6:122879c1503a 96 const float uMax1= 5.0f;
BoulusAJ 0:8e87cdf07037 97
BoulusAJ 0:8e87cdf07037 98 // Cuboid Driver Input Limits
BoulusAJ 10:4e9899cef12c 99 const float uMin2 = -13.0f;
BoulusAJ 10:4e9899cef12c 100 const float uMax2= 13.0f;
BoulusAJ 0:8e87cdf07037 101
BoulusAJ 0:8e87cdf07037 102 // Controller Loop 2 (PI-Part)
BoulusAJ 0:8e87cdf07037 103 float Kp_1 = -0.09;
BoulusAJ 0:8e87cdf07037 104 float Ki_1 = -0.09;
BoulusAJ 0:8e87cdf07037 105 float Kd_1 = 0; // No D-Part
BoulusAJ 0:8e87cdf07037 106 float Tf_1 = 1; // No D-Part
BoulusAJ 0:8e87cdf07037 107
BoulusAJ 0:8e87cdf07037 108 // Controller Loop (PI-Part) in Case 2 (breaking case)
BoulusAJ 0:8e87cdf07037 109 float Kp_2 = 4;
BoulusAJ 0:8e87cdf07037 110 float Ki_2 = 80;
BoulusAJ 0:8e87cdf07037 111 float Kd_2 = 0; // No D-Part
BoulusAJ 0:8e87cdf07037 112 float Tf_2 = 1; // No D-Part
BoulusAJ 0:8e87cdf07037 113
BoulusAJ 10:4e9899cef12c 114 // User Button
BoulusAJ 10:4e9899cef12c 115 int Button_Status = 0;
BoulusAJ 10:4e9899cef12c 116
BoulusAJ 0:8e87cdf07037 117 // ---------- Functions -------------
BoulusAJ 0:8e87cdf07037 118
BoulusAJ 0:8e87cdf07037 119 // Interrupts
BoulusAJ 0:8e87cdf07037 120 Ticker ControllerLoopTimer; // Interrupt for control loop
BoulusAJ 0:8e87cdf07037 121
BoulusAJ 0:8e87cdf07037 122 // DAC
BoulusAJ 0:8e87cdf07037 123 MCP4725 VoltageOut(p3, p4, MCP4725::Fast400kHz, 0);
BoulusAJ 0:8e87cdf07037 124
BoulusAJ 0:8e87cdf07037 125 // IMU - MPU6050 Functions and I2C Functions
BoulusAJ 0:8e87cdf07037 126 I2C i2c(MPU6050_SDA, MPU6050_SCL);
BoulusAJ 0:8e87cdf07037 127 MPU6050 mpu(i2c);
BoulusAJ 0:8e87cdf07037 128
BoulusAJ 0:8e87cdf07037 129 // Accelerometer and Gyroscope Filters
BoulusAJ 0:8e87cdf07037 130 IIR_filter FilterAccX(t, Ts, 1.0f);
BoulusAJ 0:8e87cdf07037 131 IIR_filter FilterAccY(t, Ts, 1.0f);
BoulusAJ 0:8e87cdf07037 132 IIR_filter FilterGyro(t, Ts, t);
BoulusAJ 0:8e87cdf07037 133
BoulusAJ 0:8e87cdf07037 134 // Linear Scaler
BoulusAJ 3:1a714eccfe9f 135 LinearCharacteristics CurrentToVoltage(-15.0f, 15.0f, 0.0f, 5.0f);
BoulusAJ 3:1a714eccfe9f 136 LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -4000.0f, 4000.0f);
BoulusAJ 0:8e87cdf07037 137
BoulusAJ 0:8e87cdf07037 138 // PID Controllers
BoulusAJ 7:07c5b6709d87 139 PID_Cntrl_Old C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1); // Defining the 1st Loop Controller (PI-Part)
BoulusAJ 7:07c5b6709d87 140 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
BoulusAJ 0:8e87cdf07037 141
BoulusAJ 0:8e87cdf07037 142 // Timers
BoulusAJ 0:8e87cdf07037 143 Timer Loop;
BoulusAJ 0:8e87cdf07037 144
BoulusAJ 0:8e87cdf07037 145 // ----- User defined functions -----------
BoulusAJ 0:8e87cdf07037 146 void updateControllers(void); // speed controller loop (via interrupt)
BoulusAJ 0:8e87cdf07037 147
BoulusAJ 12:ba4ce3fa4f53 148 //******************************************************************************
BoulusAJ 12:ba4ce3fa4f53 149 //---------- Main Loop -------------
BoulusAJ 12:ba4ce3fa4f53 150 //******************************************************************************
BoulusAJ 0:8e87cdf07037 151 int main()
BoulusAJ 0:8e87cdf07037 152 {
BoulusAJ 12:ba4ce3fa4f53 153 // Microcontroller initialization
BoulusAJ 0:8e87cdf07037 154 pc.baud(115200);
BoulusAJ 12:ba4ce3fa4f53 155 VoltageOut.write(CurrentToVoltage(0));
BoulusAJ 2:e088fa08e244 156 Pin_3V3.write(1);
BoulusAJ 12:ba4ce3fa4f53 157 *((unsigned int *)0x40007504) = 0x400A; // Configure the ADC to the internel reference of 1.2V.
BoulusAJ 12:ba4ce3fa4f53 158 // IMU - MPU6050 initialization
BoulusAJ 12:ba4ce3fa4f53 159 mpu.initialize();
BoulusAJ 12:ba4ce3fa4f53 160 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
BoulusAJ 12:ba4ce3fa4f53 161 mpu.setFullScaleGyroRange(2u); // Change the scale of the Gyroscope to +/- 1000 degrees/sec
BoulusAJ 12:ba4ce3fa4f53 162 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // Change the scale of the Accelerometer to +/- 4g - Sensitivity: 4096 LSB/mg
BoulusAJ 0:8e87cdf07037 163
BoulusAJ 0:8e87cdf07037 164 // Reset Filters
BoulusAJ 0:8e87cdf07037 165 FilterAccX.reset(0.0f);
BoulusAJ 0:8e87cdf07037 166 FilterAccY.reset(0.0f);
BoulusAJ 0:8e87cdf07037 167 FilterGyro.reset(0.0f);
BoulusAJ 0:8e87cdf07037 168
BoulusAJ 0:8e87cdf07037 169 // Reset PID
BoulusAJ 6:122879c1503a 170 C1.reset(0.0f);
BoulusAJ 11:af811e6f9a04 171
BoulusAJ 12:ba4ce3fa4f53 172 // Control Loop Interrupt at Ts, 200Hz Rate
BoulusAJ 7:07c5b6709d87 173 ControllerLoopTimer.attach(&updateControllers, Ts);
BoulusAJ 7:07c5b6709d87 174
BoulusAJ 7:07c5b6709d87 175 }
BoulusAJ 12:ba4ce3fa4f53 176 //******************************************************************************
BoulusAJ 12:ba4ce3fa4f53 177 //---------- control loop (called via interrupt) -------------
BoulusAJ 12:ba4ce3fa4f53 178 //******************************************************************************
BoulusAJ 7:07c5b6709d87 179 void updateControllers(void)
BoulusAJ 7:07c5b6709d87 180 {
BoulusAJ 8:71babe904b92 181
BoulusAJ 12:ba4ce3fa4f53 182 // Aquire Raw Acceleration and Gyro Data form the IMU
BoulusAJ 12:ba4ce3fa4f53 183 mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);
BoulusAJ 0:8e87cdf07037 184
BoulusAJ 7:07c5b6709d87 185 // Acquire Velocity
BoulusAJ 7:07c5b6709d87 186 Velocity_Voltage_Read = Velocity_Voltage_Input.read();
BoulusAJ 11:af811e6f9a04 187 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
BoulusAJ 7:07c5b6709d87 188 Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
BoulusAJ 7:07c5b6709d87 189 Velocity = Velocity_rpm*2.0*pi/60.0;
BoulusAJ 10:4e9899cef12c 190
BoulusAJ 7:07c5b6709d87 191 // -------------- Convert Raw data to SI Units --------------------
BoulusAJ 7:07c5b6709d87 192
BoulusAJ 7:07c5b6709d87 193 //Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
BoulusAJ 7:07c5b6709d87 194 AccX_g = AccX_Raw / 8192.0f;
BoulusAJ 7:07c5b6709d87 195 AccY_g = AccY_Raw / 8192.0f;
BoulusAJ 7:07c5b6709d87 196 AccZ_g = AccZ_Raw / 8192.0f;
BoulusAJ 0:8e87cdf07037 197
BoulusAJ 7:07c5b6709d87 198 //Convert Gyroscope Raw Data to Degrees per second
BoulusAJ 7:07c5b6709d87 199 GyroX_Degrees = GyroX_Raw / 32.768f; // (2^15/1000 = 32.768)
BoulusAJ 7:07c5b6709d87 200 GyroY_Degrees = GyroY_Raw / 32.768f; // (2^15/1000 = 32.768)
BoulusAJ 7:07c5b6709d87 201 GyroZ_Degrees = GyroZ_Raw / 32.768f; // (2^15/1000 = 32.768)
BoulusAJ 0:8e87cdf07037 202
BoulusAJ 7:07c5b6709d87 203 //Convert Gyroscope Raw Data to Degrees per second
BoulusAJ 7:07c5b6709d87 204 GyroZ_RadiansPerSecond = (GyroZ_Raw / 32.768f)* pi/180.0f;
BoulusAJ 0:8e87cdf07037 205
BoulusAJ 7:07c5b6709d87 206 // ----- Combine Accelerometer Data and Gyro Data to Get Angle ------
BoulusAJ 0:8e87cdf07037 207
BoulusAJ 7:07c5b6709d87 208 Cuboid_Angle_Radians = -1*atan2(-FilterAccX(AccX_g), FilterAccY(AccY_g)) + 0.7854f + FilterGyro(GyroZ_RadiansPerSecond); // Check later if fast enough!!
BoulusAJ 7:07c5b6709d87 209 Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;
BoulusAJ 0:8e87cdf07037 210
BoulusAJ 0:8e87cdf07037 211
BoulusAJ 7:07c5b6709d87 212 // ------------------------- Controller -----------------------------
BoulusAJ 10:4e9899cef12c 213
BoulusAJ 10:4e9899cef12c 214 switch(Button_Status) {
BoulusAJ 10:4e9899cef12c 215
BoulusAJ 10:4e9899cef12c 216 case 0:
BoulusAJ 11:af811e6f9a04 217
BoulusAJ 10:4e9899cef12c 218 // PI Controller
BoulusAJ 10:4e9899cef12c 219 PID_Input = Desired_input - Velocity;
BoulusAJ 10:4e9899cef12c 220 PID_Output = C1.update(PID_Input);
BoulusAJ 10:4e9899cef12c 221
BoulusAJ 10:4e9899cef12c 222 // System input
BoulusAJ 10:4e9899cef12c 223 Sys_input_Amps = PID_Output;
BoulusAJ 10:4e9899cef12c 224 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 11:af811e6f9a04 225
BoulusAJ 10:4e9899cef12c 226 // Counter of 10 seconds then go to Case 1
BoulusAJ 10:4e9899cef12c 227 Case1_Counter++;
BoulusAJ 10:4e9899cef12c 228 if (Case1_Counter > 2000) {
BoulusAJ 10:4e9899cef12c 229 Button_Status = 1;
BoulusAJ 10:4e9899cef12c 230 }
BoulusAJ 8:71babe904b92 231
BoulusAJ 10:4e9899cef12c 232 case 1:
BoulusAJ 0:8e87cdf07037 233
BoulusAJ 10:4e9899cef12c 234 // Current Input Updater - Amperes
BoulusAJ 10:4e9899cef12c 235 // Loop 1
BoulusAJ 10:4e9899cef12c 236 Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
BoulusAJ 10:4e9899cef12c 237 // Loop 2
BoulusAJ 10:4e9899cef12c 238 Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
BoulusAJ 10:4e9899cef12c 239 // PI Controller
BoulusAJ 11:af811e6f9a04 240 //PID_Input = Desired_input - Velocity;
BoulusAJ 11:af811e6f9a04 241 //PID_Output = C1.update(PID_Input);
BoulusAJ 10:4e9899cef12c 242
BoulusAJ 12:ba4ce3fa4f53 243 // Constant Speed Control
BoulusAJ 12:ba4ce3fa4f53 244 //PID_Input = 52.36f - Velocity;
BoulusAJ 12:ba4ce3fa4f53 245 //PID_Output = C1.update(-1*PID_Input);
BoulusAJ 10:4e9899cef12c 246
BoulusAJ 10:4e9899cef12c 247 // System input
BoulusAJ 12:ba4ce3fa4f53 248 Sys_input_Amps = 0.5f; //PID_Output; //- Loop1_output - Loop2_output;
BoulusAJ 12:ba4ce3fa4f53 249 //VoltageOut.write(0.5f);
BoulusAJ 3:1a714eccfe9f 250
BoulusAJ 10:4e9899cef12c 251 if (Sys_input_Amps > uMax2) {
BoulusAJ 10:4e9899cef12c 252 Sys_input_Amps = uMax2;
BoulusAJ 10:4e9899cef12c 253 }
BoulusAJ 10:4e9899cef12c 254 if (Sys_input_Amps < uMin2) {
BoulusAJ 10:4e9899cef12c 255 Sys_input_Amps = uMin2;
BoulusAJ 10:4e9899cef12c 256 }
BoulusAJ 0:8e87cdf07037 257
BoulusAJ 10:4e9899cef12c 258 // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
BoulusAJ 12:ba4ce3fa4f53 259 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 10:4e9899cef12c 260
BoulusAJ 10:4e9899cef12c 261 }
BoulusAJ 12:ba4ce3fa4f53 262
BoulusAJ 12:ba4ce3fa4f53 263 // Print Data
BoulusAJ 12:ba4ce3fa4f53 264 if(++k >= 10) {
BoulusAJ 12:ba4ce3fa4f53 265 k = 0;
BoulusAJ 12:ba4ce3fa4f53 266 pc.printf("Some Outputs: %0.6f, %0.6f\n\r", Sys_input_Amps, Velocity);
BoulusAJ 12:ba4ce3fa4f53 267 }
BoulusAJ 12:ba4ce3fa4f53 268
BoulusAJ 12:ba4ce3fa4f53 269 pc.printf("%0.6f, %0.6f\n\r", Sys_input_Amps, Velocity);
BoulusAJ 1:d3406369c297 270 }
BoulusAJ 0:8e87cdf07037 271