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@7:ac88c8e35048, 2022-10-05 (annotated)

Committer:
uld
Date:
Wed Oct 05 11:54:30 2022 +0000
Revision:
7:ac88c8e35048
Parent:
6:6865930c1135
Child:
8:5640c8c5088e
Child:
10:9b04c532b57b
pit test;

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