Seb Pol / Mbed 2 deprecated Roboter

LED.h & Umwandeln.h

Dependencies:   m3pi_ng mbed

basic2.h@0:d453d7554981, 2017-01-11 (annotated)

Committer:
spolak
Date:
Wed Jan 11 09:43:00 2017 +0000
Revision:
0:d453d7554981
Klassen zum Blinken und Umwandeln vom Text

Who changed what in which revision?

UserRevisionLine numberNew contents of line
spolak 0:d453d7554981 1 #include "mbed.h"
spolak 0:d453d7554981 2 #include "m3pi_ng.h"
spolak 0:d453d7554981 3
spolak 0:d453d7554981 4 m3pi m3pi;
spolak 0:d453d7554981 5
spolak 0:d453d7554981 6 // Minimum and maximum motor speeds
spolak 0:d453d7554981 7 #define MAX 1.0
spolak 0:d453d7554981 8 #define MIN 0
spolak 0:d453d7554981 9
spolak 0:d453d7554981 10 // PID terms
spolak 0:d453d7554981 11 #define P_TERM 1
spolak 0:d453d7554981 12 #define I_TERM 0
spolak 0:d453d7554981 13 #define D_TERM 20
spolak 0:d453d7554981 14
spolak 0:d453d7554981 15 int main() {
spolak 0:d453d7554981 16
spolak 0:d453d7554981 17 m3pi.locate(0,1);
spolak 0:d453d7554981 18 m3pi.printf("Line PID");
spolak 0:d453d7554981 19
spolak 0:d453d7554981 20 wait(2.0);
spolak 0:d453d7554981 21
spolak 0:d453d7554981 22 m3pi.sensor_auto_calibrate();
spolak 0:d453d7554981 23
spolak 0:d453d7554981 24 float right;
spolak 0:d453d7554981 25 float left;
spolak 0:d453d7554981 26 float current_pos_of_line = 0.0;
spolak 0:d453d7554981 27 float previous_pos_of_line = 0.0;
spolak 0:d453d7554981 28 float derivative,proportional,integral = 0;
spolak 0:d453d7554981 29 float power;
spolak 0:d453d7554981 30 float speed = MAX;
spolak 0:d453d7554981 31
spolak 0:d453d7554981 32 while (1) {
spolak 0:d453d7554981 33
spolak 0:d453d7554981 34 // Get the position of the line.
spolak 0:d453d7554981 35 current_pos_of_line = m3pi.line_position();
spolak 0:d453d7554981 36 proportional = current_pos_of_line;
spolak 0:d453d7554981 37
spolak 0:d453d7554981 38 // Compute the derivative
spolak 0:d453d7554981 39 derivative = current_pos_of_line - previous_pos_of_line;
spolak 0:d453d7554981 40
spolak 0:d453d7554981 41 // Compute the integral
spolak 0:d453d7554981 42 integral += proportional;
spolak 0:d453d7554981 43
spolak 0:d453d7554981 44 // Remember the last position.
spolak 0:d453d7554981 45 previous_pos_of_line = current_pos_of_line;
spolak 0:d453d7554981 46
spolak 0:d453d7554981 47 // Compute the power
spolak 0:d453d7554981 48 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
spolak 0:d453d7554981 49
spolak 0:d453d7554981 50 // Compute new speeds
spolak 0:d453d7554981 51 right = speed+power;
spolak 0:d453d7554981 52 left = speed-power;
spolak 0:d453d7554981 53
spolak 0:d453d7554981 54 // limit checks
spolak 0:d453d7554981 55 if (right < MIN)
spolak 0:d453d7554981 56 right = MIN;
spolak 0:d453d7554981 57 else if (right > MAX)
spolak 0:d453d7554981 58 right = MAX;
spolak 0:d453d7554981 59
spolak 0:d453d7554981 60 if (left < MIN)
spolak 0:d453d7554981 61 left = MIN;
spolak 0:d453d7554981 62 else if (left > MAX)
spolak 0:d453d7554981 63 left = MAX;
spolak 0:d453d7554981 64
spolak 0:d453d7554981 65 // set speed
spolak 0:d453d7554981 66 m3pi.left_motor(left);
spolak 0:d453d7554981 67 m3pi.right_motor(right);
spolak 0:d453d7554981 68
spolak 0:d453d7554981 69 }
spolak 0:d453d7554981 70 }