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@8:5640c8c5088e, 2022-10-06 (annotated)

Committer:
uld
Date:
Thu Oct 06 11:26:16 2022 +0000
Revision:
8:5640c8c5088e
Parent:
7:ac88c8e35048
Child:
9:7b9094864268
Optimized code for batteri

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
magnusmland 0:f562e4f9c29f 4 m3pi m3pi;
magnusmland 0:f562e4f9c29f 5
magnusmland 0:f562e4f9c29f 6 // Minimum and maximum motor speeds
magnusmland 0:f562e4f9c29f 7 #define MAX 1.0
magnusmland 0:f562e4f9c29f 8 #define MIN 0
magnusmland 0:f562e4f9c29f 9
magnusmland 0:f562e4f9c29f 10 // PID terms
magnusmland 0:f562e4f9c29f 11 #define P_TERM 1
magnusmland 0:f562e4f9c29f 12 #define I_TERM 0
magnusmland 0:f562e4f9c29f 13 #define D_TERM 20
magnusmland 0:f562e4f9c29f 14
uld 7:ac88c8e35048 15 // Prototypes
uld 8:5640c8c5088e 16 int PitTest(void); // Test if to robot needs to goto pit
uld 5:dbd32cb3650a 17
magnusmland 0:f562e4f9c29f 18 int main() {
uld 1:5431d59ee324 19
magnusmland 0:f562e4f9c29f 20 m3pi.sensor_auto_calibrate();
uld 5:dbd32cb3650a 21
uld 6:6865930c1135 22 /*Base program Variable initiation*/
magnusmland 0:f562e4f9c29f 23 float right;
magnusmland 0:f562e4f9c29f 24 float left;
magnusmland 0:f562e4f9c29f 25 float current_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 26 float previous_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 27 float derivative,proportional,integral = 0;
magnusmland 0:f562e4f9c29f 28 float power;
magnusmland 0:f562e4f9c29f 29 float speed = MAX;
uld 6:6865930c1135 30
uld 6:6865930c1135 31 /*Team 7 Variabels*/
uld 5:dbd32cb3650a 32
uld 7:ac88c8e35048 33 int gotoPit = 0; // wether or not the robot is heading to pit. Initialstate false.
uld 7:ac88c8e35048 34 int ccount; //used to count cycles
uld 6:6865930c1135 35
uld 7:ac88c8e35048 36
uld 6:6865930c1135 37
uld 6:6865930c1135 38 /*Printing secret cat mission*/
uld 6:6865930c1135 39 m3pi.cls();
uld 6:6865930c1135 40
uld 6:6865930c1135 41 m3pi.locate(0,0);
uld 6:6865930c1135 42 m3pi.printf("eliminate");
uld 6:6865930c1135 43 m3pi.locate(0,1);
uld 6:6865930c1135 44 m3pi.printf("all cats");
uld 6:6865930c1135 45 wait(200.0);
uld 6:6865930c1135 46
uld 5:dbd32cb3650a 47 m3pi.cls();
uld 5:dbd32cb3650a 48 m3pi.locate(0,0);
uld 7:ac88c8e35048 49 m3pi.printf("%f.3 ",m3pi.battery());
uld 5:dbd32cb3650a 50 m3pi.locate(0,1);
uld 7:ac88c8e35048 51 m3pi.printf("%f.3 ",m3pi.pot_voltage());
uld 6:6865930c1135 52 wait(200.0);
uld 6:6865930c1135 53 m3pi.cls();
uld 5:dbd32cb3650a 54
uld 7:ac88c8e35048 55
uld 6:6865930c1135 56
magnusmland 0:f562e4f9c29f 57 while (1) {
uld 8:5640c8c5088e 58 /* If cycle count divided by 100 does not have a rest. test if pit */
uld 7:ac88c8e35048 59 if (ccount %100 == 0 && gotoPit=0){
uld 8:5640c8c5088e 60 gotoPit = PitTest(void);
uld 6:6865930c1135 61 }
uld 7:ac88c8e35048 62
magnusmland 0:f562e4f9c29f 63 // Get the position of the line.
magnusmland 0:f562e4f9c29f 64 current_pos_of_line = m3pi.line_position();
magnusmland 0:f562e4f9c29f 65 proportional = current_pos_of_line;
magnusmland 0:f562e4f9c29f 66
magnusmland 0:f562e4f9c29f 67 // Compute the derivative
magnusmland 0:f562e4f9c29f 68 derivative = current_pos_of_line - previous_pos_of_line;
magnusmland 0:f562e4f9c29f 69
magnusmland 0:f562e4f9c29f 70 // Compute the integral
magnusmland 0:f562e4f9c29f 71 integral += proportional;
magnusmland 0:f562e4f9c29f 72
magnusmland 0:f562e4f9c29f 73 // Remember the last position.
magnusmland 0:f562e4f9c29f 74 previous_pos_of_line = current_pos_of_line;
magnusmland 0:f562e4f9c29f 75
magnusmland 0:f562e4f9c29f 76 // Compute the power
magnusmland 0:f562e4f9c29f 77 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
magnusmland 0:f562e4f9c29f 78
magnusmland 0:f562e4f9c29f 79 // Compute new speeds
magnusmland 0:f562e4f9c29f 80 right = speed+power;
magnusmland 0:f562e4f9c29f 81 left = speed-power;
magnusmland 0:f562e4f9c29f 82
magnusmland 0:f562e4f9c29f 83 // limit checks
magnusmland 0:f562e4f9c29f 84 if (right < MIN)
magnusmland 0:f562e4f9c29f 85 right = MIN;
magnusmland 0:f562e4f9c29f 86 else if (right > MAX)
magnusmland 0:f562e4f9c29f 87 right = MAX;
magnusmland 0:f562e4f9c29f 88
magnusmland 0:f562e4f9c29f 89 if (left < MIN)
magnusmland 0:f562e4f9c29f 90 left = MIN;
magnusmland 0:f562e4f9c29f 91 else if (left > MAX)
magnusmland 0:f562e4f9c29f 92 left = MAX;
magnusmland 0:f562e4f9c29f 93
magnusmland 0:f562e4f9c29f 94 // set speed
magnusmland 0:f562e4f9c29f 95 m3pi.left_motor(left);
magnusmland 0:f562e4f9c29f 96 m3pi.right_motor(right);
uld 6:6865930c1135 97
uld 8:5640c8c5088e 98 ccount++;
uld 7:ac88c8e35048 99 }
uld 7:ac88c8e35048 100 }
uld 7:ac88c8e35048 101
uld 7:ac88c8e35048 102
uld 8:5640c8c5088e 103 int PitTest(void){
uld 7:ac88c8e35048 104 /* Test the batteri voltage if the robot is not headed for pit */
uld 7:ac88c8e35048 105
uld 8:5640c8c5088e 106 const float BATVOLTTRESHOLD = 3.0; // Treshold i volt
uld 8:5640c8c5088e 107 int result;
uld 7:ac88c8e35048 108
uld 7:ac88c8e35048 109 /*Digital outs*/
uld 7:ac88c8e35048 110 DigitalOut led1(LED1);
uld 7:ac88c8e35048 111 led1 = 0; // Turn off led 1 on the embed
uld 7:ac88c8e35048 112
uld 7:ac88c8e35048 113 /*Test if the voltage is below the treshold if so turn on go to pit mode*/
uld 8:5640c8c5088e 114 if (m3pi.battery() <= BATVOLTTRESHOLD ()){
uld 8:5640c8c5088e 115 result = 1; // set goto pit condition
uld 8:5640c8c5088e 116 led1 = 1; // turn on Led 1
uld 7:ac88c8e35048 117 m3pi.cls();
uld 7:ac88c8e35048 118 m3pi.locate(0,0);
uld 7:ac88c8e35048 119 m3pi.printf("Going to");
uld 7:ac88c8e35048 120 m3pi.locate(0,1);
uld 7:ac88c8e35048 121 m3pi.printf("PIT Party");
magnusmland 0:f562e4f9c29f 122 }
uld 8:5640c8c5088e 123 return result;
magnusmland 0:f562e4f9c29f 124 }