John Fitzpatrick / Mbed OS Acc-Level-08

test

Dependencies:   FXOS8700_3 FXAS21002 mbed-rtos kalman MotionSensor EthernetInterface

main.cpp@2:e429f00a3493, 2021-03-28 (annotated)

Committer:
fitzpatrick
Date:
Sun Mar 28 20:31:29 2021 +0000
Revision:
2:e429f00a3493
Parent:
1:588b597b1c1b 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MACRUM 0:21d86aae6b2a 1 #include "mbed.h"
fitzpatrick 1:588b597b1c1b 2 #include "FXOS8700.h"
fitzpatrick 1:588b597b1c1b 3 #include "FXAS21002.h"
fitzpatrick 1:588b597b1c1b 4 #include "kalman.c"
fitzpatrick 1:588b597b1c1b 5 #include "EthernetInterface.h"
fitzpatrick 1:588b597b1c1b 6
fitzpatrick 1:588b597b1c1b 7 #define ROLL_SERVO_CENTER 1570
fitzpatrick 1:588b597b1c1b 8 #define ROLL_SERVO_MAX 2000
fitzpatrick 1:588b597b1c1b 9 #define ROLL_SERVO_MIN 1000
fitzpatrick 1:588b597b1c1b 10
fitzpatrick 1:588b597b1c1b 11 #define PITCH_SERVO_CENTER 1570
fitzpatrick 1:588b597b1c1b 12 #define PITCH_SERVO_MAX 2000
fitzpatrick 1:588b597b1c1b 13 #define PITCH_SERVO_MIN 1000
fitzpatrick 1:588b597b1c1b 14
fitzpatrick 1:588b597b1c1b 15 #define PI 3.14159265
fitzpatrick 1:588b597b1c1b 16 #define Rad2Degree 57.2957795
fitzpatrick 1:588b597b1c1b 17
fitzpatrick 1:588b597b1c1b 18 const int PORT = 7; //arbitrary port
fitzpatrick 1:588b597b1c1b 19
fitzpatrick 1:588b597b1c1b 20 static const char* SERVER_IP = "192.168.1.101"; //IP of server board
fitzpatrick 1:588b597b1c1b 21 static const char* MASK = "255.255.255.0"; //mask
fitzpatrick 1:588b597b1c1b 22 static const char* GATEWAY = "192.168.1.1"; //gateway
fitzpatrick 1:588b597b1c1b 23
fitzpatrick 1:588b597b1c1b 24
fitzpatrick 1:588b597b1c1b 25 Serial pc(USBTX, USBRX); // Serial Port 115200
fitzpatrick 1:588b597b1c1b 26
fitzpatrick 1:588b597b1c1b 27 // Sensor GPIO Definitions, I2C
fitzpatrick 1:588b597b1c1b 28 FXOS8700 accel(D14,D15);
fitzpatrick 1:588b597b1c1b 29 FXOS8700 mag(D14,D15);
fitzpatrick 1:588b597b1c1b 30 FXAS21002 gyro(D14,D15);
fitzpatrick 1:588b597b1c1b 31
fitzpatrick 1:588b597b1c1b 32 EthernetInterface eth; //create ethernet
fitzpatrick 1:588b597b1c1b 33 UDPSocket server; //creat server
fitzpatrick 1:588b597b1c1b 34 Endpoint client; //create endpoint
fitzpatrick 1:588b597b1c1b 35
fitzpatrick 1:588b597b1c1b 36 //Timer used in Kalman filter
fitzpatrick 1:588b597b1c1b 37 Timer ProgramTimer;
MACRUM 0:21d86aae6b2a 38
fitzpatrick 1:588b597b1c1b 39 // Servo GPIO Definitions
fitzpatrick 1:588b597b1c1b 40 PwmOut Pitch_M(D11);
fitzpatrick 1:588b597b1c1b 41 PwmOut Roll_M(D10);
fitzpatrick 1:588b597b1c1b 42
fitzpatrick 1:588b597b1c1b 43 //Define Kalman filter Parameters
fitzpatrick 1:588b597b1c1b 44 kalman filter_pitch;
fitzpatrick 1:588b597b1c1b 45 kalman filter_roll;
fitzpatrick 1:588b597b1c1b 46
fitzpatrick 1:588b597b1c1b 47 float R;
fitzpatrick 1:588b597b1c1b 48 float Roll_P_C = 1.0f, Roll_I_C = 0.01f, Roll_D_C = 0.01f; //Roll PID coefficient Values
fitzpatrick 1:588b597b1c1b 49 float Roll_P = 0, Roll_I = 0, Roll_D =0, Roll_Correct = 0, Roll_Error = 0; //Roll PID Control Values
fitzpatrick 1:588b597b1c1b 50 float Pitch_P_C = 1.0f, Pitch_I_C = 0.01f, Pitch_D_C = 0.01f; //Pitch PID coefficient Values
fitzpatrick 1:588b597b1c1b 51 float Pitch_P = 0, Pitch_I = 0, Pitch_D = 0, Pitch_Correct = 0, Pitch_Error = 0; //Pitch PID control Values
fitzpatrick 1:588b597b1c1b 52 int Roll_Servo_Val, Pitch_Servo_Val; // Servo pulse widths
fitzpatrick 1:588b597b1c1b 53 double angle[3]; // Kalman Pitch and Roll angles
fitzpatrick 1:588b597b1c1b 54 unsigned long timer; // absolute time stamp
fitzpatrick 1:588b597b1c1b 55 int val, Loop_Count; // Loop_Count used to settel kalman filter
fitzpatrick 1:588b597b1c1b 56
fitzpatrick 1:588b597b1c1b 57 int n; //size of received message
fitzpatrick 1:588b597b1c1b 58 char counter[1] = {0}; //sample receive/send buffer
fitzpatrick 1:588b597b1c1b 59
fitzpatrick 1:588b597b1c1b 60 int main() {
fitzpatrick 1:588b597b1c1b 61
fitzpatrick 1:588b597b1c1b 62 pc.baud(115200); // Serial Port 115200
fitzpatrick 1:588b597b1c1b 63
fitzpatrick 1:588b597b1c1b 64 // Configure Accelerometer FXOS8700, Magnetometer FXOS8700 & Gyroscope FXAS21002
fitzpatrick 1:588b597b1c1b 65 accel.accel_config();
fitzpatrick 1:588b597b1c1b 66 mag.mag_config();
fitzpatrick 1:588b597b1c1b 67 gyro.gyro_config();
fitzpatrick 1:588b597b1c1b 68
fitzpatrick 1:588b597b1c1b 69 eth.init(SERVER_IP, MASK, GATEWAY); //set up IP
fitzpatrick 1:588b597b1c1b 70 eth.connect(); //connect ethernet
fitzpatrick 1:588b597b1c1b 71 printf("\nSERVER - Server IP Address is %s\r\n", eth.getIPAddress()); //get server IP address;
fitzpatrick 1:588b597b1c1b 72 server.bind(PORT); //bind server
fitzpatrick 1:588b597b1c1b 73
fitzpatrick 1:588b597b1c1b 74 // Initialize Kalman Filter
fitzpatrick 1:588b597b1c1b 75 kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
fitzpatrick 1:588b597b1c1b 76 kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
MACRUM 0:21d86aae6b2a 77
fitzpatrick 1:588b597b1c1b 78 // Accelerometer, Magnomometer and Gyro Variables
fitzpatrick 1:588b597b1c1b 79 float adata[3]; //Accelerometer x, y, x values
fitzpatrick 1:588b597b1c1b 80 float mag_data[3]; //Magnomometer x, y, x values
fitzpatrick 1:588b597b1c1b 81 float gyro_data[3]; //Gyro pitch, roll, yaw values
fitzpatrick 1:588b597b1c1b 82
fitzpatrick 1:588b597b1c1b 83 // Roll Servo Setup
fitzpatrick 1:588b597b1c1b 84 int Roll_pulse = ROLL_SERVO_CENTER;
fitzpatrick 1:588b597b1c1b 85 Roll_M.period_us(20000); // servo requires a 20ms period
fitzpatrick 1:588b597b1c1b 86 Roll_M.pulsewidth_us(Roll_pulse); // servo position determined by a pulsewidth between 1-2ms
fitzpatrick 1:588b597b1c1b 87 // Pitch Servo Setup
fitzpatrick 1:588b597b1c1b 88 int Pitch_pulse = PITCH_SERVO_CENTER;
fitzpatrick 1:588b597b1c1b 89 Pitch_M.period_us(20000); // servo requires a 20ms period
fitzpatrick 1:588b597b1c1b 90 Pitch_M.pulsewidth_us(Pitch_pulse); // servo position determined by a pulsewidth between 1-2ms
fitzpatrick 1:588b597b1c1b 91 wait(1); // Allow servos to settel
fitzpatrick 1:588b597b1c1b 92
fitzpatrick 1:588b597b1c1b 93 //Absolute Time Needed to Calculate Delta Time
fitzpatrick 1:588b597b1c1b 94 ProgramTimer.start();
fitzpatrick 1:588b597b1c1b 95 timer = ProgramTimer.read_us();
fitzpatrick 1:588b597b1c1b 96
fitzpatrick 1:588b597b1c1b 97 Loop_Count = 15; // Allows Kalman filter to sync.
fitzpatrick 1:588b597b1c1b 98
MACRUM 0:21d86aae6b2a 99 while (true) {
fitzpatrick 1:588b597b1c1b 100
fitzpatrick 1:588b597b1c1b 101 //scanf("%d", &val); // Used for debug
fitzpatrick 1:588b597b1c1b 102 //printf("\r\n Value is %d \r\n", val); // Used for debug
fitzpatrick 1:588b597b1c1b 103
fitzpatrick 1:588b597b1c1b 104 // Acquire Accelerometer values
fitzpatrick 1:588b597b1c1b 105 accel.acquire_accel_data_g(adata); // Acquire Acceleration Vectors
fitzpatrick 1:588b597b1c1b 106 printf("\r\n Accelerometer Values\r\n");
fitzpatrick 1:588b597b1c1b 107 printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", adata[0],adata[1],adata[2]);
fitzpatrick 1:588b597b1c1b 108
fitzpatrick 1:588b597b1c1b 109 // Acquire Magnomometer Vectors
fitzpatrick 1:588b597b1c1b 110 mag.acquire_mag_data_uT(mag_data); // Acquire Magnomometer Vectors
fitzpatrick 1:588b597b1c1b 111 printf("\r\n Magnomometer Values\r\n");
fitzpatrick 1:588b597b1c1b 112 printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", mag_data[0],mag_data[1],mag_data[2]);
fitzpatrick 1:588b597b1c1b 113
fitzpatrick 1:588b597b1c1b 114 // Acquire Gyro values
fitzpatrick 1:588b597b1c1b 115 gyro.acquire_gyro_data_dps(gyro_data); // Acquire Gyro values
fitzpatrick 1:588b597b1c1b 116 printf("\r\n Gyro Values\r\n");
fitzpatrick 1:588b597b1c1b 117 printf(" Pitch: %4.2f,\t Roll: %4.2f,\t Yaw: %4.2f\r\n", gyro_data[0],gyro_data[1],gyro_data[2]);
fitzpatrick 1:588b597b1c1b 118
fitzpatrick 1:588b597b1c1b 119 // RMS Value of Accelerometer
fitzpatrick 1:588b597b1c1b 120 R = sqrt(std::pow(adata[0], 2) + std::pow(adata[1], 2) + std::pow(adata[2], 2));
fitzpatrick 1:588b597b1c1b 121
fitzpatrick 1:588b597b1c1b 122 // Kalman Filter
fitzpatrick 1:588b597b1c1b 123 kalman_predict(&filter_pitch, gyro_data[0], (ProgramTimer.read_us() - timer)); // Predict uses Pitch portion of gyro and delta time
fitzpatrick 1:588b597b1c1b 124 kalman_update(&filter_pitch, acos(adata[0]/R)); //Update uses normalized Pitch portion of accelerometer
fitzpatrick 1:588b597b1c1b 125 kalman_predict(&filter_roll, gyro_data[1], (ProgramTimer.read_us() - timer)); // Predict uses Roll portion of gyro and delta time
fitzpatrick 1:588b597b1c1b 126 kalman_update(&filter_roll, acos(adata[1]/R)); //Update uses normalized Roll portion of accelerometer
fitzpatrick 1:588b597b1c1b 127
fitzpatrick 1:588b597b1c1b 128 angle[0] = kalman_get_angle(&filter_pitch); // Kalman Pitch
fitzpatrick 1:588b597b1c1b 129 angle[1] = kalman_get_angle(&filter_roll); // Kalman Roll
fitzpatrick 1:588b597b1c1b 130
fitzpatrick 1:588b597b1c1b 131 timer = ProgramTimer.read_us(); // Update Timer
fitzpatrick 1:588b597b1c1b 132
fitzpatrick 1:588b597b1c1b 133 printf("\r\n Roll Angle X: %.6f Pitch Angle Y: %.6f \r\n", Rad2Degree * angle[1], Rad2Degree * angle[0]);
fitzpatrick 1:588b597b1c1b 134
fitzpatrick 1:588b597b1c1b 135 if (Loop_Count > 0)
fitzpatrick 1:588b597b1c1b 136 {
fitzpatrick 1:588b597b1c1b 137 // Loop Iterations to Initialize Kalman Filter to sync.
fitzpatrick 1:588b597b1c1b 138 Loop_Count--;
fitzpatrick 1:588b597b1c1b 139 printf("\n\rLoop_Count = %d\n\r", Loop_Count);
fitzpatrick 1:588b597b1c1b 140 }
fitzpatrick 1:588b597b1c1b 141 else
fitzpatrick 1:588b597b1c1b 142 {
fitzpatrick 1:588b597b1c1b 143 // Loop Iterations Complete, Apply Results of Kalman Filter
fitzpatrick 1:588b597b1c1b 144 // PID Controller (Mostly Proportional)
fitzpatrick 1:588b597b1c1b 145 Roll_Error = (Rad2Degree * angle[1]) - 90; // Calculate Roll Error
fitzpatrick 1:588b597b1c1b 146 Roll_D = Roll_Error - Roll_P; // Differentiation of Roll Error
fitzpatrick 1:588b597b1c1b 147 Roll_I = Roll_I + Roll_Error; // Intigration of Roll Error
fitzpatrick 1:588b597b1c1b 148 Roll_P = Roll_Error; // Proportional Roll Error
fitzpatrick 1:588b597b1c1b 149 Roll_Correct = Roll_P * Roll_P_C + Roll_I * Roll_I_C + Roll_D * Roll_D_C; // Sum product of PID coefficients and values
fitzpatrick 1:588b597b1c1b 150 printf("\n\rRoll_Correct = %f\n\r",Roll_Correct);
fitzpatrick 1:588b597b1c1b 151
fitzpatrick 1:588b597b1c1b 152 Roll_pulse = Roll_pulse + Roll_Correct; // Update Roll Servo PWM Value
fitzpatrick 1:588b597b1c1b 153 if (Roll_pulse <= ROLL_SERVO_MIN) Roll_pulse = ROLL_SERVO_MIN;
fitzpatrick 1:588b597b1c1b 154 if (Roll_pulse >= ROLL_SERVO_MAX) Roll_pulse = ROLL_SERVO_MAX;
fitzpatrick 1:588b597b1c1b 155 Roll_M.pulsewidth_us(Roll_pulse); // servo position determined by a pulsewidth between 1-2ms
fitzpatrick 1:588b597b1c1b 156 printf("\n\rRoll_pulse = %d\n\r",Roll_pulse);
fitzpatrick 1:588b597b1c1b 157
fitzpatrick 1:588b597b1c1b 158 Pitch_Error = (Rad2Degree * angle[0]) - 90; // Calculate Pitch Error
fitzpatrick 1:588b597b1c1b 159 Pitch_D = Pitch_Error - Pitch_P; // Differentiation of Pitch Error
fitzpatrick 1:588b597b1c1b 160 Pitch_I = Pitch_I + Pitch_Error; // Intigration of Pitch Error
fitzpatrick 1:588b597b1c1b 161 Pitch_P = Pitch_Error; // Proportional Pitch Error
fitzpatrick 1:588b597b1c1b 162 Pitch_Correct = Pitch_P * Pitch_P_C + Pitch_I * Pitch_I_C + Pitch_D * Pitch_D_C; // Sum product of PID coefficients and values
fitzpatrick 1:588b597b1c1b 163 printf("\n\rPitch_Correct = %f\n\r",Pitch_Correct);
fitzpatrick 1:588b597b1c1b 164
fitzpatrick 1:588b597b1c1b 165 Pitch_pulse = Pitch_pulse + Pitch_Correct; // Update Roll Servo PWM Value
fitzpatrick 1:588b597b1c1b 166 if (Pitch_pulse <= PITCH_SERVO_MIN) Pitch_pulse = PITCH_SERVO_MIN;
fitzpatrick 1:588b597b1c1b 167 if (Pitch_pulse >= PITCH_SERVO_MAX) Pitch_pulse = PITCH_SERVO_MAX;
fitzpatrick 1:588b597b1c1b 168 Pitch_M.pulsewidth_us(Pitch_pulse); // servo position determined by a pulsewidth between 1-2ms
fitzpatrick 1:588b597b1c1b 169 printf("\n\rPitch_pulse = %d\n\r",Pitch_pulse);
fitzpatrick 1:588b597b1c1b 170 }
fitzpatrick 1:588b597b1c1b 171
fitzpatrick 1:588b597b1c1b 172 wait(0.01); // Loop timer
fitzpatrick 1:588b597b1c1b 173 }
fitzpatrick 1:588b597b1c1b 174
MACRUM 0:21d86aae6b2a 175 }
MACRUM 0:21d86aae6b2a 176
fitzpatrick 1:588b597b1c1b 177