GRT_FS2019_VoiceCoil
Cube_Mini_Solution
Dependencies: mbed QEI MPU6050 BLE_API nRF51822 MCP4725 eMPL_MPU6050
Diff: main.cpp
- Revision:
- 7:07c5b6709d87
- Parent:
- 6:122879c1503a
- Child:
- 8:71babe904b92
--- a/main.cpp Tue May 12 15:44:23 2020 +0000
+++ b/main.cpp Tue May 12 16:51:44 2020 +0000
@@ -14,7 +14,7 @@
#include "mcp4725.h"
#include <stdbool.h>
#include "LinearCharacteristics.h"
-#include "PID_Cntrl.h"
+#include "PID_Cntrl_Old.h"
#include "PID_Cntrl_2.h"
// Serial PC Communication
@@ -130,8 +130,8 @@
LinearCharacteristics VoltageToVelocity(0.0f, 3.0f, -4000.0f, 4000.0f);
// PID Controllers
-PID_Cntrl C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1); // Defining the 1st Loop Controller (PI-Part)
-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
+PID_Cntrl_Old C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1); // Defining the 1st Loop Controller (PI-Part)
+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
// Timers
Timer Loop;
@@ -183,107 +183,117 @@
C1.reset(0.0f);
pc.printf("Hello World!\n\r");
- // ControllerLoopTimer.attach(&updateControllers, Ts);
-
wait(1);
- while(1) {
- Loop.reset();
+ ControllerLoopTimer.attach(&updateControllers, Ts);
+
+}
- // Counter
- LoopCounter++;
+void updateControllers(void)
+{
+ //Loop.reset();
- // Acquire Velocity
- Velocity_Voltage_Read = Velocity_Voltage_Input.read();
- Velocity_Voltage = 3.2442*(Velocity_Voltage_Read);
- Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
- Velocity = Velocity_rpm*2.0*pi/60.0;
+ // Counter
+ //LoopCounter++;
- // Aquire Raw Acceleration and Gyro Data form the IMU
- mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);
-
- // -------------- Convert Raw data to SI Units --------------------
+ // Acquire Velocity
+ Velocity_Voltage_Read = Velocity_Voltage_Input.read();
+ Velocity_Voltage = 3.2442178*(Velocity_Voltage_Read);
+ Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
+ Velocity = Velocity_rpm*2.0*pi/60.0;
+
- //Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
- AccX_g = AccX_Raw / 8192.0f;
- AccY_g = AccY_Raw / 8192.0f;
- AccZ_g = AccZ_Raw / 8192.0f;
+ // Aquire Raw Acceleration and Gyro Data form the IMU
+ mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);
+
+ // -------------- Convert Raw data to SI Units --------------------
+
+ //Convert Acceleration Raw Data to (ms^-2) - (Settings of +/- 4g)
+ AccX_g = AccX_Raw / 8192.0f;
+ AccY_g = AccY_Raw / 8192.0f;
+ AccZ_g = AccZ_Raw / 8192.0f;
- //Convert Gyroscope Raw Data to Degrees per second
- GyroX_Degrees = GyroX_Raw / 32.768f; // (2^15/1000 = 32.768)
- GyroY_Degrees = GyroY_Raw / 32.768f; // (2^15/1000 = 32.768)
- GyroZ_Degrees = GyroZ_Raw / 32.768f; // (2^15/1000 = 32.768)
+ //Convert Gyroscope Raw Data to Degrees per second
+ GyroX_Degrees = GyroX_Raw / 32.768f; // (2^15/1000 = 32.768)
+ GyroY_Degrees = GyroY_Raw / 32.768f; // (2^15/1000 = 32.768)
+ GyroZ_Degrees = GyroZ_Raw / 32.768f; // (2^15/1000 = 32.768)
- //Convert Gyroscope Raw Data to Degrees per second
- GyroZ_RadiansPerSecond = (GyroZ_Raw / 32.768f)* pi/180.0f;
+ //Convert Gyroscope Raw Data to Degrees per second
+ GyroZ_RadiansPerSecond = (GyroZ_Raw / 32.768f)* pi/180.0f;
- // ----- Combine Accelerometer Data and Gyro Data to Get Angle ------
+ // ----- Combine Accelerometer Data and Gyro Data to Get Angle ------
- Cuboid_Angle_Radians = -1*atan2(-FilterAccX(AccX_g), FilterAccY(AccY_g)) + 0.7854f + FilterGyro(GyroZ_RadiansPerSecond); // Check later if fast enough!!
- Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;
+ Cuboid_Angle_Radians = -1*atan2(-FilterAccX(AccX_g), FilterAccY(AccY_g)) + 0.7854f + FilterGyro(GyroZ_RadiansPerSecond); // Check later if fast enough!!
+ Cuboid_Angle_Degrees = Cuboid_Angle_Radians*180.0f/pi;
- // ------------------------- Controller -----------------------------
- // Current Input Updater - Amperes
- // Loop 1
- Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
- // Loop 2
- Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
- // PI Controller
- PID_Input = Desired_input - Velocity;
- PID_Output = C1.update(PID_Input);
- /*
- //PID_Input = 20.0 - Velocity;
- //PID_Output = C1.update(-1*PID_Input);
- //PID_Output = 0; // CHANGE LATER - This is for testing purposes
- */
+ // ------------------------- Controller -----------------------------
+ // Current Input Updater - Amperes
+ // Loop 1
+ Loop1_output = Cuboid_Angle_Radians*K_SS_Controller[0];
+ // Loop 2
+ Loop2_output = GyroZ_RadiansPerSecond*K_SS_Controller[1];
+ // PI Controller
+ PID_Input = Desired_input - Velocity;
+ PID_Output = C1.update(-1*PID_Input);
+ /*
+ //PID_Input = 20.0 - Velocity;
+ //PID_Output = C1.update(-1*PID_Input);
+ //PID_Output = 0; // CHANGE LATER - This is for testing purposes
+ */
- // System input
- Sys_input_Amps = PID_Output; //- Loop1_output - Loop2_output;
+ // System input
+ Sys_input_Amps = PID_Output; //- Loop1_output - Loop2_output;
- if (Sys_input_Amps > uMax2) {
- Sys_input_Amps = uMax2;
- }
- if (Sys_input_Amps < uMin2) {
- Sys_input_Amps = uMin2;
- }
+ if (Sys_input_Amps > uMax2) {
+ Sys_input_Amps = uMax2;
+ }
+ if (Sys_input_Amps < uMin2) {
+ Sys_input_Amps = uMin2;
+ }
- // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
- VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
+ // Scaling the controller output from -15 A --> 15 A to 0 V --> 5 V
+ VoltageOut.write(CurrentToVoltage(Sys_input_Amps));
+
+ // Print Data
+ if(++k >= 30) {
+ k = 0;
//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())
- //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));
-
- //pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
-
- //
+ 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));
+ pc.printf("\n\r");
+ }
- // wait(0.0025);
- // pc.printf(" %0.5f \n\r", Loop.read());
+ //pc.printf("%i; %0.6f, \n\r", LoopCounter, Loop.read());
- /*
- LoopTime = Loop.read();
- while(LoopTime < Ts) {
- LoopTime = Loop.read();
- }
- */
+ //
+
+ // wait(0.0025);
+ // pc.printf(" %0.5f \n\r", Loop.read());
- // pc.printf(" %0.5f \n\r", LoopTime);
+ /*
+ LoopTime = Loop.read();
+ while(LoopTime < Ts) {
+ LoopTime = Loop.read();
+ }
+ */
- //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);
+ // pc.printf(" %0.5f \n\r", LoopTime);
+
+ //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);
- /*
- // Print Data
- if(++k >= 300) {
- k = 0;
- //pc.printf("%0.8f\n\r", Loop.read());
- 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);
+ /*
+ // Print Data
+ if(++k >= 300) {
+ k = 0;
+ //pc.printf("%0.8f\n\r", Loop.read());
+ 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);
- }
- */
+ }
+ */
- }
+
}