Boulus Abu Joudom / Mbed 2 deprecated Cube_Mini_Solution

Cube Mini Solution

Dependencies:   mbed QEI MPU6050 BLE_API nRF51822 MCP4725 eMPL_MPU6050

main.cpp@14:fe003a27018d, 2020-05-29 (annotated)

Committer:
BoulusAJ
Date:
Fri May 29 11:31:02 2020 +0000
Revision:
14:fe003a27018d
Parent:
13:41edfb8e3b3c
Child:
15:3395ad948f44
Template Version 1.2

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