GRT_FS2019_VoiceCoil / Mbed 2 deprecated Cube_Mini_Template

Cube_Mini_Template

Dependencies:   mbed QEI MPU6050_2 BLE_API nRF51822 MCP4725 eMPL_MPU6050

main.cpp@17:a821e95aa21c, 2020-05-29 (annotated)

Committer:
BoulusAJ
Date:
Fri May 29 15:44:53 2020 +0000
Revision:
17:a821e95aa21c
Parent:
16:762867ba6aaa 
Version May 29, 2020

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BoulusAJ 0:8e87cdf07037 1 /*
BoulusAJ 0:8e87cdf07037 2 Boulus Abu Joudom
BoulusAJ 13:41edfb8e3b3c 3 Cube Mini - Prototype 1 V1
BoulusAJ 13:41edfb8e3b3c 4 Mbed Version: May 28, 2020. 18:00
BoulusAJ 0:8e87cdf07037 5 ZHAW - IMS
BoulusAJ 0:8e87cdf07037 6
BoulusAJ 13:41edfb8e3b3c 7 Cubiod Balancing Experiment - Control Lab
BoulusAJ 15:3395ad948f44 8 Template Version: 1.2
BoulusAJ 13:41edfb8e3b3c 9 Seeed Tiny Microcontroller on Cuboid 1.0
BoulusAJ 13:41edfb8e3b3c 10 */
BoulusAJ 13:41edfb8e3b3c 11
BoulusAJ 13:41edfb8e3b3c 12 /*
BoulusAJ 16:762867ba6aaa 13
BoulusAJ 16:762867ba6aaa 14 // Physical Model of Cuboid 2.0
BoulusAJ 16:762867ba6aaa 15
BoulusAJ 16:762867ba6aaa 16 % Mass Parameters
BoulusAJ 16:762867ba6aaa 17 Cube Mini - Mass = 0.6 + 0.170 + 0.015 = 0.7850 Kg, Mass of the Full Cuboid with Motor and Flywheel
BoulusAJ 16:762867ba6aaa 18 Cube Mini - Flywheel Mass = 0.170 kg
BoulusAJ 16:762867ba6aaa 19 Cube Mini - Without Flywheel Mass = 0.6150 kg
BoulusAJ 16:762867ba6aaa 20
BoulusAJ 16:762867ba6aaa 21 % Dimensions Parameters
BoulusAJ 16:762867ba6aaa 22 % Cube Mini Dimensions (10x10x10) cm
BoulusAJ 16:762867ba6aaa 23 Cube Mini - Length = 0.90*100e-3 m (The multiplication by 0.9 is to compensate for the rounded edges)
BoulusAJ 16:762867ba6aaa 24 Cube Mini - CoM = Cube Mini - Length * 0.5, Center of Mass Location
BoulusAJ 16:762867ba6aaa 25 % Inertia Parameters
BoulusAJ 16:762867ba6aaa 26 Cube Mini - Inertia (J) of the Body from the edge Edge = 0.003044 Kg.m^2, Inertia about the edge
BoulusAJ 16:762867ba6aaa 27 Cube Mini - Inertia (J) of Flyhweel and Rotor from Center = (0.0002104064 + 0.0000181) kg.m^2,
BoulusAJ 16:762867ba6aaa 28
BoulusAJ 16:762867ba6aaa 29 % Motor Parameters
BoulusAJ 16:762867ba6aaa 30 % Motor: Maxon Flat EC45, Part number: 411812, Specs: 24V, Km 36.9e-3
BoulusAJ 16:762867ba6aaa 31 Cube Mini - Motor.Km = 36.9e-3 % Torque Constant % Unit: Nm/A
BoulusAJ 16:762867ba6aaa 32 Cube Mini - Motor.Resistance = 0.608 % Terminal Resistance % Unit: Ohm
BoulusAJ 16:762867ba6aaa 33 Cube Mini - Motor.Inductance = 0.463e-3 % Unit: H, Henry
BoulusAJ 16:762867ba6aaa 34
BoulusAJ 16:762867ba6aaa 35 % PI Controller Escon
BoulusAJ 16:762867ba6aaa 36 Cube Mini - Escon.PI_Tn = 500e-6; %945e-6
BoulusAJ 16:762867ba6aaa 37 Cube Mini - Escon.PI_Kp = 100*(10/2^11);%576*(10/2^11)
BoulusAJ 16:762867ba6aaa 38
BoulusAJ 16:762867ba6aaa 39 % RC Physical Velocity Voltage Filter
BoulusAJ 16:762867ba6aaa 40 Filter.Velocity.R = 1e3 Ohm
BoulusAJ 16:762867ba6aaa 41 Filter.Velocity.C = 10e-6 F
BoulusAJ 16:762867ba6aaa 42
BoulusAJ 13:41edfb8e3b3c 43 Settings for Maxon ESCON controller (upload via ESCON Studio)
BoulusAJ 13:41edfb8e3b3c 44 Escon Studio file Location: ...
BoulusAJ 16:762867ba6aaa 45
BoulusAJ 13:41edfb8e3b3c 46 Hardware Connections
BoulusAJ 13:41edfb8e3b3c 47 Serial PC Communication
BoulusAJ 13:41edfb8e3b3c 48 UART_TX p9
BoulusAJ 13:41edfb8e3b3c 49 UART_RX p11
BoulusAJ 13:41edfb8e3b3c 50 IMU SDA and SCL
BoulusAJ 13:41edfb8e3b3c 51 MPU6050_SDA p12
BoulusAJ 13:41edfb8e3b3c 52 MPU6050_SCL p13
BoulusAJ 13:41edfb8e3b3c 53 Velocity Input as Analogue Signal from Escon
BoulusAJ 13:41edfb8e3b3c 54 Pin 5
BoulusAJ 14:fe003a27018d 55 Button Pin
BoulusAJ 14:fe003a27018d 56 Pin 6
BoulusAJ 13:41edfb8e3b3c 57 Current output to Escon occurs through I2C connection to a DAC (Digital to Analogue Converter). DAC outputs Analogue voltage to Escon
BoulusAJ 13:41edfb8e3b3c 58 Pin SDA p3
BoulusAJ 13:41edfb8e3b3c 59 Pin SCL P4
BoulusAJ 16:762867ba6aaa 60
BoulusAJ 13:41edfb8e3b3c 61 Notes
BoulusAJ 13:41edfb8e3b3c 62 The Maximum output Current allowed is 13A and the Minimum is -13A !!!
BoulusAJ 13:41edfb8e3b3c 63 All needed Libraries for the IMU (Accelerometer and Gyroscope), DAC are included.
BoulusAJ 13:41edfb8e3b3c 64 The PID is to be written by students. Use PID_Cntrl.cpp and PID_Cntrl.h as templates
BoulusAJ 13:41edfb8e3b3c 65 ...
BoulusAJ 16:762867ba6aaa 66
BoulusAJ 13:41edfb8e3b3c 67
BoulusAJ 0:8e87cdf07037 68 */
BoulusAJ 0:8e87cdf07037 69 // Libraries
BoulusAJ 13:41edfb8e3b3c 70 #include <stdbool.h>
BoulusAJ 0:8e87cdf07037 71 #include "mbed.h"
BoulusAJ 13:41edfb8e3b3c 72 #include "MPU6050.h" // IMU Library
BoulusAJ 13:41edfb8e3b3c 73 #include "mcp4725.h" // DAC Library
BoulusAJ 13:41edfb8e3b3c 74 #include "IIR_filter.h" // Filter Library (Please go to the Filter Library and add in your Code)
BoulusAJ 13:41edfb8e3b3c 75 #include "LinearCharacteristics.h" // Linear Scaling Library
BoulusAJ 13:41edfb8e3b3c 76 #include "PID_Cntrl.h" // PID Library (Please go to PID Library and add in your PID Code)
BoulusAJ 13:41edfb8e3b3c 77
BoulusAJ 0:8e87cdf07037 78
BoulusAJ 0:8e87cdf07037 79 // Serial PC Communication
BoulusAJ 0:8e87cdf07037 80 #define UART_TX p9
BoulusAJ 0:8e87cdf07037 81 #define UART_RX p11
BoulusAJ 0:8e87cdf07037 82
BoulusAJ 0:8e87cdf07037 83 // IMU SDA and SCL
BoulusAJ 0:8e87cdf07037 84 #define MPU6050_SDA p12
BoulusAJ 0:8e87cdf07037 85 #define MPU6050_SCL p13
BoulusAJ 0:8e87cdf07037 86
BoulusAJ 12:ba4ce3fa4f53 87 // PI Values
BoulusAJ 0:8e87cdf07037 88 #define PI 3.1415927f
BoulusAJ 0:8e87cdf07037 89 #define pi 3.1415927f
BoulusAJ 0:8e87cdf07037 90
BoulusAJ 13:41edfb8e3b3c 91 // PC Serial Connection
BoulusAJ 13:41edfb8e3b3c 92 Serial pc(UART_TX, UART_RX); // serial connection via USB - programmer
BoulusAJ 0:8e87cdf07037 93
BoulusAJ 12:ba4ce3fa4f53 94 // -------------------------------
BoulusAJ 12:ba4ce3fa4f53 95 // Analog/Digitsl I/O Definitions
BoulusAJ 12:ba4ce3fa4f53 96 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 97 AnalogIn Velocity_Voltage_Input(p5); // Velocity Input as Analogue Signal from Escon
BoulusAJ 13:41edfb8e3b3c 98 DigitalOut Pin_3V3(p30); // Defining P30 as 3V3 Pin for internal use. Please do not modify.
BoulusAJ 14:fe003a27018d 99 InterruptIn Button(p6); // User Button Interrput
BoulusAJ 12:ba4ce3fa4f53 100
BoulusAJ 13:41edfb8e3b3c 101 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 102 // Initialization of Values and Variables
BoulusAJ 13:41edfb8e3b3c 103 // -------------------------------
BoulusAJ 0:8e87cdf07037 104
BoulusAJ 0:8e87cdf07037 105 // Sample time of main loops
BoulusAJ 13:41edfb8e3b3c 106 float Ts = 0.007; // Around 143 Hz. Do not change. THis is the lowest value possible if printing to a serial connection is needed.
BoulusAJ 0:8e87cdf07037 107
BoulusAJ 0:8e87cdf07037 108 // MPU 6050 Variables - Acceleration and Gyroscope Raw and Converted Data Variables
BoulusAJ 0:8e87cdf07037 109 int16_t AccX_Raw, AccY_Raw, AccZ_Raw;
BoulusAJ 0:8e87cdf07037 110 int16_t GyroX_Raw, GyroY_Raw, GyroZ_Raw;
BoulusAJ 0:8e87cdf07037 111 double AccX_g, AccY_g, AccZ_g;
BoulusAJ 0:8e87cdf07037 112 double GyroX_Degrees, GyroY_Degrees, GyroZ_Degrees, GyroZ_RadiansPerSecond;
BoulusAJ 0:8e87cdf07037 113
BoulusAJ 0:8e87cdf07037 114 // printf Variable
BoulusAJ 0:8e87cdf07037 115 int k = 0;
BoulusAJ 13:41edfb8e3b3c 116
BoulusAJ 13:41edfb8e3b3c 117
BoulusAJ 13:41edfb8e3b3c 118 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 119 // Variables: Here define variables such as gains etc.
BoulusAJ 13:41edfb8e3b3c 120 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 121
BoulusAJ 13:41edfb8e3b3c 122
BoulusAJ 13:41edfb8e3b3c 123 // Accelerometer and Gyroscope
BoulusAJ 13:41edfb8e3b3c 124
BoulusAJ 13:41edfb8e3b3c 125
BoulusAJ 13:41edfb8e3b3c 126 // Cube Angle and Speed Variables
BoulusAJ 13:41edfb8e3b3c 127
BoulusAJ 13:41edfb8e3b3c 128
BoulusAJ 13:41edfb8e3b3c 129 // Low pass filter variables
BoulusAJ 13:41edfb8e3b3c 130
BoulusAJ 0:8e87cdf07037 131
BoulusAJ 0:8e87cdf07037 132 // Flywheel Position and Velocity variables
BoulusAJ 13:41edfb8e3b3c 133
BoulusAJ 0:8e87cdf07037 134
BoulusAJ 13:41edfb8e3b3c 135 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 136 // Controller Variables: Such as SS Controller Values and Saturation Limits
BoulusAJ 13:41edfb8e3b3c 137 // -------------------------------
BoulusAJ 16:762867ba6aaa 138 // Variables concerning the Controller Design is in reference to the Matlab and Simulink Files ..............
BoulusAJ 0:8e87cdf07037 139
BoulusAJ 0:8e87cdf07037 140 // Sate Space Controller Values
BoulusAJ 13:41edfb8e3b3c 141
BoulusAJ 0:8e87cdf07037 142
BoulusAJ 0:8e87cdf07037 143 // Controller Variables
BoulusAJ 13:41edfb8e3b3c 144
BoulusAJ 13:41edfb8e3b3c 145
BoulusAJ 13:41edfb8e3b3c 146 // PID (PI Parameters)
BoulusAJ 13:41edfb8e3b3c 147
BoulusAJ 0:8e87cdf07037 148
BoulusAJ 0:8e87cdf07037 149 // Saturation Parameters
BoulusAJ 0:8e87cdf07037 150
BoulusAJ 0:8e87cdf07037 151
BoulusAJ 13:41edfb8e3b3c 152 // Cuboid Escon Input Limits in Amps
BoulusAJ 13:41edfb8e3b3c 153 const float uMin = -13.0f; // Minimum Current Allowed
BoulusAJ 13:41edfb8e3b3c 154 const float uMax = 13.0f; // Maximum Current Allowe
BoulusAJ 0:8e87cdf07037 155
BoulusAJ 0:8e87cdf07037 156
BoulusAJ 13:41edfb8e3b3c 157 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 158 // User-Defined Functions
BoulusAJ 13:41edfb8e3b3c 159 // -------------------------------
BoulusAJ 0:8e87cdf07037 160
BoulusAJ 0:8e87cdf07037 161 // Interrupts
BoulusAJ 0:8e87cdf07037 162 Ticker ControllerLoopTimer; // Interrupt for control loop
BoulusAJ 0:8e87cdf07037 163
BoulusAJ 13:41edfb8e3b3c 164
BoulusAJ 13:41edfb8e3b3c 165 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 166 // Functions
BoulusAJ 13:41edfb8e3b3c 167 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 168
BoulusAJ 13:41edfb8e3b3c 169 // DAC - For Current output
BoulusAJ 0:8e87cdf07037 170 MCP4725 VoltageOut(p3, p4, MCP4725::Fast400kHz, 0);
BoulusAJ 0:8e87cdf07037 171
BoulusAJ 0:8e87cdf07037 172 // IMU - MPU6050 Functions and I2C Functions
BoulusAJ 0:8e87cdf07037 173 I2C i2c(MPU6050_SDA, MPU6050_SCL);
BoulusAJ 0:8e87cdf07037 174 MPU6050 mpu(i2c);
BoulusAJ 0:8e87cdf07037 175
BoulusAJ 0:8e87cdf07037 176 // Timers
BoulusAJ 0:8e87cdf07037 177 Timer Loop;
BoulusAJ 0:8e87cdf07037 178
BoulusAJ 13:41edfb8e3b3c 179 // Accelerometer and Gyroscope Filters
BoulusAJ 13:41edfb8e3b3c 180
BoulusAJ 13:41edfb8e3b3c 181 // PID (PI) Controller
BoulusAJ 13:41edfb8e3b3c 182
BoulusAJ 13:41edfb8e3b3c 183
BoulusAJ 0:8e87cdf07037 184 // ----- User defined functions -----------
BoulusAJ 13:41edfb8e3b3c 185
BoulusAJ 13:41edfb8e3b3c 186 // Controller loop (via interrupt)
BoulusAJ 16:762867ba6aaa 187 void updateControllers(void);
BoulusAJ 13:41edfb8e3b3c 188
BoulusAJ 13:41edfb8e3b3c 189 // Linear Scaler - Follow the mapping mentioned in the Notes above
BoulusAJ 0:8e87cdf07037 190
BoulusAJ 12:ba4ce3fa4f53 191 //******************************************************************************
BoulusAJ 14:fe003a27018d 192 //----------------------------- Main Loop --------------------------------------
BoulusAJ 12:ba4ce3fa4f53 193 //******************************************************************************
BoulusAJ 0:8e87cdf07037 194 int main()
BoulusAJ 0:8e87cdf07037 195 {
BoulusAJ 12:ba4ce3fa4f53 196 // Microcontroller initialization
BoulusAJ 2:e088fa08e244 197 Pin_3V3.write(1);
BoulusAJ 12:ba4ce3fa4f53 198 *((unsigned int *)0x40007504) = 0x400A; // Configure the ADC to the internel reference of 1.2V.
BoulusAJ 13:41edfb8e3b3c 199 // IMU - MPU6050 initialization - Do NOT Modify
BoulusAJ 12:ba4ce3fa4f53 200 mpu.initialize();
BoulusAJ 13:41edfb8e3b3c 201 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
BoulusAJ 12:ba4ce3fa4f53 202 mpu.setFullScaleGyroRange(2u); // Change the scale of the Gyroscope to +/- 1000 degrees/sec
BoulusAJ 12:ba4ce3fa4f53 203 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // Change the scale of the Accelerometer to +/- 4g - Sensitivity: 4096 LSB/mg
BoulusAJ 16:762867ba6aaa 204
BoulusAJ 13:41edfb8e3b3c 205 // Serial Communication
BoulusAJ 13:41edfb8e3b3c 206 pc.baud(115200);
BoulusAJ 16:762867ba6aaa 207
BoulusAJ 13:41edfb8e3b3c 208 VoltageOut.write(2.5); // Output Zero Current to the Motor
BoulusAJ 0:8e87cdf07037 209
BoulusAJ 13:41edfb8e3b3c 210 // Reset Filters and PID Controller
BoulusAJ 11:af811e6f9a04 211
BoulusAJ 12:ba4ce3fa4f53 212 // Control Loop Interrupt at Ts, 200Hz Rate
BoulusAJ 7:07c5b6709d87 213 ControllerLoopTimer.attach(&updateControllers, Ts);
BoulusAJ 7:07c5b6709d87 214
BoulusAJ 7:07c5b6709d87 215 }
BoulusAJ 12:ba4ce3fa4f53 216 //******************************************************************************
BoulusAJ 14:fe003a27018d 217 //------------------ Control Loop (called via interrupt) -----------------------
BoulusAJ 12:ba4ce3fa4f53 218 //******************************************************************************
BoulusAJ 7:07c5b6709d87 219 void updateControllers(void)
BoulusAJ 7:07c5b6709d87 220 {
BoulusAJ 16:762867ba6aaa 221
BoulusAJ 13:41edfb8e3b3c 222 // Acquire Velocity
BoulusAJ 13:41edfb8e3b3c 223 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
BoulusAJ 13:41edfb8e3b3c 224 // Velocity = .... Refer to mapping
BoulusAJ 13:41edfb8e3b3c 225
BoulusAJ 8:71babe904b92 226
BoulusAJ 12:ba4ce3fa4f53 227 // Aquire Raw Acceleration and Gyro Data form the IMU
BoulusAJ 12:ba4ce3fa4f53 228 mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);
BoulusAJ 0:8e87cdf07037 229
BoulusAJ 7:07c5b6709d87 230 // -------------- Convert Raw data to SI Units --------------------
BoulusAJ 7:07c5b6709d87 231
BoulusAJ 7:07c5b6709d87 232 //Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
BoulusAJ 16:762867ba6aaa 233 AccX_g = AccX_Raw / 8192.0f;
BoulusAJ 16:762867ba6aaa 234 AccY_g = AccY_Raw / 8192.0f;
BoulusAJ 16:762867ba6aaa 235 AccZ_g = AccZ_Raw / 8192.0f;
BoulusAJ 0:8e87cdf07037 236
BoulusAJ 13:41edfb8e3b3c 237 //Convert Gyroscope Raw Data to Degrees per second - (2^15/1000 = 32.768)
BoulusAJ 16:762867ba6aaa 238 GyroX_Degrees = GyroX_Raw / 32.768f;
BoulusAJ 16:762867ba6aaa 239 GyroY_Degrees = GyroY_Raw / 32.768f;
BoulusAJ 16:762867ba6aaa 240 GyroZ_Degrees = GyroZ_Raw / 32.768f;
BoulusAJ 0:8e87cdf07037 241
BoulusAJ 7:07c5b6709d87 242 //Convert Gyroscope Raw Data to Degrees per second
BoulusAJ 7:07c5b6709d87 243 GyroZ_RadiansPerSecond = (GyroZ_Raw / 32.768f)* pi/180.0f;
BoulusAJ 0:8e87cdf07037 244
BoulusAJ 7:07c5b6709d87 245 // ----- Combine Accelerometer Data and Gyro Data to Get Angle ------
BoulusAJ 0:8e87cdf07037 246
BoulusAJ 16:762867ba6aaa 247
BoulusAJ 0:8e87cdf07037 248
BoulusAJ 7:07c5b6709d87 249 // ------------------------- Controller -----------------------------
BoulusAJ 10:4e9899cef12c 250
BoulusAJ 14:fe003a27018d 251 // Switch Statement Maybe?......
BoulusAJ 16:762867ba6aaa 252
BoulusAJ 14:fe003a27018d 253 // ----------------
BoulusAJ 12:ba4ce3fa4f53 254
BoulusAJ 12:ba4ce3fa4f53 255 // Print Data
BoulusAJ 13:41edfb8e3b3c 256 if(++k >= 100) {
BoulusAJ 12:ba4ce3fa4f53 257 k = 0;
BoulusAJ 12:ba4ce3fa4f53 258 pc.printf("Some Outputs: %0.6f, %0.6f\n\r", Sys_input_Amps, Velocity);
BoulusAJ 12:ba4ce3fa4f53 259 }
BoulusAJ 12:ba4ce3fa4f53 260
BoulusAJ 13:41edfb8e3b3c 261
BoulusAJ 1:d3406369c297 262 }
BoulusAJ 14:fe003a27018d 263 //******************************************************************************
BoulusAJ 14:fe003a27018d 264 //------------------ User functions like buttens handle etc. -------------------
BoulusAJ 14:fe003a27018d 265 //******************************************************************************
BoulusAJ 0:8e87cdf07037 266
BoulusAJ 14:fe003a27018d 267 //...