Jose Gutierrez Cabello / Mbed 2 deprecated ProjecteRobotFinal

Projecte final de Sistemes Encastats

Dependencies:   Hc05 RawSerialPc mbed-rtos mbed

main.cpp@8:019cd6583613, 2015-12-23 (annotated)

Committer:
EvaSP
Date:
Wed Dec 23 18:25:42 2015 +0000
Revision:
8:019cd6583613
Parent:
7:723c1e609ac8
Child:
9:75a136453e21
Funciona Part Eva i Josep

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jcabello7 0:be638f6ee353 1 #include "mbed.h"
jcabello7 0:be638f6ee353 2 #include "rtos.h"
jcabello7 4:0a37a963b724 3 #include "Hc05.h"
jcabello7 4:0a37a963b724 4 #include "RawSerialPc.h"
jcabello7 0:be638f6ee353 5
EvaSP 8:019cd6583613 6 //JOSEP
jcabello7 4:0a37a963b724 7 RawSerialPc pc(USBTX, USBRX); //Per provar si funciona, no cal al programa
jcabello7 2:1ac2d1debc92 8 Hc05 bt(D8, D2); //Crea l'objecte bt, connectat als pins D8 (TX) i D2 (RX) (és la connexió sèrie BT)
jcabello7 1:c603de57c8b6 9
EvaSP 8:019cd6583613 10 //EVA
EvaSP 8:019cd6583613 11 int maximumRange = 4000; // Rang màxim
EvaSP 8:019cd6583613 12 int minimumRange = 30; // Rang mínim
EvaSP 8:019cd6583613 13
EvaSP 8:019cd6583613 14 float duration; //Variable per calcular l'amplada en us
EvaSP 8:019cd6583613 15 float distance; //Variable per calcular la distància en mm després de la conversió
EvaSP 8:019cd6583613 16
EvaSP 8:019cd6583613 17 float S1,S2,S3;
EvaSP 8:019cd6583613 18
EvaSP 8:019cd6583613 19 char F[100]; //Taula per poder passar els enters a strings
EvaSP 8:019cd6583613 20
EvaSP 8:019cd6583613 21 float SensorUltrasons(int n){
EvaSP 8:019cd6583613 22
EvaSP 8:019cd6583613 23 if (n==1){
EvaSP 8:019cd6583613 24 DigitalOut trigPin1(D3); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant)
EvaSP 8:019cd6583613 25 Timer timer1; // Creem un comptador per saber el temps que té de resposta
EvaSP 8:019cd6583613 26
EvaSP 8:019cd6583613 27 trigPin1 = 0;
EvaSP 8:019cd6583613 28 wait_ms(2);
EvaSP 8:019cd6583613 29
EvaSP 8:019cd6583613 30 trigPin1 = 1; //Envia un pols de 10ms pel pin “Trigger”
EvaSP 8:019cd6583613 31 wait_ms(10);
EvaSP 8:019cd6583613 32 trigPin1 = 0;
EvaSP 8:019cd6583613 33
EvaSP 8:019cd6583613 34 DigitalIn echoPin1(D3); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida)
EvaSP 8:019cd6583613 35
EvaSP 8:019cd6583613 36 while (!echoPin1); //Espera a la pujada
EvaSP 8:019cd6583613 37 timer1.start();
EvaSP 8:019cd6583613 38
EvaSP 8:019cd6583613 39 while (echoPin1); //Espera a la baixada
EvaSP 8:019cd6583613 40 duration = timer1.read_us();
EvaSP 8:019cd6583613 41 }
EvaSP 8:019cd6583613 42
EvaSP 8:019cd6583613 43 if (n==2){
EvaSP 8:019cd6583613 44 DigitalOut trigPin2(D5); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant)
EvaSP 8:019cd6583613 45 Timer timer2; // Creem un comptador per saber el temps que té de resposta
EvaSP 8:019cd6583613 46
EvaSP 8:019cd6583613 47 trigPin2 = 0;
EvaSP 8:019cd6583613 48 wait_ms(2);
EvaSP 8:019cd6583613 49
EvaSP 8:019cd6583613 50 trigPin2 = 1; //Envia un pols de 10ms pel pin “Trigger”
EvaSP 8:019cd6583613 51 wait_ms(10);
EvaSP 8:019cd6583613 52 trigPin2 = 0;
EvaSP 8:019cd6583613 53
EvaSP 8:019cd6583613 54 DigitalIn echoPin2(D5); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida)
EvaSP 8:019cd6583613 55
EvaSP 8:019cd6583613 56 while (!echoPin2); //Espera a la pujada
EvaSP 8:019cd6583613 57 timer2.start();
EvaSP 8:019cd6583613 58
EvaSP 8:019cd6583613 59 while (echoPin2); //Espera a la baixada
EvaSP 8:019cd6583613 60 duration = timer2.read_us();
EvaSP 8:019cd6583613 61
EvaSP 8:019cd6583613 62 }
EvaSP 8:019cd6583613 63
EvaSP 8:019cd6583613 64 if (n==3){
EvaSP 8:019cd6583613 65 DigitalOut trigPin3(D6); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant)
EvaSP 8:019cd6583613 66 Timer timer3; // Creem un comptador per saber el temps que té de resposta
EvaSP 8:019cd6583613 67
EvaSP 8:019cd6583613 68 trigPin3 = 0;
EvaSP 8:019cd6583613 69 wait_ms(2);
EvaSP 8:019cd6583613 70
EvaSP 8:019cd6583613 71 trigPin3 = 1; //Envia un pols de 10ms pel pin “Trigger”
EvaSP 8:019cd6583613 72 wait_ms(10);
EvaSP 8:019cd6583613 73 trigPin3 = 0;
EvaSP 8:019cd6583613 74
EvaSP 8:019cd6583613 75 DigitalIn echoPin3(D6); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida)
EvaSP 8:019cd6583613 76
EvaSP 8:019cd6583613 77 while (!echoPin3); //Espera a la pujada
EvaSP 8:019cd6583613 78 timer3.start();
EvaSP 8:019cd6583613 79
EvaSP 8:019cd6583613 80 while (echoPin3); //Espera a la baixada
EvaSP 8:019cd6583613 81 duration = timer3.read_us();
EvaSP 8:019cd6583613 82 }
EvaSP 8:019cd6583613 83
EvaSP 8:019cd6583613 84 // Calcular la distancia (en mm)
EvaSP 8:019cd6583613 85 float x = (duration/5.6);
EvaSP 8:019cd6583613 86 return x;
EvaSP 8:019cd6583613 87 }
EvaSP 8:019cd6583613 88
EvaSP 8:019cd6583613 89
EvaSP 8:019cd6583613 90 void ModeAutomatic(){
EvaSP 8:019cd6583613 91
EvaSP 8:019cd6583613 92 float a=3999;
EvaSP 8:019cd6583613 93 float b=3999;
EvaSP 8:019cd6583613 94 float c=3999;
EvaSP 8:019cd6583613 95
EvaSP 8:019cd6583613 96 pc.enviaString("V050FM2");
EvaSP 8:019cd6583613 97 pc.enviaString("V050FM3");
EvaSP 8:019cd6583613 98 //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 0º
EvaSP 8:019cd6583613 99 while ((a>100)&&(bt.getMode()==3)){
EvaSP 8:019cd6583613 100
EvaSP 8:019cd6583613 101 a=SensorUltrasons(1); //Crida la funció amb valor n=1, per tant sensor 1
EvaSP 8:019cd6583613 102
EvaSP 8:019cd6583613 103 //pc.enviaString(p);
EvaSP 8:019cd6583613 104
EvaSP 8:019cd6583613 105 if (a >= maximumRange || a <= minimumRange){
EvaSP 8:019cd6583613 106 // Enviar "Fora de Rang" per indicar-ho si escau a menys de 0m o més de 4m ja que segur que és un error.
EvaSP 8:019cd6583613 107 pc.enviaString("Sensor 1 fora de Rang \n");
EvaSP 8:019cd6583613 108 }
EvaSP 8:019cd6583613 109 else {
EvaSP 8:019cd6583613 110 // Enviar la distancia si està dins el rang de 0 a 4m
EvaSP 8:019cd6583613 111 snprintf(F, 100, "Sensor 1: Distancia: %f mm\n",a);
EvaSP 8:019cd6583613 112 pc.enviaString(F);
EvaSP 8:019cd6583613 113 }
EvaSP 8:019cd6583613 114 }
EvaSP 8:019cd6583613 115
EvaSP 8:019cd6583613 116 pc.enviaString("V000FM2");
EvaSP 8:019cd6583613 117 pc.enviaString("V000FM3");
EvaSP 8:019cd6583613 118
EvaSP 8:019cd6583613 119 pc.enviaString("V050FM1");
EvaSP 8:019cd6583613 120 pc.enviaString("V050FM3");
EvaSP 8:019cd6583613 121 //MARCEL: PARA MOTORS
EvaSP 8:019cd6583613 122 //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 120º
EvaSP 8:019cd6583613 123
EvaSP 8:019cd6583613 124 while ((b>100)&&(bt.getMode()==3)){
EvaSP 8:019cd6583613 125 b=SensorUltrasons(2); //Crida la funció amb valor n=2, per tant sensor 2
EvaSP 8:019cd6583613 126
EvaSP 8:019cd6583613 127 if (b >= maximumRange || b <= minimumRange){
EvaSP 8:019cd6583613 128 // Enviar "Fora de Rang" per indicar-ho si escau a menys de 0m o més de 4m ja que segur que és un error.
EvaSP 8:019cd6583613 129 pc.enviaString("Sensor 2 fora de Rang \n");
EvaSP 8:019cd6583613 130 }
EvaSP 8:019cd6583613 131 else {
EvaSP 8:019cd6583613 132 // Enviar la distancia si està dins el rang de 0 a 4m
EvaSP 8:019cd6583613 133 snprintf(F, 100, "Sensor 2: Distancia: %f mm\n",b);
EvaSP 8:019cd6583613 134 pc.enviaString(F);
EvaSP 8:019cd6583613 135 }
EvaSP 8:019cd6583613 136 }
EvaSP 8:019cd6583613 137
EvaSP 8:019cd6583613 138 pc.enviaString("V000FM1");
EvaSP 8:019cd6583613 139 pc.enviaString("V000FM3");
EvaSP 8:019cd6583613 140
EvaSP 8:019cd6583613 141 pc.enviaString("V050FM1");
EvaSP 8:019cd6583613 142 pc.enviaString("V050FM2");
EvaSP 8:019cd6583613 143 //MARCEL: PARA MOTORS
EvaSP 8:019cd6583613 144 //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 240º
EvaSP 8:019cd6583613 145
EvaSP 8:019cd6583613 146 while ((c>100)&&(bt.getMode()==3)){
EvaSP 8:019cd6583613 147 c=SensorUltrasons(3); //Crida la funció amb valor n=3, per tant sensor 3
EvaSP 8:019cd6583613 148
EvaSP 8:019cd6583613 149 if (c >= maximumRange || c <= minimumRange){
EvaSP 8:019cd6583613 150 // Enviar "Fora de Rang" per indicar-ho si escau a menys de 0m o més de 4m ja que segur que és un error.
EvaSP 8:019cd6583613 151 pc.enviaString("Sensor 3 fora de Rang \n");
EvaSP 8:019cd6583613 152 }
EvaSP 8:019cd6583613 153 else {
EvaSP 8:019cd6583613 154 // Enviar la distancia si està dins el rang de 0 a 4m
EvaSP 8:019cd6583613 155 snprintf(F, 100, "Sensor 3: Distancia: %f mm\n",c);
EvaSP 8:019cd6583613 156 pc.enviaString(F);
EvaSP 8:019cd6583613 157 }
EvaSP 8:019cd6583613 158 }
EvaSP 8:019cd6583613 159
EvaSP 8:019cd6583613 160 pc.enviaString("V000FM1");
EvaSP 8:019cd6583613 161 pc.enviaString("V000FM2");
EvaSP 8:019cd6583613 162 //MARCEL: PARA MOTORS
EvaSP 8:019cd6583613 163
EvaSP 8:019cd6583613 164 }
EvaSP 8:019cd6583613 165
EvaSP 8:019cd6583613 166
EvaSP 8:019cd6583613 167
EvaSP 8:019cd6583613 168
EvaSP 8:019cd6583613 169
jcabello7 1:c603de57c8b6 170 //Aquest serà el meu fil, que va llegint cada 300ms del HC-05
jcabello7 0:be638f6ee353 171 void llegir_thread(void const *args) {
jcabello7 0:be638f6ee353 172 while (true) {
jcabello7 6:c4af95f6e155 173 //Intenta llegir un string, i si el llegeix, el tracta
jcabello7 3:4a598ab10e87 174 if(bt.llegirString())
jcabello7 7:723c1e609ac8 175 bt.tractaString();
jcabello7 6:c4af95f6e155 176 //Comprova la connexió (mira si ha rebut alguna cosa)
jcabello7 3:4a598ab10e87 177 bt.comprovaConnexio();
jcabello7 6:c4af95f6e155 178 //Temps del Thread que està esperant
jcabello7 3:4a598ab10e87 179 Thread::wait(100);
jcabello7 3:4a598ab10e87 180
jcabello7 0:be638f6ee353 181 }
jcabello7 0:be638f6ee353 182 }
jcabello7 1:c603de57c8b6 183
EvaSP 8:019cd6583613 184
EvaSP 8:019cd6583613 185 //Aquest serà el fil de l'Eva, que va llegint els sensors d'ultrasons
EvaSP 8:019cd6583613 186 void llegir_ultrathread(void const *args) {
EvaSP 8:019cd6583613 187 while (true) {
EvaSP 8:019cd6583613 188 S1=SensorUltrasons(1);
EvaSP 8:019cd6583613 189 S2=SensorUltrasons(2);
EvaSP 8:019cd6583613 190 S3=SensorUltrasons(3);
EvaSP 8:019cd6583613 191
EvaSP 8:019cd6583613 192 //Temps del Thread que està esperant
EvaSP 8:019cd6583613 193 Thread::wait(100);
EvaSP 8:019cd6583613 194
EvaSP 8:019cd6583613 195 }
EvaSP 8:019cd6583613 196 }
EvaSP 8:019cd6583613 197
jcabello7 1:c603de57c8b6 198 //Aquest serà el programa principal
jcabello7 0:be638f6ee353 199 int main() {
EvaSP 8:019cd6583613 200 //osThreadSetPriority(osThreadGetId(),osPriorityRealtime);
EvaSP 8:019cd6583613 201 Thread thread(llegir_thread); //Inicia el fil llegir_thread
EvaSP 8:019cd6583613 202 thread.set_priority(osPriorityLow);
jcabello7 1:c603de57c8b6 203
EvaSP 8:019cd6583613 204 //Thread thread2(llegir_ultrathread); //Inicia el fil dels ultrasons
jcabello7 7:723c1e609ac8 205 //char m[128]; //Per visualitzar el mode i el timer, no es necessari
jcabello7 0:be638f6ee353 206 while (true) {
jcabello7 7:723c1e609ac8 207 //pc.enviaString(bt.strLlegit); //Per veure l'ultim string llegit, no es necessari
jcabello7 7:723c1e609ac8 208 bt.calculaMotors(); //Calcula com s'han de moure els motors
jcabello7 7:723c1e609ac8 209 //snprintf(m, 128, "Mode: %d | timer: %dms", bt.getMode(), bt.getTimer()); //Per visualitzar el mode i el timer, no es necessari
jcabello7 7:723c1e609ac8 210 //pc.enviaString(m); //Per visualitzar el mode i el timer, no es necessari
jcabello7 7:723c1e609ac8 211 //pc.enviaString(bt.strPos); //Per visualitzar la posicio on ha d'anar el robot, no es necessari
jcabello7 7:723c1e609ac8 212 if((bt.getMode() == 0) || (bt.getMode() == 1) || (bt.getMode() == 2)) {
jcabello7 7:723c1e609ac8 213 pc.enviaString(bt.strM1); //String motor1 que s'envia a l'arduino d'en Marcel
jcabello7 7:723c1e609ac8 214 pc.enviaString(bt.strM2); //String motor2 que s'envia a l'arduino d'en Marcel
jcabello7 7:723c1e609ac8 215 pc.enviaString(bt.strM3); //String motor3 que s'envia a l'arduino d'en Marcel
jcabello7 7:723c1e609ac8 216 }
jcabello7 7:723c1e609ac8 217 else if (bt.getMode() == 3) {
EvaSP 8:019cd6583613 218 ModeAutomatic();
jcabello7 7:723c1e609ac8 219 }
jcabello7 7:723c1e609ac8 220 else if (bt.getMode() == 4) {
jcabello7 7:723c1e609ac8 221 ;//Mode mando (programa Marc)
jcabello7 7:723c1e609ac8 222 }
jcabello7 7:723c1e609ac8 223 else {
jcabello7 7:723c1e609ac8 224 pc.enviaString("V000FM1"); //String motor1 que s'envia a l'arduino d'en Marcel
jcabello7 7:723c1e609ac8 225 pc.enviaString("V000FM2"); //String motor2 que s'envia a l'arduino d'en Marcel
jcabello7 7:723c1e609ac8 226 pc.enviaString("V000FM3"); //String motor3 que s'envia a l'arduino d'en Marcel
jcabello7 7:723c1e609ac8 227 }
jcabello7 6:c4af95f6e155 228 Thread::wait(500); //Temps del Thread que està esperant
jcabello7 0:be638f6ee353 229 }
jcabello7 0:be638f6ee353 230 }