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

Committer:
mikkelbredholt
Date:
Thu Oct 06 12:10:51 2022 +0000
Revision:
13:ddff4bb7c24f
Parent:
9:7b9094864268
Child:
14:12fb3e326911
LED Control, fixed

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