Joachim Schaeffer
/
Roboter
g
Fork of Roboter by
Diff: basic2.h
- Revision:
- 0:d453d7554981
diff -r 000000000000 -r d453d7554981 basic2.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/basic2.h Wed Jan 11 09:43:00 2017 +0000 @@ -0,0 +1,70 @@ +#include "mbed.h" +#include "m3pi_ng.h" + +m3pi m3pi; + +// Minimum and maximum motor speeds +#define MAX 1.0 +#define MIN 0 + +// PID terms +#define P_TERM 1 +#define I_TERM 0 +#define D_TERM 20 + +int main() { + + m3pi.locate(0,1); + m3pi.printf("Line PID"); + + wait(2.0); + + m3pi.sensor_auto_calibrate(); + + float right; + float left; + float current_pos_of_line = 0.0; + float previous_pos_of_line = 0.0; + float derivative,proportional,integral = 0; + float power; + float speed = MAX; + + while (1) { + + // Get the position of the line. + current_pos_of_line = m3pi.line_position(); + proportional = current_pos_of_line; + + // Compute the derivative + derivative = current_pos_of_line - previous_pos_of_line; + + // Compute the integral + integral += proportional; + + // Remember the last position. + previous_pos_of_line = current_pos_of_line; + + // Compute the power + power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; + + // Compute new speeds + right = speed+power; + left = speed-power; + + // limit checks + if (right < MIN) + right = MIN; + else if (right > MAX) + right = MAX; + + if (left < MIN) + left = MIN; + else if (left > MAX) + left = MAX; + + // set speed + m3pi.left_motor(left); + m3pi.right_motor(right); + + } +} \ No newline at end of file