Boulus Abu Joudom / Mbed 2 deprecated Cube_Mini_Solution

Cube Mini Solution

Dependencies:   mbed QEI MPU6050 BLE_API nRF51822 MCP4725 eMPL_MPU6050

main.cpp@16:ff375f62a95f, 2020-05-29 (annotated)

Committer:
BoulusAJ
Date:
Fri May 29 12:16:32 2020 +0000
Revision:
16:ff375f62a95f
Parent:
15:3395ad948f44
Child:
17:04eebb7c29b5
First Version after moving to the new template

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 13:41edfb8e3b3c 13 Settings for Maxon ESCON controller (upload via ESCON Studio)
BoulusAJ 13:41edfb8e3b3c 14 Escon Studio file Location: ...
BoulusAJ 16:ff375f62a95f 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 16:ff375f62a95f 30
BoulusAJ 16:ff375f62a95f 31 Escon Mapping of voltage to current and rotational velocity to voltage
BoulusAJ 16:ff375f62a95f 32 Escon Linear Mapping of input current from voltage is 0V is -15A and 5V is 15A, hence 2.5V is 0A !!!
BoulusAJ 16:ff375f62a95f 33 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 34
BoulusAJ 13:41edfb8e3b3c 35 Notes
BoulusAJ 13:41edfb8e3b3c 36 The Maximum output Current allowed is 13A and the Minimum is -13A !!!
BoulusAJ 16:ff375f62a95f 37 Escon Linear Mapping of input current from voltage is 0V is -15A and 5V is 15A, hence 2.5V is 0A !!!
BoulusAJ 13:41edfb8e3b3c 38 All needed Libraries for the IMU (Accelerometer and Gyroscope), DAC are included.
BoulusAJ 13:41edfb8e3b3c 39 The PID is to be written by students. Use PID_Cntrl.cpp and PID_Cntrl.h as templates
BoulusAJ 13:41edfb8e3b3c 40 ...
BoulusAJ 16:ff375f62a95f 41
BoulusAJ 13:41edfb8e3b3c 42
BoulusAJ 0:8e87cdf07037 43 */
BoulusAJ 0:8e87cdf07037 44 // Libraries
BoulusAJ 13:41edfb8e3b3c 45 #include <stdbool.h>
BoulusAJ 0:8e87cdf07037 46 #include "mbed.h"
BoulusAJ 13:41edfb8e3b3c 47 #include "MPU6050.h" // IMU Library
BoulusAJ 13:41edfb8e3b3c 48 #include "mcp4725.h" // DAC Library
BoulusAJ 13:41edfb8e3b3c 49 #include "IIR_filter.h" // Filter Library (Please go to the Filter Library and add in your Code)
BoulusAJ 13:41edfb8e3b3c 50 #include "LinearCharacteristics.h" // Linear Scaling Library
BoulusAJ 13:41edfb8e3b3c 51 #include "PID_Cntrl.h" // PID Library (Please go to PID Library and add in your PID Code)
BoulusAJ 13:41edfb8e3b3c 52
BoulusAJ 0:8e87cdf07037 53
BoulusAJ 0:8e87cdf07037 54 // Serial PC Communication
BoulusAJ 0:8e87cdf07037 55 #define UART_TX p9
BoulusAJ 0:8e87cdf07037 56 #define UART_RX p11
BoulusAJ 0:8e87cdf07037 57
BoulusAJ 0:8e87cdf07037 58 // IMU SDA and SCL
BoulusAJ 0:8e87cdf07037 59 #define MPU6050_SDA p12
BoulusAJ 0:8e87cdf07037 60 #define MPU6050_SCL p13
BoulusAJ 0:8e87cdf07037 61
BoulusAJ 12:ba4ce3fa4f53 62 // PI Values
BoulusAJ 0:8e87cdf07037 63 #define PI 3.1415927f
BoulusAJ 0:8e87cdf07037 64 #define pi 3.1415927f
BoulusAJ 0:8e87cdf07037 65
BoulusAJ 13:41edfb8e3b3c 66 // PC Serial Connection
BoulusAJ 13:41edfb8e3b3c 67 Serial pc(UART_TX, UART_RX); // serial connection via USB - programmer
BoulusAJ 0:8e87cdf07037 68
BoulusAJ 12:ba4ce3fa4f53 69 // -------------------------------
BoulusAJ 12:ba4ce3fa4f53 70 // Analog/Digitsl I/O Definitions
BoulusAJ 12:ba4ce3fa4f53 71 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 72 AnalogIn Velocity_Voltage_Input(p5); // Velocity Input as Analogue Signal from Escon
BoulusAJ 13:41edfb8e3b3c 73 DigitalOut Pin_3V3(p30); // Defining P30 as 3V3 Pin for internal use. Please do not modify.
BoulusAJ 14:fe003a27018d 74 InterruptIn Button(p6); // User Button Interrput
BoulusAJ 12:ba4ce3fa4f53 75
BoulusAJ 13:41edfb8e3b3c 76 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 77 // Initialization of Values and Variables
BoulusAJ 13:41edfb8e3b3c 78 // -------------------------------
BoulusAJ 0:8e87cdf07037 79
BoulusAJ 0:8e87cdf07037 80 // Sample time of main loops
BoulusAJ 16:ff375f62a95f 81 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 82
BoulusAJ 0:8e87cdf07037 83 // MPU 6050 Variables - Acceleration and Gyroscope Raw and Converted Data Variables
BoulusAJ 0:8e87cdf07037 84 int16_t AccX_Raw, AccY_Raw, AccZ_Raw;
BoulusAJ 0:8e87cdf07037 85 int16_t GyroX_Raw, GyroY_Raw, GyroZ_Raw;
BoulusAJ 0:8e87cdf07037 86 double AccX_g, AccY_g, AccZ_g;
BoulusAJ 0:8e87cdf07037 87 double GyroX_Degrees, GyroY_Degrees, GyroZ_Degrees, GyroZ_RadiansPerSecond;
BoulusAJ 0:8e87cdf07037 88
BoulusAJ 0:8e87cdf07037 89 // printf Variable
BoulusAJ 0:8e87cdf07037 90 int k = 0;
BoulusAJ 13:41edfb8e3b3c 91
BoulusAJ 13:41edfb8e3b3c 92
BoulusAJ 13:41edfb8e3b3c 93 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 94 // Variables: Here define variables such as gains etc.
BoulusAJ 13:41edfb8e3b3c 95 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 96
BoulusAJ 13:41edfb8e3b3c 97 // Accelerometer and Gyroscope
BoulusAJ 13:41edfb8e3b3c 98
BoulusAJ 13:41edfb8e3b3c 99
BoulusAJ 13:41edfb8e3b3c 100 // Cube Angle and Speed Variables
BoulusAJ 16:ff375f62a95f 101 double Cuboid_Angle_Radians, Cuboid_Angle_Degrees;
BoulusAJ 16:ff375f62a95f 102 double Cuboid_Angle_Speed_Degrees = 0.0;
BoulusAJ 13:41edfb8e3b3c 103
BoulusAJ 13:41edfb8e3b3c 104 // Low pass filter variables
BoulusAJ 16:ff375f62a95f 105 float t = 0.5f;
BoulusAJ 0:8e87cdf07037 106
BoulusAJ 0:8e87cdf07037 107 // Flywheel Position and Velocity variables
BoulusAJ 16:ff375f62a95f 108 double Velocity_Input_Voltage = 0.0f;
BoulusAJ 16:ff375f62a95f 109 double Velocity, Velocity_Voltage, Velocity_rpm, Velocity_Voltage_Read;
BoulusAJ 13:41edfb8e3b3c 110
BoulusAJ 16:ff375f62a95f 111 // User Button
BoulusAJ 16:ff375f62a95f 112 int Button_Status = 0; // User Button Status
BoulusAJ 16:ff375f62a95f 113 Timer T_Button; // define timer for button
BoulusAJ 0:8e87cdf07037 114
BoulusAJ 13:41edfb8e3b3c 115 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 116 // Controller Variables: Such as SS Controller Values and Saturation Limits
BoulusAJ 13:41edfb8e3b3c 117 // -------------------------------
BoulusAJ 16:ff375f62a95f 118 // Variables concerning the Controller Design is in reference to the Matlab and Simulink Files ..............
BoulusAJ 16:ff375f62a95f 119
BoulusAJ 16:ff375f62a95f 120 // User Input
BoulusAJ 16:ff375f62a95f 121 float Desired_input = 0.0f;
BoulusAJ 16:ff375f62a95f 122 // Initial System input in Amperes
BoulusAJ 16:ff375f62a95f 123 float Sys_input_Amps = 0.0f;
BoulusAJ 0:8e87cdf07037 124
BoulusAJ 0:8e87cdf07037 125 // Sate Space Controller Values
BoulusAJ 16:ff375f62a95f 126 float K_SS_Controller [2] = {-57.1176*0.3, -2.6398*1.5}; // From Matlab
BoulusAJ 0:8e87cdf07037 127
BoulusAJ 0:8e87cdf07037 128 // Controller Variables
BoulusAJ 16:ff375f62a95f 129 float Loop1_output; // Loop 1 controller output
BoulusAJ 16:ff375f62a95f 130 float Loop2_output; // Loop 2 controller output
BoulusAJ 16:ff375f62a95f 131 float PID_Input, PID_Output, PID_Input2, PID_Output2;
BoulusAJ 13:41edfb8e3b3c 132
BoulusAJ 13:41edfb8e3b3c 133 // PID (PI Parameters)
BoulusAJ 13:41edfb8e3b3c 134
BoulusAJ 16:ff375f62a95f 135 // PID 1 - Velocity control after lift-up
BoulusAJ 16:ff375f62a95f 136 float Kp_1 = -0.09;
BoulusAJ 16:ff375f62a95f 137 float Ki_1 = -0.09*0.5*0.5;
BoulusAJ 16:ff375f62a95f 138 float Kd_1 = 0; // No D-Part
BoulusAJ 16:ff375f62a95f 139 float Tf_1 = 1; // No D-Part
BoulusAJ 16:ff375f62a95f 140 // Controller Loop (PI-Part) in Case 2 (breaking case)
BoulusAJ 16:ff375f62a95f 141 float Kp_2 = 0.25;
BoulusAJ 16:ff375f62a95f 142 float Ki_2 = 0.25;
BoulusAJ 16:ff375f62a95f 143 float Kd_2 = 0; // No D-Part
BoulusAJ 16:ff375f62a95f 144 float Tf_2 = 1; // No D-Part
BoulusAJ 0:8e87cdf07037 145
BoulusAJ 0:8e87cdf07037 146 // Saturation Parameters
BoulusAJ 16:ff375f62a95f 147 // PI Controller Limits
BoulusAJ 16:ff375f62a95f 148 const float uMin1 = -13.0f;
BoulusAJ 16:ff375f62a95f 149 const float uMax1= 13.0f;
BoulusAJ 0:8e87cdf07037 150
BoulusAJ 13:41edfb8e3b3c 151 // Cuboid Escon Input Limits in Amps
BoulusAJ 13:41edfb8e3b3c 152 const float uMin = -13.0f; // Minimum Current Allowed
BoulusAJ 13:41edfb8e3b3c 153 const float uMax = 13.0f; // Maximum Current Allowe
BoulusAJ 0:8e87cdf07037 154
BoulusAJ 0:8e87cdf07037 155
BoulusAJ 13:41edfb8e3b3c 156 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 157 // User-Defined Functions
BoulusAJ 13:41edfb8e3b3c 158 // -------------------------------
BoulusAJ 0:8e87cdf07037 159
BoulusAJ 16:ff375f62a95f 160 // USer Button
BoulusAJ 16:ff375f62a95f 161 void pressed(void); // user Button pressed
BoulusAJ 16:ff375f62a95f 162 void released(void); // user Button released
BoulusAJ 16:ff375f62a95f 163
BoulusAJ 16:ff375f62a95f 164 // Controller loop (via interrupt)
BoulusAJ 16:ff375f62a95f 165 void updateControllers(void); // controller loop (via interrupt)
BoulusAJ 16:ff375f62a95f 166
BoulusAJ 16:ff375f62a95f 167 // Linear Scaler - Follow the mapping mentioned in the Notes above
BoulusAJ 16:ff375f62a95f 168 // Linear Scaler
BoulusAJ 16:ff375f62a95f 169 LinearCharacteristics CurrentToVoltage(-15.0f, 15.0f, 0.0f, 5.0f);
BoulusAJ 16:ff375f62a95f 170 LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -4000.0f, 4000.0f);
BoulusAJ 16:ff375f62a95f 171
BoulusAJ 16:ff375f62a95f 172 // PID (PI) Controller (My PID Controller is fine but needs clarity updates)
BoulusAJ 16:ff375f62a95f 173 PID_Cntrl C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1); // Defining the 1st Loop Controller (PI-Part)
BoulusAJ 16:ff375f62a95f 174 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 16:ff375f62a95f 175 PID_Cntrl C3(Kp_1*2,Ki_1,Kd_2,Tf_2,Ts,uMin1,uMax1); // Safety Implementation in Case 4 PID
BoulusAJ 16:ff375f62a95f 176
BoulusAJ 16:ff375f62a95f 177 // Accelerometer and Gyroscope Filters
BoulusAJ 16:ff375f62a95f 178 IIR_filter FilterAccX(t, Ts, 1.0f);
BoulusAJ 16:ff375f62a95f 179 IIR_filter FilterAccY(t, Ts, 1.0f);
BoulusAJ 16:ff375f62a95f 180 IIR_filter FilterGyro(t, Ts, t);
BoulusAJ 16:ff375f62a95f 181
BoulusAJ 0:8e87cdf07037 182 // Interrupts
BoulusAJ 0:8e87cdf07037 183 Ticker ControllerLoopTimer; // Interrupt for control loop
BoulusAJ 0:8e87cdf07037 184
BoulusAJ 13:41edfb8e3b3c 185
BoulusAJ 13:41edfb8e3b3c 186 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 187 // Functions
BoulusAJ 13:41edfb8e3b3c 188 // -------------------------------
BoulusAJ 13:41edfb8e3b3c 189
BoulusAJ 13:41edfb8e3b3c 190 // DAC - For Current output
BoulusAJ 0:8e87cdf07037 191 MCP4725 VoltageOut(p3, p4, MCP4725::Fast400kHz, 0);
BoulusAJ 0:8e87cdf07037 192
BoulusAJ 0:8e87cdf07037 193 // IMU - MPU6050 Functions and I2C Functions
BoulusAJ 0:8e87cdf07037 194 I2C i2c(MPU6050_SDA, MPU6050_SCL);
BoulusAJ 0:8e87cdf07037 195 MPU6050 mpu(i2c);
BoulusAJ 0:8e87cdf07037 196
BoulusAJ 0:8e87cdf07037 197 // Timers
BoulusAJ 0:8e87cdf07037 198 Timer Loop;
BoulusAJ 0:8e87cdf07037 199
BoulusAJ 12:ba4ce3fa4f53 200 //******************************************************************************
BoulusAJ 14:fe003a27018d 201 //----------------------------- Main Loop --------------------------------------
BoulusAJ 12:ba4ce3fa4f53 202 //******************************************************************************
BoulusAJ 0:8e87cdf07037 203 int main()
BoulusAJ 0:8e87cdf07037 204 {
BoulusAJ 12:ba4ce3fa4f53 205 // Microcontroller initialization
BoulusAJ 2:e088fa08e244 206 Pin_3V3.write(1);
BoulusAJ 12:ba4ce3fa4f53 207 *((unsigned int *)0x40007504) = 0x400A; // Configure the ADC to the internel reference of 1.2V.
BoulusAJ 13:41edfb8e3b3c 208 // IMU - MPU6050 initialization - Do NOT Modify
BoulusAJ 12:ba4ce3fa4f53 209 mpu.initialize();
BoulusAJ 13:41edfb8e3b3c 210 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 211 mpu.setFullScaleGyroRange(2u); // Change the scale of the Gyroscope to +/- 1000 degrees/sec
BoulusAJ 12:ba4ce3fa4f53 212 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // Change the scale of the Accelerometer to +/- 4g - Sensitivity: 4096 LSB/mg
BoulusAJ 16:ff375f62a95f 213
BoulusAJ 13:41edfb8e3b3c 214 // Serial Communication
BoulusAJ 13:41edfb8e3b3c 215 pc.baud(115200);
BoulusAJ 16:ff375f62a95f 216
BoulusAJ 13:41edfb8e3b3c 217 VoltageOut.write(2.5); // Output Zero Current to the Motor
BoulusAJ 0:8e87cdf07037 218
BoulusAJ 13:41edfb8e3b3c 219 // Reset Filters and PID Controller
BoulusAJ 16:ff375f62a95f 220 FilterAccX.reset(0.0f);
BoulusAJ 16:ff375f62a95f 221 FilterAccY.reset(0.0f);
BoulusAJ 16:ff375f62a95f 222 FilterGyro.reset(0.0f);
BoulusAJ 16:ff375f62a95f 223 C1.reset(0.0f);
BoulusAJ 16:ff375f62a95f 224 C2.reset(0.0f);
BoulusAJ 16:ff375f62a95f 225 C3.reset(0.0f);
BoulusAJ 11:af811e6f9a04 226
BoulusAJ 16:ff375f62a95f 227 pc.printf("Hello World!\n\r");
BoulusAJ 16:ff375f62a95f 228 wait(5);
BoulusAJ 12:ba4ce3fa4f53 229 // Control Loop Interrupt at Ts, 200Hz Rate
BoulusAJ 7:07c5b6709d87 230 ControllerLoopTimer.attach(&updateControllers, Ts);
BoulusAJ 16:ff375f62a95f 231 Button.fall(&pressed); // attach key pressed function
BoulusAJ 16:ff375f62a95f 232 Button.rise(&released); // attach key pressed function
BoulusAJ 16:ff375f62a95f 233
BoulusAJ 7:07c5b6709d87 234
BoulusAJ 7:07c5b6709d87 235 }
BoulusAJ 12:ba4ce3fa4f53 236 //******************************************************************************
BoulusAJ 14:fe003a27018d 237 //------------------ Control Loop (called via interrupt) -----------------------
BoulusAJ 12:ba4ce3fa4f53 238 //******************************************************************************
BoulusAJ 7:07c5b6709d87 239 void updateControllers(void)
BoulusAJ 7:07c5b6709d87 240 {
BoulusAJ 16:ff375f62a95f 241
BoulusAJ 13:41edfb8e3b3c 242 // Acquire Velocity
BoulusAJ 13:41edfb8e3b3c 243 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 244 // Velocity = .... Refer to mapping
BoulusAJ 16:ff375f62a95f 245 Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
BoulusAJ 16:ff375f62a95f 246 Velocity = Velocity_rpm*2.0*pi/60.0;
BoulusAJ 13:41edfb8e3b3c 247
BoulusAJ 8:71babe904b92 248
BoulusAJ 12:ba4ce3fa4f53 249 // Aquire Raw Acceleration and Gyro Data form the IMU
BoulusAJ 12:ba4ce3fa4f53 250 mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);
BoulusAJ 0:8e87cdf07037 251
BoulusAJ 7:07c5b6709d87 252 // -------------- Convert Raw data to SI Units --------------------
BoulusAJ 7:07c5b6709d87 253
BoulusAJ 7:07c5b6709d87 254 //Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
BoulusAJ 16:ff375f62a95f 255 AccX_g = AccX_Raw / 8192.0f;
BoulusAJ 16:ff375f62a95f 256 AccY_g = AccY_Raw / 8192.0f;
BoulusAJ 16:ff375f62a95f 257 AccZ_g = AccZ_Raw / 8192.0f;
BoulusAJ 0:8e87cdf07037 258
BoulusAJ 13:41edfb8e3b3c 259 //Convert Gyroscope Raw Data to Degrees per second - (2^15/1000 = 32.768)
BoulusAJ 16:ff375f62a95f 260 GyroX_Degrees = GyroX_Raw / 32.768f;
BoulusAJ 16:ff375f62a95f 261 GyroY_Degrees = GyroY_Raw / 32.768f;
BoulusAJ 16:ff375f62a95f 262 GyroZ_Degrees = GyroZ_Raw / 32.768f;
BoulusAJ 0:8e87cdf07037 263
BoulusAJ 7:07c5b6709d87 264 //Convert Gyroscope Raw Data to Degrees per second
BoulusAJ 7:07c5b6709d87 265 GyroZ_RadiansPerSecond = (GyroZ_Raw / 32.768f)* pi/180.0f;
BoulusAJ 0:8e87cdf07037 266
BoulusAJ 7:07c5b6709d87 267 // ----- Combine Accelerometer Data and Gyro Data to Get Angle ------
BoulusAJ 16:ff375f62a95f 268 Cuboid_Angle_Radians = -1*atan2(-FilterAccX(AccX_g), FilterAccY(AccY_g)) + 0.7854f + FilterGyro(GyroZ_RadiansPerSecond);
BoulusAJ 16:ff375f62a95f 269 Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;
BoulusAJ 0:8e87cdf07037 270
BoulusAJ 0:8e87cdf07037 271
BoulusAJ 7:07c5b6709d87 272 // ------------------------- Controller -----------------------------
BoulusAJ 10:4e9899cef12c 273
BoulusAJ 14:fe003a27018d 274 // Switch Statement Maybe?......
BoulusAJ 16:ff375f62a95f 275 switch(Button_Status) {
BoulusAJ 16:ff375f62a95f 276
BoulusAJ 16:ff375f62a95f 277 case 0: // Output of 0 Amps
BoulusAJ 16:ff375f62a95f 278
BoulusAJ 16:ff375f62a95f 279 VoltageOut.write(CurrentToVoltage(0));
BoulusAJ 16:ff375f62a95f 280 break;
BoulusAJ 16:ff375f62a95f 281
BoulusAJ 16:ff375f62a95f 282 case 1: // Breaking
BoulusAJ 16:ff375f62a95f 283
BoulusAJ 16:ff375f62a95f 284 // PI Controller
BoulusAJ 16:ff375f62a95f 285 PID_Input = 0.0f - Velocity;
BoulusAJ 16:ff375f62a95f 286 PID_Output = C2.update(PID_Input);
BoulusAJ 16:ff375f62a95f 287
BoulusAJ 16:ff375f62a95f 288 // System input
BoulusAJ 16:ff375f62a95f 289 Sys_input_Amps = PID_Output;
BoulusAJ 16:ff375f62a95f 290 // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
BoulusAJ 16:ff375f62a95f 291 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 16:ff375f62a95f 292 break;
BoulusAJ 16:ff375f62a95f 293
BoulusAJ 16:ff375f62a95f 294 case 2: // Balancing and lift-up
BoulusAJ 16:ff375f62a95f 295
BoulusAJ 16:ff375f62a95f 296 // Current Input Updater - In Amps
BoulusAJ 16:ff375f62a95f 297 // Loop 1
BoulusAJ 16:ff375f62a95f 298 Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
BoulusAJ 16:ff375f62a95f 299 // Loop 2
BoulusAJ 16:ff375f62a95f 300 Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
BoulusAJ 16:ff375f62a95f 301 // PI Controller
BoulusAJ 16:ff375f62a95f 302 PID_Input = Desired_input - Velocity;
BoulusAJ 16:ff375f62a95f 303 PID_Output = C1.update(PID_Input);
BoulusAJ 16:ff375f62a95f 304
BoulusAJ 16:ff375f62a95f 305 // System input
BoulusAJ 16:ff375f62a95f 306 Sys_input_Amps = PID_Output - Loop1_output - Loop2_output;
BoulusAJ 16:ff375f62a95f 307 // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
BoulusAJ 16:ff375f62a95f 308 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 16:ff375f62a95f 309
BoulusAJ 16:ff375f62a95f 310
BoulusAJ 16:ff375f62a95f 311 // 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 312 if ( abs(Velocity) > 250.0f && (abs(Cuboid_Angle_Degrees)<50.0f && abs(Cuboid_Angle_Degrees)>40.0f) ) {
BoulusAJ 16:ff375f62a95f 313 Button_Status = 4;
BoulusAJ 16:ff375f62a95f 314 break;
BoulusAJ 16:ff375f62a95f 315 }
BoulusAJ 16:ff375f62a95f 316 break;
BoulusAJ 16:ff375f62a95f 317
BoulusAJ 16:ff375f62a95f 318 case 3: // Fall
BoulusAJ 16:ff375f62a95f 319
BoulusAJ 16:ff375f62a95f 320 VoltageOut.write(CurrentToVoltage(0.0f));
BoulusAJ 16:ff375f62a95f 321 // Not implemented Yet :)
BoulusAJ 16:ff375f62a95f 322 break;
BoulusAJ 16:ff375f62a95f 323
BoulusAJ 16:ff375f62a95f 324 case 4: // Lift up when stuck
BoulusAJ 16:ff375f62a95f 325 // 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 326
BoulusAJ 16:ff375f62a95f 327 PID_Input2 = 0.0f - Velocity;
BoulusAJ 16:ff375f62a95f 328 PID_Output2 = C3.update(-1*PID_Input2);
BoulusAJ 16:ff375f62a95f 329
BoulusAJ 16:ff375f62a95f 330 // System input
BoulusAJ 16:ff375f62a95f 331 Sys_input_Amps = PID_Output2;
BoulusAJ 16:ff375f62a95f 332 if (Sys_input_Amps > 1.0f) {
BoulusAJ 16:ff375f62a95f 333 Sys_input_Amps = 1.0f;
BoulusAJ 16:ff375f62a95f 334 }
BoulusAJ 16:ff375f62a95f 335 if (Sys_input_Amps < -1.0f) {
BoulusAJ 16:ff375f62a95f 336 Sys_input_Amps = -1.0f;
BoulusAJ 16:ff375f62a95f 337 }
BoulusAJ 16:ff375f62a95f 338 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 16:ff375f62a95f 339 //pc.printf("%0.7f, %0.7f, \n\r", abs(Cuboid_Angle_Degrees), abs(Velocity));
BoulusAJ 16:ff375f62a95f 340
BoulusAJ 16:ff375f62a95f 341 if (abs(Velocity) < 20.0f) {
BoulusAJ 16:ff375f62a95f 342
BoulusAJ 16:ff375f62a95f 343 Button_Status = 2;
BoulusAJ 16:ff375f62a95f 344 break;
BoulusAJ 16:ff375f62a95f 345 }
BoulusAJ 16:ff375f62a95f 346 break;
BoulusAJ 16:ff375f62a95f 347 }
BoulusAJ 16:ff375f62a95f 348 // End of Switch Statement
BoulusAJ 14:fe003a27018d 349
BoulusAJ 16:ff375f62a95f 350 if (Sys_input_Amps > uMax) {
BoulusAJ 16:ff375f62a95f 351 Sys_input_Amps = uMax;
BoulusAJ 16:ff375f62a95f 352 }
BoulusAJ 16:ff375f62a95f 353 if (Sys_input_Amps < uMin) {
BoulusAJ 16:ff375f62a95f 354 Sys_input_Amps = uMin;
BoulusAJ 16:ff375f62a95f 355 }
BoulusAJ 16:ff375f62a95f 356
BoulusAJ 14:fe003a27018d 357 // ----------------
BoulusAJ 12:ba4ce3fa4f53 358
BoulusAJ 16:ff375f62a95f 359 // Print Data // Printing Rate (in this case) is every Ts*100 = 0.007*100 = every 0.7 seconds
BoulusAJ 13:41edfb8e3b3c 360 if(++k >= 100) {
BoulusAJ 12:ba4ce3fa4f53 361 k = 0;
BoulusAJ 12:ba4ce3fa4f53 362 pc.printf("Some Outputs: %0.6f, %0.6f\n\r", Sys_input_Amps, Velocity);
BoulusAJ 12:ba4ce3fa4f53 363 }
BoulusAJ 12:ba4ce3fa4f53 364
BoulusAJ 13:41edfb8e3b3c 365
BoulusAJ 1:d3406369c297 366 }
BoulusAJ 14:fe003a27018d 367 //******************************************************************************
BoulusAJ 14:fe003a27018d 368 //------------------ User functions like buttens handle etc. -------------------
BoulusAJ 14:fe003a27018d 369 //******************************************************************************
BoulusAJ 0:8e87cdf07037 370
BoulusAJ 14:fe003a27018d 371 //...
BoulusAJ 16:ff375f62a95f 372 // start timer as soon as Button is pressed
BoulusAJ 16:ff375f62a95f 373 void pressed()
BoulusAJ 16:ff375f62a95f 374 {
BoulusAJ 16:ff375f62a95f 375 T_Button.start();
BoulusAJ 16:ff375f62a95f 376 }
BoulusAJ 16:ff375f62a95f 377 // Falling edge of button: enable/disable controller
BoulusAJ 16:ff375f62a95f 378 void released()
BoulusAJ 16:ff375f62a95f 379 {
BoulusAJ 16:ff375f62a95f 380 // readout, stop and reset timer
BoulusAJ 16:ff375f62a95f 381 float ButtonTime = T_Button.read();
BoulusAJ 16:ff375f62a95f 382 T_Button.stop();
BoulusAJ 16:ff375f62a95f 383 T_Button.reset();
BoulusAJ 16:ff375f62a95f 384 if(ButtonTime > 0.05f) {
BoulusAJ 16:ff375f62a95f 385 Button_Status = Button_Status + 1;
BoulusAJ 16:ff375f62a95f 386 if (Button_Status > 3.0f) {
BoulusAJ 16:ff375f62a95f 387 Button_Status = 1.0;
BoulusAJ 16:ff375f62a95f 388 }
BoulusAJ 16:ff375f62a95f 389 pc.printf("Button Status: %i\r\n", Button_Status);
BoulusAJ 16:ff375f62a95f 390 }
BoulusAJ 16:ff375f62a95f 391 }