Boulus Abu Joudom / Mbed 2 deprecated Cube_Mini_Solution

Cube Mini Solution

Dependencies:   mbed QEI MPU6050 BLE_API nRF51822 MCP4725 eMPL_MPU6050

main.cpp@11:af811e6f9a04, 2020-05-19 (annotated)

Committer:
BoulusAJ
Date:
Tue May 19 17:50:52 2020 +0000
Revision:
11:af811e6f9a04
Parent:
10:4e9899cef12c
Child:
12:ba4ce3fa4f53
Fixed ADC config. More accurate velocity reading now.; May 19, 2020 - 19:50

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BoulusAJ 0:8e87cdf07037 1 /*
BoulusAJ 0:8e87cdf07037 2 Boulus Abu Joudom
BoulusAJ 3:1a714eccfe9f 3 Version: May 5, 2020. 17:00
BoulusAJ 0:8e87cdf07037 4 ZHAW - IMS
BoulusAJ 0:8e87cdf07037 5
BoulusAJ 0:8e87cdf07037 6 Cubiod Balancing Experiment - Control Lab
BoulusAJ 0:8e87cdf07037 7 Seeed Tiny Microcontroller on Cuboid 1.0
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 0:8e87cdf07037 28 // PI Value
BoulusAJ 0:8e87cdf07037 29 #define PI 3.1415927f
BoulusAJ 0:8e87cdf07037 30 #define pi 3.1415927f
BoulusAJ 0:8e87cdf07037 31
BoulusAJ 0:8e87cdf07037 32 // Analog/Digitsl I/O Definitions
BoulusAJ 0:8e87cdf07037 33 AnalogIn Velocity_Voltage_Input(p5);
BoulusAJ 2:e088fa08e244 34 DigitalOut Pin_3V3(p30);
BoulusAJ 0:8e87cdf07037 35
BoulusAJ 0:8e87cdf07037 36 // PC Serial
BoulusAJ 0:8e87cdf07037 37 Serial pc(UART_TX, UART_RX);
BoulusAJ 0:8e87cdf07037 38
BoulusAJ 0:8e87cdf07037 39 //-------- USER INPUT ( Desired Values) -------
BoulusAJ 0:8e87cdf07037 40 // User Input
BoulusAJ 0:8e87cdf07037 41 float Desired_input = 0.0f;
BoulusAJ 0:8e87cdf07037 42 // Initial System input in Amperes
BoulusAJ 0:8e87cdf07037 43 float Sys_input_Amps = 0.0f;
BoulusAJ 0:8e87cdf07037 44 //--------------------------------------------
BoulusAJ 0:8e87cdf07037 45
BoulusAJ 0:8e87cdf07037 46 // ------------- Variables -------------------
BoulusAJ 0:8e87cdf07037 47
BoulusAJ 0:8e87cdf07037 48 // Sample time of main loops
BoulusAJ 8:71babe904b92 49 float Ts = 0.005;
BoulusAJ 0:8e87cdf07037 50
BoulusAJ 0:8e87cdf07037 51 // MPU 6050 Variables - Acceleration and Gyroscope Raw and Converted Data Variables
BoulusAJ 0:8e87cdf07037 52 int16_t AccX_Raw, AccY_Raw, AccZ_Raw;
BoulusAJ 0:8e87cdf07037 53 int16_t GyroX_Raw, GyroY_Raw, GyroZ_Raw;
BoulusAJ 0:8e87cdf07037 54
BoulusAJ 0:8e87cdf07037 55 double AccX_g, AccY_g, AccZ_g;
BoulusAJ 0:8e87cdf07037 56 double GyroX_Degrees, GyroY_Degrees, GyroZ_Degrees, GyroZ_RadiansPerSecond;
BoulusAJ 0:8e87cdf07037 57
BoulusAJ 0:8e87cdf07037 58 int16_t IMU_Temperature;
BoulusAJ 0:8e87cdf07037 59 // ----------------
BoulusAJ 0:8e87cdf07037 60
BoulusAJ 0:8e87cdf07037 61 // Angle Variables
BoulusAJ 0:8e87cdf07037 62 double Cuboid_Angle_Radians, Cuboid_Angle_Degrees;
BoulusAJ 0:8e87cdf07037 63 double Cuboid_Angle_Speed_Degrees = 0.0;
BoulusAJ 0:8e87cdf07037 64
BoulusAJ 0:8e87cdf07037 65 // Low pass filter variables
BoulusAJ 0:8e87cdf07037 66 float t = 0.5f;
BoulusAJ 0:8e87cdf07037 67
BoulusAJ 0:8e87cdf07037 68 // printf Variable
BoulusAJ 0:8e87cdf07037 69 int k = 0;
BoulusAJ 0:8e87cdf07037 70 int LoopCounter = 0;
BoulusAJ 1:d3406369c297 71 float LoopTime = 0.0;
BoulusAJ 10:4e9899cef12c 72 int Case1_Counter = 0;
BoulusAJ 0:8e87cdf07037 73
BoulusAJ 0:8e87cdf07037 74 // Flywheel Position and Velocity variables
BoulusAJ 0:8e87cdf07037 75 double Velocity_Input_Voltage = 0.0f;
BoulusAJ 0:8e87cdf07037 76 double Velocity, Velocity_Voltage, Velocity_rpm, Velocity_Voltage_Read;
BoulusAJ 0:8e87cdf07037 77
BoulusAJ 0:8e87cdf07037 78 // ------------------
BoulusAJ 0:8e87cdf07037 79
BoulusAJ 0:8e87cdf07037 80 //----------------- Controller VARIABLES------------------
BoulusAJ 0:8e87cdf07037 81 // Variables concerning the Controller, the Design is in reference to the Matlab Simulink .............. and Variables are in reference to the ............. File
BoulusAJ 0:8e87cdf07037 82
BoulusAJ 0:8e87cdf07037 83 // Sate Space Controller Values
BoulusAJ 8:71babe904b92 84 float K_SS_Controller [2] = {-57.1176*0.3, -2.6398}; // From Matlab
BoulusAJ 0:8e87cdf07037 85
BoulusAJ 0:8e87cdf07037 86 // Controller Variables
BoulusAJ 0:8e87cdf07037 87 float Loop1_output; // Loop 1 controller output
BoulusAJ 0:8e87cdf07037 88 float Loop2_output; // Loop 2 controller output
BoulusAJ 0:8e87cdf07037 89 float PID_Input, PID_Output;
BoulusAJ 0:8e87cdf07037 90
BoulusAJ 0:8e87cdf07037 91 // Saturation Parameters
BoulusAJ 0:8e87cdf07037 92 // PI Controller Limits
BoulusAJ 6:122879c1503a 93 const float uMin1 = -5.0f;
BoulusAJ 6:122879c1503a 94 const float uMax1= 5.0f;
BoulusAJ 0:8e87cdf07037 95
BoulusAJ 0:8e87cdf07037 96 // Cuboid Driver Input Limits
BoulusAJ 10:4e9899cef12c 97 const float uMin2 = -13.0f;
BoulusAJ 10:4e9899cef12c 98 const float uMax2= 13.0f;
BoulusAJ 0:8e87cdf07037 99
BoulusAJ 0:8e87cdf07037 100 // Controller Loop 2 (PI-Part)
BoulusAJ 0:8e87cdf07037 101 float Kp_1 = -0.09;
BoulusAJ 0:8e87cdf07037 102 float Ki_1 = -0.09;
BoulusAJ 0:8e87cdf07037 103 float Kd_1 = 0; // No D-Part
BoulusAJ 0:8e87cdf07037 104 float Tf_1 = 1; // No D-Part
BoulusAJ 0:8e87cdf07037 105
BoulusAJ 0:8e87cdf07037 106 // Controller Loop (PI-Part) in Case 2 (breaking case)
BoulusAJ 0:8e87cdf07037 107 float Kp_2 = 4;
BoulusAJ 0:8e87cdf07037 108 float Ki_2 = 80;
BoulusAJ 0:8e87cdf07037 109 float Kd_2 = 0; // No D-Part
BoulusAJ 0:8e87cdf07037 110 float Tf_2 = 1; // No D-Part
BoulusAJ 0:8e87cdf07037 111
BoulusAJ 10:4e9899cef12c 112 // User Button
BoulusAJ 10:4e9899cef12c 113 int Button_Status = 0;
BoulusAJ 10:4e9899cef12c 114
BoulusAJ 0:8e87cdf07037 115 // ---------- Functions -------------
BoulusAJ 0:8e87cdf07037 116
BoulusAJ 0:8e87cdf07037 117 // Interrupts
BoulusAJ 0:8e87cdf07037 118 Ticker ControllerLoopTimer; // Interrupt for control loop
BoulusAJ 0:8e87cdf07037 119
BoulusAJ 0:8e87cdf07037 120 // DAC
BoulusAJ 0:8e87cdf07037 121 MCP4725 VoltageOut(p3, p4, MCP4725::Fast400kHz, 0);
BoulusAJ 0:8e87cdf07037 122
BoulusAJ 0:8e87cdf07037 123 // IMU - MPU6050 Functions and I2C Functions
BoulusAJ 0:8e87cdf07037 124 I2C i2c(MPU6050_SDA, MPU6050_SCL);
BoulusAJ 0:8e87cdf07037 125 MPU6050 mpu(i2c);
BoulusAJ 0:8e87cdf07037 126
BoulusAJ 0:8e87cdf07037 127 // Accelerometer and Gyroscope Filters
BoulusAJ 0:8e87cdf07037 128 IIR_filter FilterAccX(t, Ts, 1.0f);
BoulusAJ 0:8e87cdf07037 129 IIR_filter FilterAccY(t, Ts, 1.0f);
BoulusAJ 0:8e87cdf07037 130 IIR_filter FilterGyro(t, Ts, t);
BoulusAJ 0:8e87cdf07037 131
BoulusAJ 0:8e87cdf07037 132 // Linear Scaler
BoulusAJ 3:1a714eccfe9f 133 LinearCharacteristics CurrentToVoltage(-15.0f, 15.0f, 0.0f, 5.0f);
BoulusAJ 3:1a714eccfe9f 134 LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -4000.0f, 4000.0f);
BoulusAJ 0:8e87cdf07037 135
BoulusAJ 0:8e87cdf07037 136 // PID Controllers
BoulusAJ 7:07c5b6709d87 137 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 138 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 139
BoulusAJ 0:8e87cdf07037 140 // Timers
BoulusAJ 0:8e87cdf07037 141 Timer Loop;
BoulusAJ 0:8e87cdf07037 142
BoulusAJ 0:8e87cdf07037 143 // ----- User defined functions -----------
BoulusAJ 0:8e87cdf07037 144 void updateControllers(void); // speed controller loop (via interrupt)
BoulusAJ 0:8e87cdf07037 145
BoulusAJ 0:8e87cdf07037 146 // -------------- MAIN LOOP ----------------
BoulusAJ 0:8e87cdf07037 147 int main()
BoulusAJ 0:8e87cdf07037 148 {
BoulusAJ 9:43e5e3430621 149 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 10:4e9899cef12c 150 Button_Status = 0;
BoulusAJ 0:8e87cdf07037 151
BoulusAJ 0:8e87cdf07037 152 pc.baud(115200);
BoulusAJ 2:e088fa08e244 153 Pin_3V3.write(1);
BoulusAJ 4:2a5cd0ad8100 154
BoulusAJ 8:71babe904b92 155 //Loop.start();
BoulusAJ 0:8e87cdf07037 156
BoulusAJ 0:8e87cdf07037 157 // Reset Filters
BoulusAJ 0:8e87cdf07037 158 FilterAccX.reset(0.0f);
BoulusAJ 0:8e87cdf07037 159 FilterAccY.reset(0.0f);
BoulusAJ 0:8e87cdf07037 160 FilterGyro.reset(0.0f);
BoulusAJ 0:8e87cdf07037 161
BoulusAJ 0:8e87cdf07037 162 //--------------------------------- IMU - MPU6050 initialize -------------------------------------
BoulusAJ 0:8e87cdf07037 163 pc.printf("MPU6050 initialize \n\r");
BoulusAJ 0:8e87cdf07037 164 mpu.initialize();
BoulusAJ 0:8e87cdf07037 165 pc.printf("MPU6050 testConnection \n\r");
BoulusAJ 0:8e87cdf07037 166
BoulusAJ 0:8e87cdf07037 167 bool mpu6050TestResult = mpu.testConnection();
BoulusAJ 0:8e87cdf07037 168 if(mpu6050TestResult) {
BoulusAJ 0:8e87cdf07037 169 pc.printf("MPU6050 test passed \n\r");
BoulusAJ 0:8e87cdf07037 170 } else {
BoulusAJ 0:8e87cdf07037 171 pc.printf("MPU6050 test failed \n\r");
BoulusAJ 0:8e87cdf07037 172 }
BoulusAJ 0:8e87cdf07037 173
BoulusAJ 0:8e87cdf07037 174 // Set Low Pass Filter Bandwidth to 44Hz (4.9ms) for the Acc and 42Hz (4.8ms) for the Gyroscope
BoulusAJ 0:8e87cdf07037 175 //mpu.setDLPFMode(MPU6050_DLPF_BW_42);
BoulusAJ 0:8e87cdf07037 176
BoulusAJ 0:8e87cdf07037 177 // Set Low Pass Filter Bandwidth to 21Hz (8.5ms) for the Acc and 20Hz (8.3ms) for the Gyroscope
BoulusAJ 0:8e87cdf07037 178 mpu.setDLPFMode(MPU6050_DLPF_BW_20);
BoulusAJ 0:8e87cdf07037 179
BoulusAJ 0:8e87cdf07037 180 // Change the scale of the Gyroscope to +/- 1000 degrees/sec
BoulusAJ 0:8e87cdf07037 181 mpu.setFullScaleGyroRange(2u);
BoulusAJ 0:8e87cdf07037 182
BoulusAJ 0:8e87cdf07037 183 // Change the scale of the Accelerometer to +/- 4g - Sensitivity: 4096 LSB/mg
BoulusAJ 0:8e87cdf07037 184 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);
BoulusAJ 0:8e87cdf07037 185
BoulusAJ 0:8e87cdf07037 186 //----------------------------------------------------------------------------------------------
BoulusAJ 4:2a5cd0ad8100 187
BoulusAJ 0:8e87cdf07037 188 // Reset PID
BoulusAJ 6:122879c1503a 189 C1.reset(0.0f);
BoulusAJ 11:af811e6f9a04 190
BoulusAJ 11:af811e6f9a04 191 // Wait
BoulusAJ 10:4e9899cef12c 192 wait(10);
BoulusAJ 0:8e87cdf07037 193
BoulusAJ 0:8e87cdf07037 194 pc.printf("Hello World!\n\r");
BoulusAJ 9:43e5e3430621 195 //wait(15);
BoulusAJ 8:71babe904b92 196 Loop.start();
BoulusAJ 10:4e9899cef12c 197 Button_Status = 0;
BoulusAJ 7:07c5b6709d87 198 ControllerLoopTimer.attach(&updateControllers, Ts);
BoulusAJ 11:af811e6f9a04 199
BoulusAJ 11:af811e6f9a04 200
BoulusAJ 11:af811e6f9a04 201 // Configure the ADC to the internel reference of 1.2V. ADC -> Congif Reg 0x40007504 // Multiply the ADC output (PIN input) by 1.2 to get the actual input relative to 3.0V. Then multiply by 3.0 to get the actual voltage
BoulusAJ 11:af811e6f9a04 202 *((unsigned int *)0x40007504) = 0x400A;
BoulusAJ 7:07c5b6709d87 203
BoulusAJ 7:07c5b6709d87 204 }
BoulusAJ 4:2a5cd0ad8100 205
BoulusAJ 10:4e9899cef12c 206 // Loop.reset();
BoulusAJ 8:71babe904b92 207
BoulusAJ 7:07c5b6709d87 208 void updateControllers(void)
BoulusAJ 7:07c5b6709d87 209 {
BoulusAJ 8:71babe904b92 210
BoulusAJ 0:8e87cdf07037 211
BoulusAJ 7:07c5b6709d87 212 // Counter
BoulusAJ 7:07c5b6709d87 213 //LoopCounter++;
BoulusAJ 0:8e87cdf07037 214
BoulusAJ 7:07c5b6709d87 215 // Acquire Velocity
BoulusAJ 7:07c5b6709d87 216 Velocity_Voltage_Read = Velocity_Voltage_Input.read();
BoulusAJ 11:af811e6f9a04 217 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 218 Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
BoulusAJ 7:07c5b6709d87 219 Velocity = Velocity_rpm*2.0*pi/60.0;
BoulusAJ 10:4e9899cef12c 220
BoulusAJ 0:8e87cdf07037 221
BoulusAJ 7:07c5b6709d87 222 // Aquire Raw Acceleration and Gyro Data form the IMU
BoulusAJ 7:07c5b6709d87 223 mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);
BoulusAJ 7:07c5b6709d87 224
BoulusAJ 7:07c5b6709d87 225 // -------------- Convert Raw data to SI Units --------------------
BoulusAJ 7:07c5b6709d87 226
BoulusAJ 7:07c5b6709d87 227 //Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
BoulusAJ 7:07c5b6709d87 228 AccX_g = AccX_Raw / 8192.0f;
BoulusAJ 7:07c5b6709d87 229 AccY_g = AccY_Raw / 8192.0f;
BoulusAJ 7:07c5b6709d87 230 AccZ_g = AccZ_Raw / 8192.0f;
BoulusAJ 0:8e87cdf07037 231
BoulusAJ 7:07c5b6709d87 232 //Convert Gyroscope Raw Data to Degrees per second
BoulusAJ 7:07c5b6709d87 233 GyroX_Degrees = GyroX_Raw / 32.768f; // (2^15/1000 = 32.768)
BoulusAJ 7:07c5b6709d87 234 GyroY_Degrees = GyroY_Raw / 32.768f; // (2^15/1000 = 32.768)
BoulusAJ 7:07c5b6709d87 235 GyroZ_Degrees = GyroZ_Raw / 32.768f; // (2^15/1000 = 32.768)
BoulusAJ 0:8e87cdf07037 236
BoulusAJ 7:07c5b6709d87 237 //Convert Gyroscope Raw Data to Degrees per second
BoulusAJ 7:07c5b6709d87 238 GyroZ_RadiansPerSecond = (GyroZ_Raw / 32.768f)* pi/180.0f;
BoulusAJ 0:8e87cdf07037 239
BoulusAJ 7:07c5b6709d87 240 // ----- Combine Accelerometer Data and Gyro Data to Get Angle ------
BoulusAJ 0:8e87cdf07037 241
BoulusAJ 7:07c5b6709d87 242 Cuboid_Angle_Radians = -1*atan2(-FilterAccX(AccX_g), FilterAccY(AccY_g)) + 0.7854f + FilterGyro(GyroZ_RadiansPerSecond); // Check later if fast enough!!
BoulusAJ 7:07c5b6709d87 243 Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;
BoulusAJ 0:8e87cdf07037 244
BoulusAJ 0:8e87cdf07037 245
BoulusAJ 7:07c5b6709d87 246 // ------------------------- Controller -----------------------------
BoulusAJ 10:4e9899cef12c 247
BoulusAJ 10:4e9899cef12c 248 switch(Button_Status) {
BoulusAJ 10:4e9899cef12c 249
BoulusAJ 10:4e9899cef12c 250 case 0:
BoulusAJ 11:af811e6f9a04 251
BoulusAJ 10:4e9899cef12c 252 // PI Controller
BoulusAJ 10:4e9899cef12c 253 PID_Input = Desired_input - Velocity;
BoulusAJ 10:4e9899cef12c 254 PID_Output = C1.update(PID_Input);
BoulusAJ 10:4e9899cef12c 255
BoulusAJ 10:4e9899cef12c 256 // System input
BoulusAJ 10:4e9899cef12c 257 Sys_input_Amps = PID_Output;
BoulusAJ 10:4e9899cef12c 258 VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 11:af811e6f9a04 259
BoulusAJ 10:4e9899cef12c 260 // Counter of 10 seconds then go to Case 1
BoulusAJ 10:4e9899cef12c 261 Case1_Counter++;
BoulusAJ 10:4e9899cef12c 262 if (Case1_Counter > 2000) {
BoulusAJ 10:4e9899cef12c 263 Button_Status = 1;
BoulusAJ 10:4e9899cef12c 264 }
BoulusAJ 8:71babe904b92 265
BoulusAJ 10:4e9899cef12c 266 case 1:
BoulusAJ 0:8e87cdf07037 267
BoulusAJ 10:4e9899cef12c 268 // Current Input Updater - Amperes
BoulusAJ 10:4e9899cef12c 269 // Loop 1
BoulusAJ 10:4e9899cef12c 270 Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
BoulusAJ 10:4e9899cef12c 271 // Loop 2
BoulusAJ 10:4e9899cef12c 272 Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
BoulusAJ 10:4e9899cef12c 273 // PI Controller
BoulusAJ 11:af811e6f9a04 274 //PID_Input = Desired_input - Velocity;
BoulusAJ 11:af811e6f9a04 275 //PID_Output = C1.update(PID_Input);
BoulusAJ 10:4e9899cef12c 276
BoulusAJ 11:af811e6f9a04 277 PID_Input = 52.36f - Velocity;
BoulusAJ 11:af811e6f9a04 278 PID_Output = C1.update(-1*PID_Input);
BoulusAJ 10:4e9899cef12c 279
BoulusAJ 10:4e9899cef12c 280 // System input
BoulusAJ 11:af811e6f9a04 281 //Sys_input_Amps = 2.0f; //PID_Output; //- Loop1_output - Loop2_output;
BoulusAJ 11:af811e6f9a04 282 VoltageOut.write(0.5f);
BoulusAJ 3:1a714eccfe9f 283
BoulusAJ 10:4e9899cef12c 284 if (Sys_input_Amps > uMax2) {
BoulusAJ 10:4e9899cef12c 285 Sys_input_Amps = uMax2;
BoulusAJ 10:4e9899cef12c 286 }
BoulusAJ 10:4e9899cef12c 287 if (Sys_input_Amps < uMin2) {
BoulusAJ 10:4e9899cef12c 288 Sys_input_Amps = uMin2;
BoulusAJ 10:4e9899cef12c 289 }
BoulusAJ 0:8e87cdf07037 290
BoulusAJ 10:4e9899cef12c 291 // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
BoulusAJ 11:af811e6f9a04 292 //VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
BoulusAJ 4:2a5cd0ad8100 293
BoulusAJ 10:4e9899cef12c 294 // Print Data
BoulusAJ 11:af811e6f9a04 295
BoulusAJ 10:4e9899cef12c 296 if(++k >= 200) {
BoulusAJ 7:07c5b6709d87 297 k = 0;
BoulusAJ 0:8e87cdf07037 298
BoulusAJ 10:4e9899cef12c 299 //1LoopCounter,2.1CurrentToVoltage, 2Sys_input_Amps, 3Cuboid_Angle_Degrees, 4GyroZ_RadiansPerSecond, 5Velocity, 6Velocity_Voltage, 7AccX_g, 8AccY_g, 9PID_Input, 10PID_Output, (Loop1_output+Loop2_output), Loop.read())
BoulusAJ 10:4e9899cef12c 300 pc.printf(" %i, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, %0.5f, ", LoopCounter, CurrentToVoltage(Sys_input_Amps), Sys_input_Amps, Cuboid_Angle_Degrees, GyroZ_RadiansPerSecond, Velocity, Velocity_Voltage, AccX_g, AccY_g, PID_Input, PID_Output, (Loop1_output+Loop2_output));
BoulusAJ 10:4e9899cef12c 301 pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
BoulusAJ 11:af811e6f9a04 302 // Printing Register Values
BoulusAJ 11:af811e6f9a04 303 //printf("ADC : 0x%x\r\n", *((unsigned int *)0x40007504));
BoulusAJ 11:af811e6f9a04 304 //printf("ADC : %0.5f\n", Velocity_Voltage_Input.read());
BoulusAJ 7:07c5b6709d87 305 }
BoulusAJ 4:2a5cd0ad8100 306
BoulusAJ 10:4e9899cef12c 307 //pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
BoulusAJ 7:07c5b6709d87 308
BoulusAJ 10:4e9899cef12c 309 //
BoulusAJ 10:4e9899cef12c 310
BoulusAJ 10:4e9899cef12c 311 // wait(0.0025);
BoulusAJ 10:4e9899cef12c 312 // pc.printf(" %0.5f \n\r", Loop.read());
BoulusAJ 10:4e9899cef12c 313
BoulusAJ 10:4e9899cef12c 314 /*
BoulusAJ 10:4e9899cef12c 315 LoopTime = Loop.read();
BoulusAJ 10:4e9899cef12c 316 while(LoopTime < Ts) {
BoulusAJ 10:4e9899cef12c 317 LoopTime = Loop.read();
BoulusAJ 10:4e9899cef12c 318 }
BoulusAJ 10:4e9899cef12c 319 */
BoulusAJ 10:4e9899cef12c 320
BoulusAJ 10:4e9899cef12c 321 // pc.printf(" %0.5f \n\r", LoopTime);
BoulusAJ 10:4e9899cef12c 322
BoulusAJ 10:4e9899cef12c 323 //pc.printf("Angle = %0.2f, Current = %0.2f, VoltageOut = %0.3f, Velocity = %0.2f, VoltageIn = %0.3f\n\r", Cuboid_Angle_Degrees, Sys_input_Amps, CurrentToVoltage(Sys_input_Amps), Velocity, Velocity_Voltage);
BoulusAJ 10:4e9899cef12c 324
BoulusAJ 10:4e9899cef12c 325 /*
BoulusAJ 10:4e9899cef12c 326 // Print Data
BoulusAJ 10:4e9899cef12c 327 if(++k >= 300) {
BoulusAJ 10:4e9899cef12c 328 k = 0;
BoulusAJ 10:4e9899cef12c 329 //pc.printf("%0.8f\n\r", Loop.read());
BoulusAJ 10:4e9899cef12c 330 pc.printf("Angle = %0.2f, Current = %0.2f, VoltageOut = %0.3f, Velocity = %0.2f, VoltageIn = %0.3f\n\r", Cuboid_Angle_Degrees, Sys_input_Amps, CurrentToVoltage(Sys_input_Amps), Velocity, Velocity_Voltage);
BoulusAJ 10:4e9899cef12c 331
BoulusAJ 10:4e9899cef12c 332 }
BoulusAJ 10:4e9899cef12c 333 */
BoulusAJ 10:4e9899cef12c 334
BoulusAJ 10:4e9899cef12c 335 }
BoulusAJ 1:d3406369c297 336 }
BoulusAJ 0:8e87cdf07037 337
BoulusAJ 0:8e87cdf07037 338
BoulusAJ 0:8e87cdf07037 339
BoulusAJ 0:8e87cdf07037 340
BoulusAJ 0:8e87cdf07037 341
BoulusAJ 0:8e87cdf07037 342 //-------------------------------------------------------------------
BoulusAJ 0:8e87cdf07037 343 //--------------------------Example Codes-----------------------------
BoulusAJ 0:8e87cdf07037 344
BoulusAJ 0:8e87cdf07037 345 // This code demonstrates how to change the Low Pass Filter Bandwidth
BoulusAJ 0:8e87cdf07037 346 /*
BoulusAJ 0:8e87cdf07037 347 pc.printf("Digital Low Pass Filter Bandwidth: %u \n\r", mpu.getDLPFMode());
BoulusAJ 0:8e87cdf07037 348
BoulusAJ 0:8e87cdf07037 349 mpu.setDLPFMode(MPU6050_DLPF_BW_42);
BoulusAJ 0:8e87cdf07037 350 wait(0.1);
BoulusAJ 0:8e87cdf07037 351 pc.printf("Digital Low Pass Filter Bandwidth: %u \n\r", mpu.getDLPFMode());
BoulusAJ 0:8e87cdf07037 352 */
BoulusAJ 0:8e87cdf07037 353 // ----------------
BoulusAJ 0:8e87cdf07037 354
BoulusAJ 0:8e87cdf07037 355 // This code demonstrates how to change the scale settings for the Gyro scope
BoulusAJ 0:8e87cdf07037 356 /*
BoulusAJ 0:8e87cdf07037 357 pc.printf("Gyro Range Scale: %u \n\r", mpu.getFullScaleGyroRange());
BoulusAJ 0:8e87cdf07037 358 int Temp123 = 0;
BoulusAJ 0:8e87cdf07037 359 while(Temp123 < 5) {
BoulusAJ 0:8e87cdf07037 360
BoulusAJ 0:8e87cdf07037 361 wait(0.4);
BoulusAJ 0:8e87cdf07037 362 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
BoulusAJ 0:8e87cdf07037 363 //writing current accelerometer and gyro position
BoulusAJ 0:8e87cdf07037 364 pc.printf(" AccX: %d; AccY: %d; AccZ: %d; GyroX: %d; GyroY: %d; GyroZ: %d;\n\r",ax,ay,az,gx,gy,gz);
BoulusAJ 0:8e87cdf07037 365 Temp123 = Temp123 + 1;
BoulusAJ 0:8e87cdf07037 366 }
BoulusAJ 0:8e87cdf07037 367 Temp123 = 0;
BoulusAJ 0:8e87cdf07037 368
BoulusAJ 0:8e87cdf07037 369 mpu.setFullScaleGyroRange(3u);
BoulusAJ 0:8e87cdf07037 370 wait(0.1);
BoulusAJ 0:8e87cdf07037 371 pc.printf("Gyro Range Scale: %u \n\r", mpu.getFullScaleGyroRange());
BoulusAJ 0:8e87cdf07037 372 while(Temp123 < 5) {
BoulusAJ 0:8e87cdf07037 373
BoulusAJ 0:8e87cdf07037 374 wait(0.4);
BoulusAJ 0:8e87cdf07037 375 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
BoulusAJ 0:8e87cdf07037 376 //writing current accelerometer and gyro position
BoulusAJ 0:8e87cdf07037 377 pc.printf(" AccX: %d; AccY: %d; AccZ: %d; GyroX: %d; GyroY: %d; GyroZ: %d;\n\r",ax,ay,az,gx,gy,gz);
BoulusAJ 0:8e87cdf07037 378 Temp123 = Temp123 + 1;
BoulusAJ 0:8e87cdf07037 379 }
BoulusAJ 0:8e87cdf07037 380 Temp123 = 0;
BoulusAJ 0:8e87cdf07037 381 */
BoulusAJ 0:8e87cdf07037 382
BoulusAJ 0:8e87cdf07037 383 /*
BoulusAJ 0:8e87cdf07037 384 while(1) {
BoulusAJ 0:8e87cdf07037 385 wait(0.4);
BoulusAJ 0:8e87cdf07037 386 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
BoulusAJ 0:8e87cdf07037 387 //writing current accelerometer and gyro position
BoulusAJ 0:8e87cdf07037 388 pc.printf(" AccX: %d; AccY: %d; AccZ: %d; GyroX: %d; GyroY: %d; GyroZ: %d;\n\r",ax,ay,az,gx,gy,gz);
BoulusAJ 0:8e87cdf07037 389 }
BoulusAJ 0:8e87cdf07037 390 */
BoulusAJ 0:8e87cdf07037 391 // ------------------
BoulusAJ 0:8e87cdf07037 392
BoulusAJ 0:8e87cdf07037 393
BoulusAJ 0:8e87cdf07037 394 // Printing Register Values
BoulusAJ 0:8e87cdf07037 395 //printf("i2c SCL : 0x%x\r\n", *((unsigned int *)0x40004508));
BoulusAJ 0:8e87cdf07037 396
BoulusAJ 0:8e87cdf07037 397 // ------------------
BoulusAJ 0:8e87cdf07037 398
BoulusAJ 0:8e87cdf07037 399 // Velocity Measurement Code for Designing a Filter
BoulusAJ 0:8e87cdf07037 400 /*
BoulusAJ 0:8e87cdf07037 401 if (aaa < (600*3)) {
BoulusAJ 0:8e87cdf07037 402 if (++aaa <= 900) {
BoulusAJ 0:8e87cdf07037 403 VoltageOut.write(CurrentToVoltage(1.0f));
BoulusAJ 0:8e87cdf07037 404 } else {
BoulusAJ 0:8e87cdf07037 405 VoltageOut.write(CurrentToVoltage(-1.0f));
BoulusAJ 0:8e87cdf07037 406 }
BoulusAJ 0:8e87cdf07037 407 pc.printf("%0.8f\n\r", Velocity_Voltage_Read);
BoulusAJ 0:8e87cdf07037 408 } else {
BoulusAJ 0:8e87cdf07037 409 VoltageOut.write(CurrentToVoltage(0.0f));
BoulusAJ 0:8e87cdf07037 410 }
BoulusAJ 0:8e87cdf07037 411 */