Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
1 contributor

Users who have contributed to this file

124 lines (99 sloc) 3.7 KB
/*
* ODE solver
*
* This code implements the runge-kutta (4th order) algorithm to solve a system of
* ordinary differential equations (ODEs). The code requires a given time step dt
* and will continue indefinitely as long as the arduino runs (unless an exit condition
* is added).
*
* The ODE needs to be implemented in state-representation. Instead of Matlab convention
* y'=f(y,t), the function ODE(t,y,y_p) is used, where t is a given time, y the state
* vector and y_p the array which will be updated.
*
* Created: April 2015, Daniel Heinrich
* Updated: August 2016, Daniel Heinrich: changed data type to double,
* fixed errors on numerical constants (2.0 instead of 2)
* released as CC BY-SA 4.0 licence, see https://creativecommons.org/licenses/by-sa/4.0/
*
* Data type considerations: on most arduinos except due, double is treated as float
* (4-bit precision). On a due, double is 8-bit precision. If you want to use float on
* a due, open the sketch in a text editor, hit STRG+H and replace double with float.
* (you can also use #define to swich data types easily, but it makes the code less
* readable so I decided against it)
* Also, on a due, values entered numerically (e.g. "0.5") are automatically treated as
* double. To get float, (float)0.5 needs to be used (improves speed by ~5% for float).
*
* calculation time for the 1000 steps with the ODE included below:
* arduino UNO / Nano: ~760 ms (~ 1300 steps / second)
* arduino due: ~140 ms (~ 7000 steps / second)
* arduion due with float (see comments above): ~115 ms (~8700 steps / second)
*/
const double dt=0.001; // Timestep used by the algorithm
const int n=4; // number of state variables in the ODE
int steps=0; // number of calculated steps
double t=0; // current time
double Y_current[n]={1, 0, 0, 0}; // Initial state of the ODE
double Y_temp[n]; // temp array for the ODE function output.
double K1[n], K2[n], K3[n], K4[n]; // arrays for the Runge-Kutta intermediate points
// ODE parameters:
const double c1=1;
const double c2=1;
const double d1=0.01;
const double d2=0.01;
// Debugging parameters:
long time_lastprintout = millis();
void setup() {
// initialize Serial to output the integrator results.
Serial.begin(9600);
}
void loop() {
// calculate another timestep: t0 --> t1
// K1 = Y_p(t0, Y_current)
ODE(t, Y_current, K1);
// K2 = Y_p(t0 + dt/2, Y_current + dt/2 * K1)
for (int i=0; i<n; i++) {
Y_temp[i] = Y_current[i] + K1[i]*dt*0.5;
}
ODE(t + dt*0.5, Y_temp, K2);
// K3 = Y_p(t0 + dt/2, Y_current + dt/2 * K2)
for (int i=0; i<n; i++) {
Y_temp[i] = Y_current[i] + K2[i]*dt*0.5;
}
ODE(t + dt*0.5, Y_temp, K3);
// K4 = Y_p(t0 + dt, Y_current + dt * K3)
for (int i=0; i<n; i++) {
Y_temp[i] = Y_current[i] + K3[i]*dt;
}
ODE(t + dt, Y_temp, K4);
// Calculate new step:
// Y_nextstep = Y_current + dt/6 * (K1 + 2*(K2+K3) + K4)
for (int i=0; i<n; i++) {
Y_current[i] = Y_current[i] + dt / 6.0 * (K1[i] + 2.0*(K2[i] + K3[i]) + K4[i]);
}
t=t+dt;
steps++;
// Print output each X calculated steps:
if (steps>=1000) {
Serial.print("state at ");
Serial.print(t);
Serial.print("s: ");
for (int i=0; i<n; i++) {
Serial.print(Y_current[i]);
Serial.print(" ");
}
Serial.print(" calculation time: ");
Serial.print(millis()-time_lastprintout);
time_lastprintout=millis();
Serial.println();
steps=0;
}
//delay(10);
}
void ODE(double t, double Y[], double Y_p[]) {
// simple 2-mass oscillator:
// y = pos1, pos2, vel1, vel2
Y_p[0]=Y[2];
Y_p[1]=Y[3];
Y_p[2]=-Y[0]*c1 + (Y[1]-Y[0])*c2 - Y[2]*d1;
Y_p[3]=-(Y[1]-Y[0])*c2 - Y[3]*d2;
}
You can’t perform that action at this time.