GRT_FS2019_VoiceCoil
Cube_Mini_Template
Dependencies: mbed QEI MPU6050_2 BLE_API nRF51822 MCP4725 eMPL_MPU6050
Diff: main.cpp
- Revision:
- 4:2a5cd0ad8100
- Parent:
- 3:1a714eccfe9f
- Child:
- 5:411f5351220f
--- a/main.cpp Fri May 08 17:02:29 2020 +0000
+++ b/main.cpp Tue May 12 14:46:42 2020 +0000
@@ -14,7 +14,7 @@
#include "mcp4725.h"
#include <stdbool.h>
#include "LinearCharacteristics.h"
-#include "PID_Cntrl.h"
+#include "PID_Cntrl_3.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_2 C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1); // Defining the 1st Loop Controller (PI-Part)
-PID_Cntrl_2 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_3 C1(Kp_1,Ki_1,Kd_1,Tf_1,Ts,uMin1,uMax1); // Defining the 1st Loop Controller (PI-Part)
+PID_Cntrl_3 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;
@@ -145,7 +145,7 @@
pc.baud(115200);
Pin_3V3.write(1);
-
+
Loop.start();
// Reset Filters
@@ -178,114 +178,115 @@
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);
//----------------------------------------------------------------------------------------------
-
+
// Reset PID
- C1.reset();
+ C1.reset(0.0);
pc.printf("Hello World!\n\r");
// ControllerLoopTimer.attach(&updateControllers, Ts);
-wait(1);
+ wait(1);
-while(1)
-{
- Loop.reset();
-
- // Counter
- LoopCounter++;
+ while(1) {
+ Loop.reset();
+
+ // Counter
+ LoopCounter++;
- // Acquire Velocity
- Velocity_Voltage_Read = Velocity_Voltage_Input.read();
- Velocity_Voltage = 3.25*(Velocity_Voltage_Read);
- Velocity_rpm = VoltageToVelocity(Velocity_Voltage);
- Velocity = Velocity_rpm*2.0*pi/60.0;
+ // 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;
- // Aquire Raw Acceleration and Gyro Data form the IMU
- mpu.getMotion6(&AccX_Raw, &AccY_Raw, &AccZ_Raw, &GyroX_Raw, &GyroY_Raw, &GyroZ_Raw);
+ // 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 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 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);
+ // ------------------------- 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(PID_Input);
/*
- //PID_Input = 20.0 - Velocity;
+ //PID_Input = 20.0 - Velocity;
//PID_Output = C1.update(-1*PID_Input);
*/
-PID_Output = 0; // CHANGE LATER - This is for testing purposes
+ 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));
+ //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());
+
+ //
+
+ // wait(0.0025);
+ // pc.printf(" %0.5f \n\r", Loop.read());
- //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());
-
- //
-
- // wait(0.0025);
- // pc.printf(" %0.5f \n\r", Loop.read());
-
- LoopTime = Loop.read();
- while(LoopTime < Ts) {
- LoopTime = Loop.read();
- }
-
- 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);
+ /*
+ LoopTime = Loop.read();
+ while(LoopTime < Ts) {
+ LoopTime = Loop.read();
+ }
+ */
+
+ // 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);
-}
+ }
+ */
+
+ }
}