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@17:3fcc43140382, 2022-10-11 (annotated)

Committer:
uld
Date:
Tue Oct 11 10:52:30 2022 +0000
Revision:
17:3fcc43140382
Parent:
16:ef72e302c653
Parent:
15:8b76add42254
Child:
18:91e521f1f0f4
??

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
uld 17:3fcc43140382 23 <<<<<<< working copy
uld 17:3fcc43140382 24 =======
vehus 15:8b76add42254 25 void PitStop(void); //
vehus 15:8b76add42254 26 void Blink(int ledNumber); // make ledNumber blinik
uld 17:3fcc43140382 27 >>>>>>> merge rev
mikkelbredholt 13:ddff4bb7c24f 28
magnusmland 0:f562e4f9c29f 29 int main() {
uld 1:5431d59ee324 30
magnusmland 0:f562e4f9c29f 31 m3pi.sensor_auto_calibrate();
uld 5:dbd32cb3650a 32
uld 6:6865930c1135 33 /*Base program Variable initiation*/
magnusmland 0:f562e4f9c29f 34 float right;
magnusmland 0:f562e4f9c29f 35 float left;
magnusmland 0:f562e4f9c29f 36 float current_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 37 float previous_pos_of_line = 0.0;
magnusmland 0:f562e4f9c29f 38 float derivative,proportional,integral = 0;
magnusmland 0:f562e4f9c29f 39 float power;
magnusmland 0:f562e4f9c29f 40 float speed = MAX;
uld 6:6865930c1135 41
uld 6:6865930c1135 42 /*Team 7 Variabels*/
uld 7:ac88c8e35048 43 int gotoPit = 0; // wether or not the robot is heading to pit. Initialstate false.
vehus 15:8b76add42254 44 int ccount = 0; //used to count cycles
uld 6:6865930c1135 45
uld 7:ac88c8e35048 46
uld 6:6865930c1135 47
uld 6:6865930c1135 48 /*Printing secret cat mission*/
vehus 15:8b76add42254 49 InitialMessages();
uld 9:7b9094864268 50
uld 7:ac88c8e35048 51
uld 6:6865930c1135 52
magnusmland 0:f562e4f9c29f 53 while (1) {
uld 8:5640c8c5088e 54 /* If cycle count divided by 100 does not have a rest. test if pit */
vehus 15:8b76add42254 55 if (ccount % 100 == 0 && gotoPit == 0)
vehus 15:8b76add42254 56 {
vehus 15:8b76add42254 57 gotoPit = PitTest();
vehus 15:8b76add42254 58 }
vehus 15:8b76add42254 59 if (gotoPit == 1)
vehus 15:8b76add42254 60 {
vehus 15:8b76add42254 61 PitStop ();
vehus 15:8b76add42254 62
vehus 15:8b76add42254 63 }
vehus 15:8b76add42254 64
uld 7:ac88c8e35048 65
magnusmland 0:f562e4f9c29f 66 // Get the position of the line.
magnusmland 0:f562e4f9c29f 67 current_pos_of_line = m3pi.line_position();
magnusmland 0:f562e4f9c29f 68 proportional = current_pos_of_line;
magnusmland 0:f562e4f9c29f 69
magnusmland 0:f562e4f9c29f 70 // Compute the derivative
magnusmland 0:f562e4f9c29f 71 derivative = current_pos_of_line - previous_pos_of_line;
magnusmland 0:f562e4f9c29f 72
magnusmland 0:f562e4f9c29f 73 // Compute the integral
magnusmland 0:f562e4f9c29f 74 integral += proportional;
magnusmland 0:f562e4f9c29f 75
magnusmland 0:f562e4f9c29f 76 // Remember the last position.
magnusmland 0:f562e4f9c29f 77 previous_pos_of_line = current_pos_of_line;
magnusmland 0:f562e4f9c29f 78
magnusmland 0:f562e4f9c29f 79 // Compute the power
magnusmland 0:f562e4f9c29f 80 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
magnusmland 0:f562e4f9c29f 81
magnusmland 0:f562e4f9c29f 82 // Compute new speeds
magnusmland 0:f562e4f9c29f 83 right = speed+power;
magnusmland 0:f562e4f9c29f 84 left = speed-power;
magnusmland 0:f562e4f9c29f 85
magnusmland 0:f562e4f9c29f 86 // limit checks
magnusmland 0:f562e4f9c29f 87 if (right < MIN)
magnusmland 0:f562e4f9c29f 88 right = MIN;
magnusmland 0:f562e4f9c29f 89 else if (right > MAX)
magnusmland 0:f562e4f9c29f 90 right = MAX;
magnusmland 0:f562e4f9c29f 91
magnusmland 0:f562e4f9c29f 92 if (left < MIN)
magnusmland 0:f562e4f9c29f 93 left = MIN;
magnusmland 0:f562e4f9c29f 94 else if (left > MAX)
magnusmland 0:f562e4f9c29f 95 left = MAX;
magnusmland 0:f562e4f9c29f 96
magnusmland 0:f562e4f9c29f 97 // set speed
magnusmland 0:f562e4f9c29f 98 m3pi.left_motor(left);
magnusmland 0:f562e4f9c29f 99 m3pi.right_motor(right);
uld 6:6865930c1135 100
uld 8:5640c8c5088e 101 ccount++;
uld 7:ac88c8e35048 102 }
vehus 15:8b76add42254 103
vehus 15:8b76add42254 104
uld 7:ac88c8e35048 105 }
uld 7:ac88c8e35048 106
vehus 15:8b76add42254 107 void InitialMessages(void){
uld 9:7b9094864268 108 /*Prints iniatl secret mission*/
uld 9:7b9094864268 109
uld 9:7b9094864268 110 m3pi.cls();
uld 9:7b9094864268 111 m3pi.locate(0,0);
uld 9:7b9094864268 112 m3pi.printf("eliminate");
uld 9:7b9094864268 113 m3pi.locate(0,1);
uld 9:7b9094864268 114 m3pi.printf("all cats");
uld 9:7b9094864268 115 wait(200.0);
uld 9:7b9094864268 116
uld 9:7b9094864268 117 m3pi.cls();
uld 9:7b9094864268 118 m3pi.locate(0,0);
uld 9:7b9094864268 119 m3pi.printf("%f.3 ",m3pi.battery());
uld 9:7b9094864268 120 m3pi.locate(0,1);
uld 9:7b9094864268 121 m3pi.printf("%f.3 ",m3pi.pot_voltage());
uld 9:7b9094864268 122 wait(200.0);
uld 9:7b9094864268 123 m3pi.cls();
uld 9:7b9094864268 124 }
uld 8:5640c8c5088e 125 int PitTest(void){
uld 7:ac88c8e35048 126 /* Test the batteri voltage if the robot is not headed for pit */
uld 7:ac88c8e35048 127
uld 8:5640c8c5088e 128 const float BATVOLTTRESHOLD = 3.0; // Treshold i volt
vehus 15:8b76add42254 129 int result = 0;
uld 7:ac88c8e35048 130
mikkelbredholt 13:ddff4bb7c24f 131 /*Test if the voltage is below the threshold if so turn on go to pit mode*/
vehus 15:8b76add42254 132 if (m3pi.battery() <= BATVOLTTRESHOLD ){
uld 9:7b9094864268 133 result = 1; // Set goto pit condition
vehus 15:8b76add42254 134 LED_Control(1, 1);
uld 7:ac88c8e35048 135 m3pi.cls();
uld 7:ac88c8e35048 136 m3pi.locate(0,0);
uld 7:ac88c8e35048 137 m3pi.printf("Going to");
uld 7:ac88c8e35048 138 m3pi.locate(0,1);
uld 7:ac88c8e35048 139 m3pi.printf("PIT Party");
magnusmland 0:f562e4f9c29f 140 }
uld 8:5640c8c5088e 141 return result;
mikkelbredholt 13:ddff4bb7c24f 142 }
mikkelbredholt 13:ddff4bb7c24f 143
mikkelbredholt 13:ddff4bb7c24f 144 void LED_Control(int ledNumber, int state){
mikkelbredholt 13:ddff4bb7c24f 145 //LED1 on if robot is looking for pit
vehus 15:8b76add42254 146 if (ledNumber == 1)
vehus 15:8b76add42254 147 {
vehus 15:8b76add42254 148 led1 = state;
vehus 15:8b76add42254 149 }
vehus 15:8b76add42254 150 if (ledNumber == 2)
vehus 15:8b76add42254 151 {
vehus 15:8b76add42254 152 led2 = state;
mikkelbredholt 13:ddff4bb7c24f 153 }
vehus 15:8b76add42254 154 if (ledNumber == 3)
vehus 15:8b76add42254 155 {
vehus 15:8b76add42254 156 led3 = state;
vehus 15:8b76add42254 157 }
vehus 15:8b76add42254 158 if (ledNumber == 4)
vehus 15:8b76add42254 159 {
vehus 15:8b76add42254 160 led4 = state;
mikkelbredholt 13:ddff4bb7c24f 161 }
vehus 15:8b76add42254 162 }
vehus 15:8b76add42254 163
vehus 15:8b76add42254 164 void Blink(int ledNumber)
vehus 15:8b76add42254 165 {
vehus 15:8b76add42254 166 int a = 2;
vehus 15:8b76add42254 167 LED_Control (ledNumber , 0);
vehus 15:8b76add42254 168 wait(a);
vehus 15:8b76add42254 169 LED_Control (ledNumber , 1);
vehus 15:8b76add42254 170 wait(a);
mikkelbredholt 13:ddff4bb7c24f 171 }
vehus 15:8b76add42254 172
vehus 15:8b76add42254 173 void PitStop(void)
vehus 15:8b76add42254 174 {
vehus 15:8b76add42254 175 /* Testing alternative stop function
vehus 15:8b76add42254 176 m3pi.left_motor(0);
vehus 15:8b76add42254 177 m3pi.right_motor(0);
vehus 15:8b76add42254 178 */
vehus 15:8b76add42254 179 m3pi.stop(); // stop all engine
vehus 15:8b76add42254 180
vehus 15:8b76add42254 181 // increase counter with one
vehus 15:8b76add42254 182 while (1)
vehus 15:8b76add42254 183 {
vehus 15:8b76add42254 184 Blink (1); // signal in pit
vehus 15:8b76add42254 185
vehus 15:8b76add42254 186 /* missing input to stop blink. */
vehus 15:8b76add42254 187
mikkelbredholt 13:ddff4bb7c24f 188 }
magnusmland 0:f562e4f9c29f 189 }