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@15:8b76add42254, 2022-10-11 (annotated)

Committer:
vehus
Date:
Tue Oct 11 09:20:15 2022 +0000
Revision:
15:8b76add42254
Parent:
14:12fb3e326911
Child:
17:3fcc43140382
revision of code; Blink function; in pit function

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;
vehus 15:8b76add42254 5 DigitalOut led1(LED1);
vehus 15:8b76add42254 6 DigitalOut led2(LED2);
vehus 15:8b76add42254 7 DigitalOut led3(LED3);
vehus 15:8b76add42254 8 DigitalOut led4(LED4);
magnusmland 0:f562e4f9c29f 9
magnusmland 0:f562e4f9c29f 10 // Minimum and maximum motor speeds
magnusmland 0:f562e4f9c29f 11 #define MAX 1.0
magnusmland 0:f562e4f9c29f 12 #define MIN 0
magnusmland 0:f562e4f9c29f 13
magnusmland 0:f562e4f9c29f 14 // PID terms
magnusmland 0:f562e4f9c29f 15 #define P_TERM 1
magnusmland 0:f562e4f9c29f 16 #define I_TERM 0
magnusmland 0:f562e4f9c29f 17 #define D_TERM 20
magnusmland 0:f562e4f9c29f 18
uld 7:ac88c8e35048 19 // Prototypes
uld 8:5640c8c5088e 20 int PitTest(void); // Test if to robot needs to goto pit
vehus 15:8b76add42254 21 void InitialMessages(void); // Prints initial message to the LCD
vehus 15:8b76add42254 22 void LED_Control(int ledNumber, int state); //turn ledNumber to 1=on, 0 = off
vehus 15:8b76add42254 23 void PitStop(void); //
vehus 15:8b76add42254 24 void Blink(int ledNumber); // make ledNumber blinik
mikkelbredholt 13:ddff4bb7c24f 25
magnusmland 0:f562e4f9c29f 26 int main() {
uld 1:5431d59ee324 27
magnusmland 0:f562e4f9c29f 28 m3pi.sensor_auto_calibrate();
uld 5:dbd32cb3650a 29
uld 6:6865930c1135 30 /*Base program Variable initiation*/
magnusmland 0:f562e4f9c29f 31 float right;
magnusmland 0:f562e4f9c29f 32 float left;
magnusmland 0:f562e4f9c29f 33 float current_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 34 float previous_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 35 float derivative,proportional,integral = 0;
magnusmland 0:f562e4f9c29f 36 float power;
magnusmland 0:f562e4f9c29f 37 float speed = MAX;
uld 6:6865930c1135 38
uld 6:6865930c1135 39 /*Team 7 Variabels*/
uld 7:ac88c8e35048 40 int gotoPit = 0; // wether or not the robot is heading to pit. Initialstate false.
vehus 15:8b76add42254 41 int ccount = 0; //used to count cycles
uld 6:6865930c1135 42
uld 7:ac88c8e35048 43
uld 6:6865930c1135 44
uld 6:6865930c1135 45 /*Printing secret cat mission*/
vehus 15:8b76add42254 46 InitialMessages();
uld 9:7b9094864268 47
uld 7:ac88c8e35048 48
uld 6:6865930c1135 49
magnusmland 0:f562e4f9c29f 50 while (1) {
uld 8:5640c8c5088e 51 /* If cycle count divided by 100 does not have a rest. test if pit */
vehus 15:8b76add42254 52 if (ccount % 100 == 0 && gotoPit == 0)
vehus 15:8b76add42254 53 {
vehus 15:8b76add42254 54 gotoPit = PitTest();
vehus 15:8b76add42254 55 }
vehus 15:8b76add42254 56 if (gotoPit == 1)
vehus 15:8b76add42254 57 {
vehus 15:8b76add42254 58 PitStop ();
vehus 15:8b76add42254 59
vehus 15:8b76add42254 60 }
vehus 15:8b76add42254 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 }
vehus 15:8b76add42254 100
vehus 15:8b76add42254 101
uld 7:ac88c8e35048 102 }
uld 7:ac88c8e35048 103
vehus 15:8b76add42254 104 void InitialMessages(void){
uld 9:7b9094864268 105 /*Prints iniatl secret mission*/
uld 9:7b9094864268 106
uld 9:7b9094864268 107 m3pi.cls();
uld 9:7b9094864268 108 m3pi.locate(0,0);
uld 9:7b9094864268 109 m3pi.printf("eliminate");
uld 9:7b9094864268 110 m3pi.locate(0,1);
uld 9:7b9094864268 111 m3pi.printf("all cats");
uld 9:7b9094864268 112 wait(200.0);
uld 9:7b9094864268 113
uld 9:7b9094864268 114 m3pi.cls();
uld 9:7b9094864268 115 m3pi.locate(0,0);
uld 9:7b9094864268 116 m3pi.printf("%f.3 ",m3pi.battery());
uld 9:7b9094864268 117 m3pi.locate(0,1);
uld 9:7b9094864268 118 m3pi.printf("%f.3 ",m3pi.pot_voltage());
uld 9:7b9094864268 119 wait(200.0);
uld 9:7b9094864268 120 m3pi.cls();
uld 9:7b9094864268 121 }
uld 8:5640c8c5088e 122 int PitTest(void){
uld 7:ac88c8e35048 123 /* Test the batteri voltage if the robot is not headed for pit */
uld 7:ac88c8e35048 124
uld 8:5640c8c5088e 125 const float BATVOLTTRESHOLD = 3.0; // Treshold i volt
vehus 15:8b76add42254 126 int result = 0;
uld 7:ac88c8e35048 127
mikkelbredholt 13:ddff4bb7c24f 128 /*Test if the voltage is below the threshold if so turn on go to pit mode*/
vehus 15:8b76add42254 129 if (m3pi.battery() <= BATVOLTTRESHOLD ){
uld 9:7b9094864268 130 result = 1; // Set goto pit condition
vehus 15:8b76add42254 131 LED_Control(1, 1);
uld 7:ac88c8e35048 132 m3pi.cls();
uld 7:ac88c8e35048 133 m3pi.locate(0,0);
uld 7:ac88c8e35048 134 m3pi.printf("Going to");
uld 7:ac88c8e35048 135 m3pi.locate(0,1);
uld 7:ac88c8e35048 136 m3pi.printf("PIT Party");
magnusmland 0:f562e4f9c29f 137 }
uld 8:5640c8c5088e 138 return result;
mikkelbredholt 13:ddff4bb7c24f 139 }
mikkelbredholt 13:ddff4bb7c24f 140
mikkelbredholt 13:ddff4bb7c24f 141 void LED_Control(int ledNumber, int state){
mikkelbredholt 13:ddff4bb7c24f 142 //LED1 on if robot is looking for pit
vehus 15:8b76add42254 143 if (ledNumber == 1)
vehus 15:8b76add42254 144 {
vehus 15:8b76add42254 145 led1 = state;
vehus 15:8b76add42254 146 }
vehus 15:8b76add42254 147 if (ledNumber == 2)
vehus 15:8b76add42254 148 {
vehus 15:8b76add42254 149 led2 = state;
mikkelbredholt 13:ddff4bb7c24f 150 }
vehus 15:8b76add42254 151 if (ledNumber == 3)
vehus 15:8b76add42254 152 {
vehus 15:8b76add42254 153 led3 = state;
vehus 15:8b76add42254 154 }
vehus 15:8b76add42254 155 if (ledNumber == 4)
vehus 15:8b76add42254 156 {
vehus 15:8b76add42254 157 led4 = state;
mikkelbredholt 13:ddff4bb7c24f 158 }
vehus 15:8b76add42254 159 }
vehus 15:8b76add42254 160
vehus 15:8b76add42254 161 void Blink(int ledNumber)
vehus 15:8b76add42254 162 {
vehus 15:8b76add42254 163 int a = 2;
vehus 15:8b76add42254 164 LED_Control (ledNumber , 0);
vehus 15:8b76add42254 165 wait(a);
vehus 15:8b76add42254 166 LED_Control (ledNumber , 1);
vehus 15:8b76add42254 167 wait(a);
mikkelbredholt 13:ddff4bb7c24f 168 }
vehus 15:8b76add42254 169
vehus 15:8b76add42254 170 void PitStop(void)
vehus 15:8b76add42254 171 {
vehus 15:8b76add42254 172 /* Testing alternative stop function
vehus 15:8b76add42254 173 m3pi.left_motor(0);
vehus 15:8b76add42254 174 m3pi.right_motor(0);
vehus 15:8b76add42254 175 */
vehus 15:8b76add42254 176 m3pi.stop(); // stop all engine
vehus 15:8b76add42254 177
vehus 15:8b76add42254 178 // increase counter with one
vehus 15:8b76add42254 179 while (1)
vehus 15:8b76add42254 180 {
vehus 15:8b76add42254 181 Blink (1); // signal in pit
vehus 15:8b76add42254 182
vehus 15:8b76add42254 183 /* missing input to stop blink. */
vehus 15:8b76add42254 184
mikkelbredholt 13:ddff4bb7c24f 185 }
magnusmland 0:f562e4f9c29f 186 }