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@6:6865930c1135, 2022-10-04 (annotated)

Committer:
uld
Date:
Tue Oct 04 12:09:19 2022 +0000
Revision:
6:6865930c1135
Parent:
5:dbd32cb3650a
Child:
7:ac88c8e35048
ADD first draft of battery test code

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
uld 5:dbd32cb3650a 16 // Tresholds
uld 5:dbd32cb3650a 17 #define BATVOLTRESHOLD 3.0
uld 5:dbd32cb3650a 18
magnusmland 0:f562e4f9c29f 19 int main() {
uld 1:5431d59ee324 20
magnusmland 0:f562e4f9c29f 21 m3pi.sensor_auto_calibrate();
uld 5:dbd32cb3650a 22
uld 6:6865930c1135 23 /*Base program Variable initiation*/
magnusmland 0:f562e4f9c29f 24 float right;
magnusmland 0:f562e4f9c29f 25 float left;
magnusmland 0:f562e4f9c29f 26 float current_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 27 float previous_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 28 float derivative,proportional,integral = 0;
magnusmland 0:f562e4f9c29f 29 float power;
magnusmland 0:f562e4f9c29f 30 float speed = MAX;
uld 6:6865930c1135 31
uld 6:6865930c1135 32 /*Team 7 Variabels*/
uld 5:dbd32cb3650a 33 float batVol = m3pi.battery(); //Storing battery voltage in variable
uld 5:dbd32cb3650a 34 float potVol = m3pi.pot_voltage(); //Storing pot voltage
uld 6:6865930c1135 35 int gotopit = 0; // variabel storing weather or not the robot is heading to pit
uld 5:dbd32cb3650a 36
uld 6:6865930c1135 37 /*Digital outs*/
uld 6:6865930c1135 38 DigitalOut led1(LED1);
uld 6:6865930c1135 39
uld 6:6865930c1135 40
uld 6:6865930c1135 41 /*Printing secret cat mission*/
uld 6:6865930c1135 42 m3pi.cls();
uld 6:6865930c1135 43
uld 6:6865930c1135 44 m3pi.locate(0,0);
uld 6:6865930c1135 45 m3pi.printf("eliminate");
uld 6:6865930c1135 46 m3pi.locate(0,1);
uld 6:6865930c1135 47 m3pi.printf("all cats");
uld 6:6865930c1135 48 wait(200.0);
uld 6:6865930c1135 49
uld 5:dbd32cb3650a 50 m3pi.cls();
uld 5:dbd32cb3650a 51 m3pi.locate(0,0);
uld 5:dbd32cb3650a 52 m3pi.printf("%f.3 ",batVol);
uld 5:dbd32cb3650a 53 m3pi.locate(0,1);
uld 5:dbd32cb3650a 54 m3pi.printf("%f.3 ",b);
uld 6:6865930c1135 55 wait(200.0);
uld 6:6865930c1135 56 m3pi.cls();
uld 5:dbd32cb3650a 57
uld 6:6865930c1135 58 /*Inital state*/
uld 6:6865930c1135 59 led1 = 0; // Turn off led 1 on the embed
uld 6:6865930c1135 60
magnusmland 0:f562e4f9c29f 61 while (1) {
uld 6:6865930c1135 62
uld 6:6865930c1135 63 /* Test the batteri voltage if the robot is not headed for pit */
uld 6:6865930c1135 64 if (gotopit == 0) {
uld 6:6865930c1135 65 /*Sets the battery voltage to the current voltage of the battery*/
uld 6:6865930c1135 66 batVol = battery ();
uld 6:6865930c1135 67
uld 6:6865930c1135 68 if batteryVoltage <= BATVOLTRESHOLD (){
uld 6:6865930c1135 69 gotopit = 1; // set goto pit condition
uld 6:6865930c1135 70 led1 = 1; // trun on Led 1
uld 6:6865930c1135 71 }
uld 6:6865930c1135 72
magnusmland 0:f562e4f9c29f 73 // Get the position of the line.
magnusmland 0:f562e4f9c29f 74 current_pos_of_line = m3pi.line_position();
magnusmland 0:f562e4f9c29f 75 proportional = current_pos_of_line;
magnusmland 0:f562e4f9c29f 76
magnusmland 0:f562e4f9c29f 77 // Compute the derivative
magnusmland 0:f562e4f9c29f 78 derivative = current_pos_of_line - previous_pos_of_line;
magnusmland 0:f562e4f9c29f 79
magnusmland 0:f562e4f9c29f 80 // Compute the integral
magnusmland 0:f562e4f9c29f 81 integral += proportional;
magnusmland 0:f562e4f9c29f 82
magnusmland 0:f562e4f9c29f 83 // Remember the last position.
magnusmland 0:f562e4f9c29f 84 previous_pos_of_line = current_pos_of_line;
magnusmland 0:f562e4f9c29f 85
magnusmland 0:f562e4f9c29f 86 // Compute the power
magnusmland 0:f562e4f9c29f 87 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
magnusmland 0:f562e4f9c29f 88
magnusmland 0:f562e4f9c29f 89 // Compute new speeds
magnusmland 0:f562e4f9c29f 90 right = speed+power;
magnusmland 0:f562e4f9c29f 91 left = speed-power;
magnusmland 0:f562e4f9c29f 92
magnusmland 0:f562e4f9c29f 93 // limit checks
magnusmland 0:f562e4f9c29f 94 if (right < MIN)
magnusmland 0:f562e4f9c29f 95 right = MIN;
magnusmland 0:f562e4f9c29f 96 else if (right > MAX)
magnusmland 0:f562e4f9c29f 97 right = MAX;
magnusmland 0:f562e4f9c29f 98
magnusmland 0:f562e4f9c29f 99 if (left < MIN)
magnusmland 0:f562e4f9c29f 100 left = MIN;
magnusmland 0:f562e4f9c29f 101 else if (left > MAX)
magnusmland 0:f562e4f9c29f 102 left = MAX;
magnusmland 0:f562e4f9c29f 103
magnusmland 0:f562e4f9c29f 104 // set speed
magnusmland 0:f562e4f9c29f 105 m3pi.left_motor(left);
magnusmland 0:f562e4f9c29f 106 m3pi.right_motor(right);
uld 6:6865930c1135 107
uld 6:6865930c1135 108
magnusmland 0:f562e4f9c29f 109
magnusmland 0:f562e4f9c29f 110 }
magnusmland 0:f562e4f9c29f 111 }