Boulus Abu Joudom / Mbed 2 deprecated Cube_Mini_Solution

Cube Mini Solution

Dependencies:   mbed QEI MPU6050 BLE_API nRF51822 MCP4725 eMPL_MPU6050

main.cpp@24:c953b74ed88b, 2020-08-28 (annotated)

Committer:
BoulusAJ
Date:
Fri Aug 28 09:23:40 2020 +0000
Revision:
24:c953b74ed88b
Parent:
23:b7b7271deb11 
Prototype 2 - August 28, 2020.; Controller working well with updated parameters; State machine still needs updates

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 18:3b1aa67e11ca 13
BoulusAJ 18:3b1aa67e11ca 14 // Physical Model of Cuboid 2.0
BoulusAJ 21:762f8769cba4 15
BoulusAJ 18:3b1aa67e11ca 16 % Mass Parameters
BoulusAJ 18:3b1aa67e11ca 17 Cube Mini - Mass = 0.6 + 0.170 + 0.015 = 0.7850 Kg, Mass of the Full Cuboid with Motor and Flywheel
BoulusAJ 18:3b1aa67e11ca 18 Cube Mini - Flywheel Mass = 0.170 kg
BoulusAJ 18:3b1aa67e11ca 19 Cube Mini - Without Flywheel Mass = 0.6150 kg
BoulusAJ 21:762f8769cba4 20
BoulusAJ 18:3b1aa67e11ca 21 % Dimensions Parameters
BoulusAJ 18:3b1aa67e11ca 22 % Cube Mini Dimensions (10x10x10) cm
BoulusAJ 18:3b1aa67e11ca 23 Cube Mini - Length = 0.90*100e-3 m (The multiplication by 0.9 is to compensate for the rounded edges)
BoulusAJ 21:762f8769cba4 24 Cube Mini - CoM = Cube Mini - Length * 0.5, Center of Mass Location
BoulusAJ 18:3b1aa67e11ca 25 % Inertia Parameters
BoulusAJ 18:3b1aa67e11ca 26 Cube Mini - Inertia (J) of the Body from the edge Edge = 0.003044 Kg.m^2, Inertia about the edge
BoulusAJ 18:3b1aa67e11ca 27 Cube Mini - Inertia (J) of Flyhweel and Rotor from Center = (0.0002104064 + 0.0000181) kg.m^2,
BoulusAJ 21:762f8769cba4 28
BoulusAJ 18:3b1aa67e11ca 29 % Motor Parameters
BoulusAJ 18:3b1aa67e11ca 30 % Motor: Maxon Flat EC45, Part number: 411812, Specs: 24V, Km 36.9e-3
BoulusAJ 18:3b1aa67e11ca 31 Cube Mini - Motor.Km = 36.9e-3 % Torque Constant % Unit: Nm/A
BoulusAJ 18:3b1aa67e11ca 32 Cube Mini - Motor.Resistance = 0.608 % Terminal Resistance % Unit: Ohm
BoulusAJ 18:3b1aa67e11ca 33 Cube Mini - Motor.Inductance = 0.463e-3 % Unit: H, Henry
BoulusAJ 21:762f8769cba4 34
BoulusAJ 21:762f8769cba4 35 % PI Controller Escon
BoulusAJ 18:3b1aa67e11ca 36 Cube Mini - Escon.PI_Tn = 500e-6; %945e-6
BoulusAJ 18:3b1aa67e11ca 37 Cube Mini - Escon.PI_Kp = 100*(10/2^11);%576*(10/2^11)
BoulusAJ 21:762f8769cba4 38
BoulusAJ 18:3b1aa67e11ca 39 % RC Physical Velocity Voltage Filter
BoulusAJ 18:3b1aa67e11ca 40 Filter.Velocity.R = 1e3 Ohm
BoulusAJ 18:3b1aa67e11ca 41 Filter.Velocity.C = 10e-6 F
BoulusAJ 18:3b1aa67e11ca 42
BoulusAJ 13:41edfb8e3b3c 43 Settings for Maxon ESCON controller (upload via ESCON Studio)
BoulusAJ 13:41edfb8e3b3c 44 Escon Studio file Location: ...
BoulusAJ 16:ff375f62a95f 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:ff375f62a95f 60
BoulusAJ 16:ff375f62a95f 61 Escon Mapping of voltage to current and rotational velocity to voltage
BoulusAJ 16:ff375f62a95f 62 Escon Linear Mapping of input current from voltage is 0V is -15A and 5V is 15A, hence 2.5V is 0A !!!
BoulusAJ 16:ff375f62a95f 63 Escon Linear Mapping of output rotational velocity (in rpm) to voltage is -4000rpm is 0V and 4000rpm is 3.0V, hence 1.5V is 0 RPM !!!
BoulusAJ 16:ff375f62a95f 64
BoulusAJ 13:41edfb8e3b3c 65 Notes
BoulusAJ 13:41edfb8e3b3c 66 The Maximum output Current allowed is 13A and the Minimum is -13A !!!
BoulusAJ 16:ff375f62a95f 67 Escon Linear Mapping of input current from voltage is 0V is -15A and 5V is 15A, hence 2.5V is 0A !!!
BoulusAJ 13:41edfb8e3b3c 68 All needed Libraries for the IMU (Accelerometer and Gyroscope), DAC are included.
BoulusAJ 13:41edfb8e3b3c 69 The PID is to be written by students. Use PID_Cntrl.cpp and PID_Cntrl.h as templates
BoulusAJ 13:41edfb8e3b3c 70 ...
BoulusAJ 16:ff375f62a95f 71
BoulusAJ 13:41edfb8e3b3c 72
BoulusAJ 0:8e87cdf07037 73 */
BoulusAJ 0:8e87cdf07037 74 // Libraries
BoulusAJ 13:41edfb8e3b3c 75 #include <stdbool.h>
BoulusAJ 0:8e87cdf07037 76 #include "mbed.h"
BoulusAJ 13:41edfb8e3b3c 77 #include "MPU6050.h" // IMU Library
BoulusAJ 13:41edfb8e3b3c 78 #include "mcp4725.h" // DAC Library
BoulusAJ 13:41edfb8e3b3c 79 #include "IIR_filter.h" // Filter Library (Please go to the Filter Library and add in your Code)
BoulusAJ 13:41edfb8e3b3c 80 #include "LinearCharacteristics.h" // Linear Scaling Library
BoulusAJ 13:41edfb8e3b3c 81 #include "PID_Cntrl.h" // PID Library (Please go to PID Library and add in your PID Code)
BoulusAJ 13:41edfb8e3b3c 82
BoulusAJ 0:8e87cdf07037 83
BoulusAJ 0:8e87cdf07037 84 // Serial PC Communication
BoulusAJ 0:8e87cdf07037 85 #define UART_TX p9
BoulusAJ 0:8e87cdf07037 86 #define UART_RX p11
BoulusAJ 0:8e87cdf07037 87
BoulusAJ 0:8e87cdf07037 88 // IMU SDA and SCL
BoulusAJ 0:8e87cdf07037 89 #define MPU6050_SDA p12
BoulusAJ 0:8e87cdf07037 90 #define MPU6050_SCL p13
BoulusAJ 0:8e87cdf07037 91
BoulusAJ 12:ba4ce3fa4f53 92 // PI Values
BoulusAJ 0:8e87cdf07037 93 #define PI 3.1415927f
BoulusAJ 0:8e87cdf07037 94 #define pi 3.1415927f
BoulusAJ 0:8e87cdf07037 95
BoulusAJ 13:41edfb8e3b3c 96 // PC Serial Connection
BoulusAJ 13:41edfb8e3b3c 97 Serial pc(UART_TX, UART_RX); // serial connection via USB - programmer
BoulusAJ 0:8e87cdf07037 98
BoulusAJ 12:ba4ce3fa4f53 99 // -------------------------------
BoulusAJ 12:ba4ce3fa4f53 100 // Analog/Digitsl I/O Definitions
BoulusAJ 12:ba4ce3fa4f53 101 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 102 AnalogIn Velocity_Voltage_Input(p5); // Velocity Input as Analogue Signal from Escon
BoulusAJ 13:41edfb8e3b3c 103 DigitalOut Pin_3V3(p30); // Defining P30 as 3V3 Pin for internal use. Please do not modify.
BoulusAJ 14:fe003a27018d 104 InterruptIn Button(p6); // User Button Interrput
BoulusAJ 19:2ffbd2af2b7f 105 DigitalOut GLED(p21);
BoulusAJ 12:ba4ce3fa4f53 106
BoulusAJ 13:41edfb8e3b3c 107 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 108 // Initialization of Values and Variables
BoulusAJ 13:41edfb8e3b3c 109 // -------------------------------
BoulusAJ 0:8e87cdf07037 110
BoulusAJ 0:8e87cdf07037 111 // Sample time of main loops
BoulusAJ 16:ff375f62a95f 112 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 113
BoulusAJ 0:8e87cdf07037 114 // MPU 6050 Variables - Acceleration and Gyroscope Raw and Converted Data Variables
BoulusAJ 0:8e87cdf07037 115 int16_t AccX_Raw, AccY_Raw, AccZ_Raw;
BoulusAJ 0:8e87cdf07037 116 int16_t GyroX_Raw, GyroY_Raw, GyroZ_Raw;
BoulusAJ 0:8e87cdf07037 117 double AccX_g, AccY_g, AccZ_g;
BoulusAJ 0:8e87cdf07037 118 double GyroX_Degrees, GyroY_Degrees, GyroZ_Degrees, GyroZ_RadiansPerSecond;
BoulusAJ 0:8e87cdf07037 119
BoulusAJ 0:8e87cdf07037 120 // printf Variable
BoulusAJ 0:8e87cdf07037 121 int k = 0;
BoulusAJ 13:41edfb8e3b3c 122
BoulusAJ 13:41edfb8e3b3c 123
BoulusAJ 13:41edfb8e3b3c 124 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 125 // Variables: Here define variables such as gains etc.
BoulusAJ 13:41edfb8e3b3c 126 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 127
BoulusAJ 13:41edfb8e3b3c 128 // Accelerometer and Gyroscope
BoulusAJ 13:41edfb8e3b3c 129
BoulusAJ 13:41edfb8e3b3c 130
BoulusAJ 13:41edfb8e3b3c 131 // Cube Angle and Speed Variables
BoulusAJ 16:ff375f62a95f 132 double Cuboid_Angle_Radians, Cuboid_Angle_Degrees;
BoulusAJ 16:ff375f62a95f 133 double Cuboid_Angle_Speed_Degrees = 0.0;
BoulusAJ 13:41edfb8e3b3c 134
BoulusAJ 13:41edfb8e3b3c 135 // Low pass filter variables
BoulusAJ 24:c953b74ed88b 136 float t = 0.9f; //Old 0.5f;
BoulusAJ 0:8e87cdf07037 137
BoulusAJ 0:8e87cdf07037 138 // Flywheel Position and Velocity variables
BoulusAJ 16:ff375f62a95f 139 double Velocity_Input_Voltage = 0.0f;
BoulusAJ 16:ff375f62a95f 140 double Velocity, Velocity_Voltage, Velocity_rpm, Velocity_Voltage_Read;
BoulusAJ 13:41edfb8e3b3c 141
BoulusAJ 16:ff375f62a95f 142 // User Button
BoulusAJ 16:ff375f62a95f 143 int Button_Status = 0; // User Button Status
BoulusAJ 16:ff375f62a95f 144 Timer T_Button; // define timer for button
BoulusAJ 0:8e87cdf07037 145
BoulusAJ 13:41edfb8e3b3c 146 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 147 // Controller Variables: Such as SS Controller Values and Saturation Limits
BoulusAJ 13:41edfb8e3b3c 148 // -------------------------------
BoulusAJ 16:ff375f62a95f 149 // Variables concerning the Controller Design is in reference to the Matlab and Simulink Files ..............
BoulusAJ 16:ff375f62a95f 150
BoulusAJ 16:ff375f62a95f 151 // User Input
BoulusAJ 16:ff375f62a95f 152 float Desired_input = 0.0f;
BoulusAJ 16:ff375f62a95f 153 // Initial System input in Amperes
BoulusAJ 16:ff375f62a95f 154 float Sys_input_Amps = 0.0f;
BoulusAJ 0:8e87cdf07037 155
BoulusAJ 0:8e87cdf07037 156 // Sate Space Controller Values
BoulusAJ 24:c953b74ed88b 157 //float K_SS_Controller [2] = {-57.1176*0.3, -2.6398*1.5}; // From Matlab
BoulusAJ 24:c953b74ed88b 158 float K_SS_Controller [2] = {-64.6865, -4.2719}; // From Matlab
BoulusAJ 24:c953b74ed88b 159 //float K_SS_Controller [2] = {-45.7134, -2.3733}; // From Matlab
BoulusAJ 24:c953b74ed88b 160 //float K_SS_Controller [2] = {-27.775, -2.034}; // From Matlab
BoulusAJ 0:8e87cdf07037 161
BoulusAJ 0:8e87cdf07037 162 // Controller Variables
BoulusAJ 16:ff375f62a95f 163 float Loop1_output; // Loop 1 controller output
BoulusAJ 16:ff375f62a95f 164 float Loop2_output; // Loop 2 controller output
BoulusAJ 16:ff375f62a95f 165 float PID_Input, PID_Output, PID_Input2, PID_Output2;
BoulusAJ 13:41edfb8e3b3c 166
BoulusAJ 13:41edfb8e3b3c 167 // PID (PI Parameters)
BoulusAJ 13:41edfb8e3b3c 168
BoulusAJ 16:ff375f62a95f 169 // PID 1 - Velocity control after lift-up
BoulusAJ 24:c953b74ed88b 170 float Kp_1 = -0.02;//- 0.01;
BoulusAJ 24:c953b74ed88b 171 float Ki_1 = -0.004;//- 0.05;
BoulusAJ 21:762f8769cba4 172 //float Ki_1 = 0;
BoulusAJ 24:c953b74ed88b 173 //float Kp_1 = - 0.09;
BoulusAJ 24:c953b74ed88b 174 //float Ki_1 = - 0.09*0.5*0.5;
BoulusAJ 16:ff375f62a95f 175 float Kd_1 = 0; // No D-Part
BoulusAJ 16:ff375f62a95f 176 float Tf_1 = 1; // No D-Part
BoulusAJ 23:b7b7271deb11 177
BoulusAJ 16:ff375f62a95f 178 // Controller Loop (PI-Part) in Case 2 (breaking case)
BoulusAJ 17:04eebb7c29b5 179 float Kp_2 = 0.25/4.0;
BoulusAJ 17:04eebb7c29b5 180 float Ki_2 = 0.25/4.0;
BoulusAJ 16:ff375f62a95f 181 float Kd_2 = 0; // No D-Part
BoulusAJ 16:ff375f62a95f 182 float Tf_2 = 1; // No D-Part
BoulusAJ 0:8e87cdf07037 183
BoulusAJ 0:8e87cdf07037 184 // Saturation Parameters
BoulusAJ 16:ff375f62a95f 185 // PI Controller Limits
BoulusAJ 24:c953b74ed88b 186 const float uMin1 = -5.0f;
BoulusAJ 24:c953b74ed88b 187 const float uMax1= 5.0f;
BoulusAJ 0:8e87cdf07037 188
BoulusAJ 13:41edfb8e3b3c 189 // Cuboid Escon Input Limits in Amps
BoulusAJ 24:c953b74ed88b 190 const float uMin = -15.0f; // Minimum Current Allowed
BoulusAJ 24:c953b74ed88b 191 const float uMax = 15.0f; // Maximum Current Allowed
BoulusAJ 21:762f8769cba4 192
BoulusAJ 21:762f8769cba4 193 // temp
BoulusAJ 21:762f8769cba4 194 int counter = 0;
BoulusAJ 0:8e87cdf07037 195
BoulusAJ 0:8e87cdf07037 196
BoulusAJ 13:41edfb8e3b3c 197 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 198 // User-Defined Functions
BoulusAJ 13:41edfb8e3b3c 199 // -------------------------------
BoulusAJ 0:8e87cdf07037 200
BoulusAJ 16:ff375f62a95f 201 // USer Button
BoulusAJ 16:ff375f62a95f 202 void pressed(void); // user Button pressed
BoulusAJ 16:ff375f62a95f 203 void released(void); // user Button released
BoulusAJ 16:ff375f62a95f 204
BoulusAJ 16:ff375f62a95f 205 // Controller loop (via interrupt)
BoulusAJ 16:ff375f62a95f 206 void updateControllers(void); // controller loop (via interrupt)
BoulusAJ 16:ff375f62a95f 207
BoulusAJ 16:ff375f62a95f 208 // Linear Scaler - Follow the mapping mentioned in the Notes above
BoulusAJ 16:ff375f62a95f 209 // Linear Scaler
BoulusAJ 16:ff375f62a95f 210 LinearCharacteristics CurrentToVoltage(-15.0f, 15.0f, 0.0f, 5.0f);
BoulusAJ 16:ff375f62a95f 211 LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -4000.0f, 4000.0f);
BoulusAJ 16:ff375f62a95f 212
BoulusAJ 16:ff375f62a95f 213 // PID (PI) Controller (My PID Controller is fine but needs clarity updates)
BoulusAJ 16:ff375f62a95f 214 PID_Cntrl C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1); // Defining the 1st Loop Controller (PI-Part)
BoulusAJ 16:ff375f62a95f 215 PID_Cntrl 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 20:b142ae11a12a 216 PID_Cntrl C3(Kp_1*3,Ki_1*2.0,Kd_2,Tf_2,Ts,uMin1,uMax1); // Safety Implementation in Case 4 PID
BoulusAJ 16:ff375f62a95f 217
BoulusAJ 16:ff375f62a95f 218 // Accelerometer and Gyroscope Filters
BoulusAJ 16:ff375f62a95f 219 IIR_filter FilterAccX(t, Ts, 1.0f);
BoulusAJ 16:ff375f62a95f 220 IIR_filter FilterAccY(t, Ts, 1.0f);
BoulusAJ 16:ff375f62a95f 221 IIR_filter FilterGyro(t, Ts, t);
BoulusAJ 16:ff375f62a95f 222
BoulusAJ 0:8e87cdf07037 223 // Interrupts
BoulusAJ 0:8e87cdf07037 224 Ticker ControllerLoopTimer; // Interrupt for control loop
BoulusAJ 0:8e87cdf07037 225
BoulusAJ 13:41edfb8e3b3c 226
BoulusAJ 13:41edfb8e3b3c 227 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 228 // Functions
BoulusAJ 13:41edfb8e3b3c 229 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 230
BoulusAJ 13:41edfb8e3b3c 231 // DAC - For Current output
BoulusAJ 0:8e87cdf07037 232 MCP4725 VoltageOut(p3, p4, MCP4725::Fast400kHz, 0);
BoulusAJ 0:8e87cdf07037 233
BoulusAJ 0:8e87cdf07037 234 // IMU - MPU6050 Functions and I2C Functions
BoulusAJ 0:8e87cdf07037 235 I2C i2c(MPU6050_SDA, MPU6050_SCL);
BoulusAJ 0:8e87cdf07037 236 MPU6050 mpu(i2c);
BoulusAJ 0:8e87cdf07037 237
BoulusAJ 0:8e87cdf07037 238 // Timers
BoulusAJ 0:8e87cdf07037 239 Timer Loop;
BoulusAJ 0:8e87cdf07037 240
BoulusAJ 12:ba4ce3fa4f53 241 //******************************************************************************
BoulusAJ 14:fe003a27018d 242 //----------------------------- Main Loop --------------------------------------
BoulusAJ 12:ba4ce3fa4f53 243 //******************************************************************************
BoulusAJ 0:8e87cdf07037 244 int main()
BoulusAJ 0:8e87cdf07037 245 {
BoulusAJ 21:762f8769cba4 246
BoulusAJ 17:04eebb7c29b5 247 VoltageOut.write(2.5); // Output Zero Current to the Motor
BoulusAJ 19:2ffbd2af2b7f 248 GLED = 1;
BoulusAJ 17:04eebb7c29b5 249
BoulusAJ 12:ba4ce3fa4f53 250 // Microcontroller initialization
BoulusAJ 2:e088fa08e244 251 Pin_3V3.write(1);
BoulusAJ 12:ba4ce3fa4f53 252 *((unsigned int *)0x40007504) = 0x400A; // Configure the ADC to the internel reference of 1.2V.
BoulusAJ 13:41edfb8e3b3c 253 // IMU - MPU6050 initialization - Do NOT Modify
BoulusAJ 12:ba4ce3fa4f53 254 mpu.initialize();
BoulusAJ 17:04eebb7c29b5 255 bool mpu6050TestResult = mpu.testConnection();
BoulusAJ 17:04eebb7c29b5 256 if(mpu6050TestResult) {
BoulusAJ 17:04eebb7c29b5 257 pc.printf("MPU6050 test passed \n\r");
BoulusAJ 17:04eebb7c29b5 258 } else {
BoulusAJ 17:04eebb7c29b5 259 pc.printf("MPU6050 test failed \n\r");
BoulusAJ 17:04eebb7c29b5 260 }
BoulusAJ 13:41edfb8e3b3c 261 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 262 mpu.setFullScaleGyroRange(2u); // Change the scale of the Gyroscope to +/- 1000 degrees/sec
BoulusAJ 12:ba4ce3fa4f53 263 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // Change the scale of the Accelerometer to +/- 4g - Sensitivity: 4096 LSB/mg
BoulusAJ 16:ff375f62a95f 264
BoulusAJ 13:41edfb8e3b3c 265 // Serial Communication
BoulusAJ 13:41edfb8e3b3c 266 pc.baud(115200);
BoulusAJ 16:ff375f62a95f 267
BoulusAJ 17:04eebb7c29b5 268
BoulusAJ 0:8e87cdf07037 269
BoulusAJ 13:41edfb8e3b3c 270 // Reset Filters and PID Controller
BoulusAJ 16:ff375f62a95f 271 FilterAccX.reset(0.0f);
BoulusAJ 16:ff375f62a95f 272 FilterAccY.reset(0.0f);
BoulusAJ 16:ff375f62a95f 273 FilterGyro.reset(0.0f);
BoulusAJ 16:ff375f62a95f 274 C1.reset(0.0f);
BoulusAJ 16:ff375f62a95f 275 C2.reset(0.0f);
BoulusAJ 16:ff375f62a95f 276 C3.reset(0.0f);
BoulusAJ 11:af811e6f9a04 277
BoulusAJ 16:ff375f62a95f 278 pc.printf("Hello World!\n\r");
BoulusAJ 23:b7b7271deb11 279 wait(1);
BoulusAJ 12:ba4ce3fa4f53 280 // Control Loop Interrupt at Ts, 200Hz Rate
BoulusAJ 7:07c5b6709d87 281 ControllerLoopTimer.attach(&updateControllers, Ts);
BoulusAJ 16:ff375f62a95f 282 Button.fall(&pressed); // attach key pressed function
BoulusAJ 16:ff375f62a95f 283 Button.rise(&released); // attach key pressed function
BoulusAJ 16:ff375f62a95f 284
BoulusAJ 7:07c5b6709d87 285
BoulusAJ 7:07c5b6709d87 286 }
BoulusAJ 12:ba4ce3fa4f53 287 //******************************************************************************
BoulusAJ 14:fe003a27018d 288 //------------------ Control Loop (called via interrupt) -----------------------
BoulusAJ 12:ba4ce3fa4f53 289 //******************************************************************************
BoulusAJ 7:07c5b6709d87 290 void updateControllers(void)
BoulusAJ 7:07c5b6709d87 291 {
BoulusAJ 16:ff375f62a95f 292
BoulusAJ 13:41edfb8e3b3c 293 // Acquire Velocity
BoulusAJ 13:41edfb8e3b3c 294 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 295 // Velocity = .... Refer to mapping
BoulusAJ 16:ff375f62a95f 296 Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
BoulusAJ 16:ff375f62a95f 297 Velocity = Velocity_rpm*2.0*pi/60.0;
BoulusAJ 13:41edfb8e3b3c 298
BoulusAJ 8:71babe904b92 299
BoulusAJ 12:ba4ce3fa4f53 300 // Aquire Raw Acceleration and Gyro Data form the IMU
BoulusAJ 12:ba4ce3fa4f53 301 mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);
BoulusAJ 0:8e87cdf07037 302
BoulusAJ 7:07c5b6709d87 303 // -------------- Convert Raw data to SI Units --------------------
BoulusAJ 7:07c5b6709d87 304
BoulusAJ 17:04eebb7c29b5 305 //Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
BoulusAJ 16:ff375f62a95f 306 AccX_g = AccX_Raw / 8192.0f;
BoulusAJ 16:ff375f62a95f 307 AccY_g = AccY_Raw / 8192.0f;
BoulusAJ 16:ff375f62a95f 308 AccZ_g = AccZ_Raw / 8192.0f;
BoulusAJ 0:8e87cdf07037 309
BoulusAJ 17:04eebb7c29b5 310 //Convert Gyroscope Raw Data to Degrees per second
BoulusAJ 17:04eebb7c29b5 311 GyroX_Degrees = GyroX_Raw / 32.768f; // (2^15/1000 = 32.768)
BoulusAJ 17:04eebb7c29b5 312 GyroY_Degrees = GyroY_Raw / 32.768f; // (2^15/1000 = 32.768)
BoulusAJ 17:04eebb7c29b5 313 GyroZ_Degrees = GyroZ_Raw / 32.768f; // (2^15/1000 = 32.768)
BoulusAJ 0:8e87cdf07037 314
BoulusAJ 7:07c5b6709d87 315 //Convert Gyroscope Raw Data to Degrees per second
BoulusAJ 7:07c5b6709d87 316 GyroZ_RadiansPerSecond = (GyroZ_Raw / 32.768f)* pi/180.0f;
BoulusAJ 0:8e87cdf07037 317
BoulusAJ 7:07c5b6709d87 318 // ----- Combine Accelerometer Data and Gyro Data to Get Angle ------
BoulusAJ 17:04eebb7c29b5 319
BoulusAJ 24:c953b74ed88b 320 Cuboid_Angle_Radians = -1*atan2(-FilterAccX(AccX_g), FilterAccY(AccY_g)) + 0.7854f + FilterGyro(GyroZ_RadiansPerSecond); //- pi;
BoulusAJ 16:ff375f62a95f 321 Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;
BoulusAJ 0:8e87cdf07037 322
BoulusAJ 0:8e87cdf07037 323
BoulusAJ 21:762f8769cba4 324
BoulusAJ 7:07c5b6709d87 325 // ------------------------- Controller -----------------------------
BoulusAJ 10:4e9899cef12c 326
BoulusAJ 14:fe003a27018d 327 // Switch Statement Maybe?......
BoulusAJ 16:ff375f62a95f 328 switch(Button_Status) {
BoulusAJ 16:ff375f62a95f 329
BoulusAJ 16:ff375f62a95f 330 case 0: // Output of 0 Amps
BoulusAJ 23:b7b7271deb11 331
BoulusAJ 17:04eebb7c29b5 332 Sys_input_Amps = 0.0;
BoulusAJ 23:b7b7271deb11 333 //counter = 0;
BoulusAJ 20:b142ae11a12a 334 C1.reset(0.0f);
BoulusAJ 16:ff375f62a95f 335 break;
BoulusAJ 16:ff375f62a95f 336
BoulusAJ 16:ff375f62a95f 337 case 1: // Breaking
BoulusAJ 16:ff375f62a95f 338
BoulusAJ 16:ff375f62a95f 339 // PI Controller
BoulusAJ 16:ff375f62a95f 340 PID_Input = 0.0f - Velocity;
BoulusAJ 20:b142ae11a12a 341 //PID_Output = C2.update(PID_Input);
BoulusAJ 20:b142ae11a12a 342 PID_Output = 0;
BoulusAJ 20:b142ae11a12a 343 C1.reset(0.0f);
BoulusAJ 20:b142ae11a12a 344 float Test1 = C1.reset(0.0f);
BoulusAJ 24:c953b74ed88b 345 //pc.printf("PI Outpu: %0.6f\n\r", Test1);
BoulusAJ 16:ff375f62a95f 346 // System input
BoulusAJ 20:b142ae11a12a 347 //Sys_input_Amps = PID_Output;
BoulusAJ 20:b142ae11a12a 348 Sys_input_Amps = 0;
BoulusAJ 21:762f8769cba4 349
BoulusAJ 23:b7b7271deb11 350 //counter = 0;
BoulusAJ 16:ff375f62a95f 351 break;
BoulusAJ 16:ff375f62a95f 352
BoulusAJ 16:ff375f62a95f 353 case 2: // Balancing and lift-up
BoulusAJ 16:ff375f62a95f 354
BoulusAJ 21:762f8769cba4 355
BoulusAJ 17:04eebb7c29b5 356 // Current Input Updater - Amperes
BoulusAJ 16:ff375f62a95f 357 // Loop 1
BoulusAJ 16:ff375f62a95f 358 Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
BoulusAJ 16:ff375f62a95f 359 // Loop 2
BoulusAJ 16:ff375f62a95f 360 Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
BoulusAJ 16:ff375f62a95f 361 // PI Controller
BoulusAJ 16:ff375f62a95f 362 PID_Input = Desired_input - Velocity;
BoulusAJ 16:ff375f62a95f 363 PID_Output = C1.update(PID_Input);
BoulusAJ 16:ff375f62a95f 364
BoulusAJ 16:ff375f62a95f 365 // System input
BoulusAJ 23:b7b7271deb11 366 Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
BoulusAJ 23:b7b7271deb11 367 //Sys_input_Amps = 0.0f - Loop1_output - Loop2_output;
BoulusAJ 24:c953b74ed88b 368 //Sys_input_Amps = 13.0f;
BoulusAJ 21:762f8769cba4 369
BoulusAJ 21:762f8769cba4 370
BoulusAJ 21:762f8769cba4 371 //if(++counter < 30) {
BoulusAJ 21:762f8769cba4 372 // Sys_input_Amps = 12.5;
BoulusAJ 21:762f8769cba4 373 //} else {
BoulusAJ 21:762f8769cba4 374 // Sys_input_Amps = 0.0f - Loop1_output - Loop2_output;
BoulusAJ 21:762f8769cba4 375 // }
BoulusAJ 16:ff375f62a95f 376
BoulusAJ 16:ff375f62a95f 377
BoulusAJ 16:ff375f62a95f 378 // Do Not Modify - This is implemented to prevent the Cube from continuously speeding up while being still for after falling due to the user interrupting its movement
BoulusAJ 23:b7b7271deb11 379 if ( abs(Velocity) > 300.0f && (abs(Cuboid_Angle_Degrees)<50.0f && abs(Cuboid_Angle_Degrees)>40.0f) ) {
BoulusAJ 16:ff375f62a95f 380 Button_Status = 4;
BoulusAJ 16:ff375f62a95f 381 break;
BoulusAJ 16:ff375f62a95f 382 }
BoulusAJ 16:ff375f62a95f 383 break;
BoulusAJ 16:ff375f62a95f 384
BoulusAJ 16:ff375f62a95f 385 case 3: // Fall
BoulusAJ 21:762f8769cba4 386
BoulusAJ 17:04eebb7c29b5 387 Sys_input_Amps = 0.0;
BoulusAJ 16:ff375f62a95f 388 // Not implemented Yet :)
BoulusAJ 21:762f8769cba4 389 counter = 0;
BoulusAJ 16:ff375f62a95f 390 break;
BoulusAJ 17:04eebb7c29b5 391
BoulusAJ 16:ff375f62a95f 392 case 4: // Lift up when stuck
BoulusAJ 17:04eebb7c29b5 393 // Do Not Modify - This is implemented to prevent the Cube from continuously speeding up while being still for after falling due to the user interrupting its movement
BoulusAJ 16:ff375f62a95f 394
BoulusAJ 21:762f8769cba4 395 counter = 0;
BoulusAJ 20:b142ae11a12a 396 C1.reset(0.0f);
BoulusAJ 20:b142ae11a12a 397
BoulusAJ 16:ff375f62a95f 398 PID_Input2 = 0.0f - Velocity;
BoulusAJ 16:ff375f62a95f 399 PID_Output2 = C3.update(-1*PID_Input2);
BoulusAJ 16:ff375f62a95f 400
BoulusAJ 16:ff375f62a95f 401 // System input
BoulusAJ 16:ff375f62a95f 402 Sys_input_Amps = PID_Output2;
BoulusAJ 16:ff375f62a95f 403 if (Sys_input_Amps > 1.0f) {
BoulusAJ 16:ff375f62a95f 404 Sys_input_Amps = 1.0f;
BoulusAJ 16:ff375f62a95f 405 }
BoulusAJ 16:ff375f62a95f 406 if (Sys_input_Amps < -1.0f) {
BoulusAJ 16:ff375f62a95f 407 Sys_input_Amps = -1.0f;
BoulusAJ 16:ff375f62a95f 408 }
BoulusAJ 16:ff375f62a95f 409 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 16:ff375f62a95f 410 //pc.printf("%0.7f, %0.7f, \n\r", abs(Cuboid_Angle_Degrees), abs(Velocity));
BoulusAJ 16:ff375f62a95f 411
BoulusAJ 23:b7b7271deb11 412 if (abs(Velocity) < 4.0f) {
BoulusAJ 17:04eebb7c29b5 413 Sys_input_Amps = 0.0;
BoulusAJ 16:ff375f62a95f 414 Button_Status = 2;
BoulusAJ 16:ff375f62a95f 415 break;
BoulusAJ 16:ff375f62a95f 416 }
BoulusAJ 16:ff375f62a95f 417 break;
BoulusAJ 16:ff375f62a95f 418 }
BoulusAJ 16:ff375f62a95f 419 // End of Switch Statement
BoulusAJ 17:04eebb7c29b5 420
BoulusAJ 16:ff375f62a95f 421 if (Sys_input_Amps > uMax) {
BoulusAJ 16:ff375f62a95f 422 Sys_input_Amps = uMax;
BoulusAJ 16:ff375f62a95f 423 }
BoulusAJ 16:ff375f62a95f 424 if (Sys_input_Amps < uMin) {
BoulusAJ 16:ff375f62a95f 425 Sys_input_Amps = uMin;
BoulusAJ 16:ff375f62a95f 426 }
BoulusAJ 18:3b1aa67e11ca 427 if (Cuboid_Angle_Degrees > -50.0f && Cuboid_Angle_Degrees < 50.0f) {
BoulusAJ 21:762f8769cba4 428 // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
BoulusAJ 21:762f8769cba4 429 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 21:762f8769cba4 430 } else {
BoulusAJ 18:3b1aa67e11ca 431 Sys_input_Amps = 0.0f;
BoulusAJ 18:3b1aa67e11ca 432 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 21:762f8769cba4 433 }
BoulusAJ 16:ff375f62a95f 434
BoulusAJ 14:fe003a27018d 435 // ----------------
BoulusAJ 12:ba4ce3fa4f53 436
BoulusAJ 16:ff375f62a95f 437 // Print Data // Printing Rate (in this case) is every Ts*100 = 0.007*100 = every 0.7 seconds
BoulusAJ 24:c953b74ed88b 438 //if(++k >= 100) {
BoulusAJ 24:c953b74ed88b 439 // k = 0;
BoulusAJ 24:c953b74ed88b 440 // pc.printf("Some Outputs: %0.6f, %0.6f, %0.6f, %i\n\r", Sys_input_Amps, Cuboid_Angle_Degrees, PID_Output, Button_Status);
BoulusAJ 24:c953b74ed88b 441 // }
BoulusAJ 12:ba4ce3fa4f53 442
BoulusAJ 24:c953b74ed88b 443 pc.printf("%0.6f, %0.6f, %0.6f, %0.6f, %i\n\r", Sys_input_Amps, Cuboid_Angle_Degrees, PID_Output, Velocity, Button_Status);
BoulusAJ 1:d3406369c297 444 }
BoulusAJ 14:fe003a27018d 445 //******************************************************************************
BoulusAJ 14:fe003a27018d 446 //------------------ User functions like buttens handle etc. -------------------
BoulusAJ 14:fe003a27018d 447 //******************************************************************************
BoulusAJ 0:8e87cdf07037 448
BoulusAJ 14:fe003a27018d 449 //...
BoulusAJ 16:ff375f62a95f 450 // start timer as soon as Button is pressed
BoulusAJ 16:ff375f62a95f 451 void pressed()
BoulusAJ 16:ff375f62a95f 452 {
BoulusAJ 16:ff375f62a95f 453 T_Button.start();
BoulusAJ 16:ff375f62a95f 454 }
BoulusAJ 16:ff375f62a95f 455 // Falling edge of button: enable/disable controller
BoulusAJ 16:ff375f62a95f 456 void released()
BoulusAJ 16:ff375f62a95f 457 {
BoulusAJ 16:ff375f62a95f 458 // readout, stop and reset timer
BoulusAJ 16:ff375f62a95f 459 float ButtonTime = T_Button.read();
BoulusAJ 16:ff375f62a95f 460 T_Button.stop();
BoulusAJ 16:ff375f62a95f 461 T_Button.reset();
BoulusAJ 16:ff375f62a95f 462 if(ButtonTime > 0.05f) {
BoulusAJ 16:ff375f62a95f 463 Button_Status = Button_Status + 1;
BoulusAJ 20:b142ae11a12a 464 C1.reset(0.0f);
BoulusAJ 16:ff375f62a95f 465 if (Button_Status > 3.0f) {
BoulusAJ 16:ff375f62a95f 466 Button_Status = 1.0;
BoulusAJ 16:ff375f62a95f 467 }
BoulusAJ 17:04eebb7c29b5 468 //pc.printf("Button Status: %i\r\n", Button_Status);
BoulusAJ 16:ff375f62a95f 469 }
BoulusAJ 16:ff375f62a95f 470 }