Julesnaps / Mbed 2 deprecated Linefollowproject

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   m3pi mbed

main.cpp@4:803dc29393cc, 2022-10-04 (annotated)

Committer:
mikkelbredholt
Date:
Tue Oct 04 11:51:10 2022 +0000
Revision:
4:803dc29393cc
Parent:
3:88a6763a7e44
Parent:
2:5369f34d061e
Child:
5:dbd32cb3650a
Mikkel og Niels

Who changed what in which revision?

UserRevisionLine numberNew contents of line
magnusmland 0:f562e4f9c29f 1 #include "mbed.h"
magnusmland 0:f562e4f9c29f 2 #include "m3pi.h"
magnusmland 0:f562e4f9c29f 3
mikkelbredholt 3:88a6763a7e44 4 //Mikkel har været her
magnusmland 0:f562e4f9c29f 5 m3pi m3pi;
magnusmland 0:f562e4f9c29f 6
magnusmland 0:f562e4f9c29f 7 // Minimum and maximum motor speeds
magnusmland 0:f562e4f9c29f 8 #define MAX 1.0
magnusmland 0:f562e4f9c29f 9 #define MIN 0
magnusmland 0:f562e4f9c29f 10
magnusmland 0:f562e4f9c29f 11 // PID terms
magnusmland 0:f562e4f9c29f 12 #define P_TERM 1
magnusmland 0:f562e4f9c29f 13 #define I_TERM 0
magnusmland 0:f562e4f9c29f 14 #define D_TERM 20
magnusmland 0:f562e4f9c29f 15
magnusmland 0:f562e4f9c29f 16 int main() {
magnusmland 0:f562e4f9c29f 17 float a=m3pi.battery();
magnusmland 0:f562e4f9c29f 18 float b=m3pi.pot_voltage();
magnusmland 0:f562e4f9c29f 19 m3pi.locate(0,0);
magnusmland 0:f562e4f9c29f 20
magnusmland 0:f562e4f9c29f 21
magnusmland 0:f562e4f9c29f 22 m3pi.cls();
magnusmland 0:f562e4f9c29f 23 m3pi.printf("%f.3 ",a);
magnusmland 0:f562e4f9c29f 24 m3pi.locate(0,1);
magnusmland 0:f562e4f9c29f 25 m3pi.printf("%f.3 ",b);
uld 1:5431d59ee324 26
uld 1:5431d59ee324 27
uld 1:5431d59ee324 28 // dette er en mega god kommentar 13:44
uld 2:5369f34d061e 29 // denen er endnu bedre
magnusmland 0:f562e4f9c29f 30
magnusmland 0:f562e4f9c29f 31 wait(2.0);
magnusmland 0:f562e4f9c29f 32
magnusmland 0:f562e4f9c29f 33 m3pi.sensor_auto_calibrate();
magnusmland 0:f562e4f9c29f 34
magnusmland 0:f562e4f9c29f 35 float right;
magnusmland 0:f562e4f9c29f 36 float left;
magnusmland 0:f562e4f9c29f 37 float current_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 38 float previous_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 39 float derivative,proportional,integral = 0;
magnusmland 0:f562e4f9c29f 40 float power;
magnusmland 0:f562e4f9c29f 41 float speed = MAX;
magnusmland 0:f562e4f9c29f 42
magnusmland 0:f562e4f9c29f 43 while (1) {
magnusmland 0:f562e4f9c29f 44
magnusmland 0:f562e4f9c29f 45 // Get the position of the line.
magnusmland 0:f562e4f9c29f 46 current_pos_of_line = m3pi.line_position();
magnusmland 0:f562e4f9c29f 47 proportional = current_pos_of_line;
magnusmland 0:f562e4f9c29f 48
magnusmland 0:f562e4f9c29f 49 // Compute the derivative
magnusmland 0:f562e4f9c29f 50 derivative = current_pos_of_line - previous_pos_of_line;
magnusmland 0:f562e4f9c29f 51
magnusmland 0:f562e4f9c29f 52 // Compute the integral
magnusmland 0:f562e4f9c29f 53 integral += proportional;
magnusmland 0:f562e4f9c29f 54
magnusmland 0:f562e4f9c29f 55 // Remember the last position.
magnusmland 0:f562e4f9c29f 56 previous_pos_of_line = current_pos_of_line;
magnusmland 0:f562e4f9c29f 57
magnusmland 0:f562e4f9c29f 58 // Compute the power
magnusmland 0:f562e4f9c29f 59 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
magnusmland 0:f562e4f9c29f 60
magnusmland 0:f562e4f9c29f 61 // Compute new speeds
magnusmland 0:f562e4f9c29f 62 right = speed+power;
magnusmland 0:f562e4f9c29f 63 left = speed-power;
magnusmland 0:f562e4f9c29f 64
magnusmland 0:f562e4f9c29f 65 // limit checks
magnusmland 0:f562e4f9c29f 66 if (right < MIN)
magnusmland 0:f562e4f9c29f 67 right = MIN;
magnusmland 0:f562e4f9c29f 68 else if (right > MAX)
magnusmland 0:f562e4f9c29f 69 right = MAX;
magnusmland 0:f562e4f9c29f 70
magnusmland 0:f562e4f9c29f 71 if (left < MIN)
magnusmland 0:f562e4f9c29f 72 left = MIN;
magnusmland 0:f562e4f9c29f 73 else if (left > MAX)
magnusmland 0:f562e4f9c29f 74 left = MAX;
magnusmland 0:f562e4f9c29f 75
magnusmland 0:f562e4f9c29f 76 // set speed
magnusmland 0:f562e4f9c29f 77 m3pi.left_motor(left);
magnusmland 0:f562e4f9c29f 78 m3pi.right_motor(right);
magnusmland 0:f562e4f9c29f 79
magnusmland 0:f562e4f9c29f 80 }
magnusmland 0:f562e4f9c29f 81 }