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@9:7b9094864268, 2022-10-06 (annotated)

Committer:
uld
Date:
Thu Oct 06 11:32:01 2022 +0000
Revision:
9:7b9094864268
Parent:
8:5640c8c5088e
Child:
11:d13596393d56
Child:
13:ddff4bb7c24f
Fixed secret cat message

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