DFFTeam / Mbed 2 deprecated LineFollower_4

Smart car linefollower 1.0

Dependencies:   Motor mbed

main.cpp@1:dea1971b7c83, 2016-11-28 (annotated)

Committer:
dfalanga
Date:
Mon Nov 28 16:15:27 2016 +0000
Revision:
1:dea1971b7c83
Parent:
0:9cabfa16c820 
Programma aggiornato al 29/11

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MDevolution 0:9cabfa16c820 1 #include "mbed.h"
MDevolution 0:9cabfa16c820 2 #include "Motor.h"
MDevolution 0:9cabfa16c820 3
MDevolution 0:9cabfa16c820 4 #define N true
MDevolution 0:9cabfa16c820 5 #define B false
MDevolution 0:9cabfa16c820 6 #define NUMSENS 4
dfalanga 1:dea1971b7c83 7 #define SPEED 0.7
MDevolution 0:9cabfa16c820 8
MDevolution 0:9cabfa16c820 9 /*----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 10 Con velocità base di 0.3
MDevolution 0:9cabfa16c820 11
MDevolution 0:9cabfa16c820 12 //----------------------------------------------------------------------------*/
MDevolution 0:9cabfa16c820 13
MDevolution 0:9cabfa16c820 14
MDevolution 0:9cabfa16c820 15
MDevolution 0:9cabfa16c820 16 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 17 // Inizializzazione dei Pin //
MDevolution 0:9cabfa16c820 18 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 19
MDevolution 0:9cabfa16c820 20 Motor MotS(D9, D10, D11); //pwm (E1), fwd (1A), rev (1B) motore sinistro
MDevolution 0:9cabfa16c820 21 Motor MotD(D6, D7, D8); //pwm (E2), fwd (2A), rev (2B) motore destro
MDevolution 0:9cabfa16c820 22 DigitalIn start(PC_13);
MDevolution 0:9cabfa16c820 23 DigitalIn sensd[] = {D2, D3, D4, D5}; //array dei sensori digitali, da sinistra a destra
MDevolution 0:9cabfa16c820 24 AnalogIn sensa[] = {A0, A1, A2, A3}; //array dei sensori analogici, da sinistra a destra
MDevolution 0:9cabfa16c820 25
MDevolution 0:9cabfa16c820 26
MDevolution 0:9cabfa16c820 27 int LetturaSensore(void);
MDevolution 0:9cabfa16c820 28 void Avanti(void);
MDevolution 0:9cabfa16c820 29 void Destra(int curv);
MDevolution 0:9cabfa16c820 30 void Sinistra(int curv);
MDevolution 0:9cabfa16c820 31 void Inversione(void);
MDevolution 0:9cabfa16c820 32
MDevolution 0:9cabfa16c820 33 Serial pc(USBTX, USBRX);
MDevolution 0:9cabfa16c820 34
MDevolution 0:9cabfa16c820 35 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 36
MDevolution 0:9cabfa16c820 37 int stato = 0;
MDevolution 0:9cabfa16c820 38
MDevolution 0:9cabfa16c820 39 int main()
MDevolution 0:9cabfa16c820 40 {
MDevolution 0:9cabfa16c820 41
MDevolution 0:9cabfa16c820 42 while(start==1) {}
MDevolution 0:9cabfa16c820 43
MDevolution 0:9cabfa16c820 44 while(1) {
MDevolution 0:9cabfa16c820 45 stato = LetturaSensore();
MDevolution 0:9cabfa16c820 46 //pc.printf("stato %i\r\n", stato);
MDevolution 0:9cabfa16c820 47
MDevolution 0:9cabfa16c820 48 // andare avanti
MDevolution 0:9cabfa16c820 49 if(stato==6) {
MDevolution 0:9cabfa16c820 50 Avanti();
MDevolution 0:9cabfa16c820 51 }
MDevolution 0:9cabfa16c820 52 // Girare a sinistra
MDevolution 0:9cabfa16c820 53 else if(stato==12||stato==14) {
MDevolution 0:9cabfa16c820 54 Sinistra(0);
MDevolution 0:9cabfa16c820 55 }
MDevolution 0:9cabfa16c820 56 // Girare a destra
MDevolution 0:9cabfa16c820 57 else if(stato==3||stato==7) {
MDevolution 0:9cabfa16c820 58 Destra(0);
MDevolution 0:9cabfa16c820 59 }
MDevolution 0:9cabfa16c820 60 // Girare stretta a sinistra
MDevolution 0:9cabfa16c820 61 else if(stato==8) {
MDevolution 0:9cabfa16c820 62 Sinistra(1);
MDevolution 0:9cabfa16c820 63 }
MDevolution 0:9cabfa16c820 64 // Girare stretta a destra
MDevolution 0:9cabfa16c820 65 else if(stato==1) {
MDevolution 0:9cabfa16c820 66 Destra(1);
MDevolution 0:9cabfa16c820 67 }
MDevolution 0:9cabfa16c820 68 else if(stato==15) {
dfalanga 1:dea1971b7c83 69 wait_ms(100);
MDevolution 0:9cabfa16c820 70 if(stato==15){
MDevolution 0:9cabfa16c820 71 Inversione();
MDevolution 0:9cabfa16c820 72 }
MDevolution 0:9cabfa16c820 73 }
dfalanga 1:dea1971b7c83 74
MDevolution 0:9cabfa16c820 75 }
MDevolution 0:9cabfa16c820 76 }
MDevolution 0:9cabfa16c820 77
MDevolution 0:9cabfa16c820 78 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 79 // Funzioni //
MDevolution 0:9cabfa16c820 80 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 81 void Avanti(){
MDevolution 0:9cabfa16c820 82 MotS.speed(SPEED);
MDevolution 0:9cabfa16c820 83 MotD.speed(SPEED);
MDevolution 0:9cabfa16c820 84 }
MDevolution 0:9cabfa16c820 85 void Destra(int curv){
MDevolution 0:9cabfa16c820 86 if(curv == 0){
MDevolution 0:9cabfa16c820 87 MotS.speed(SPEED);
MDevolution 0:9cabfa16c820 88 MotD.speed(SPEED-0.1);
MDevolution 0:9cabfa16c820 89 }else if(curv == 1){
MDevolution 0:9cabfa16c820 90 MotS.speed(SPEED+0.1);
dfalanga 1:dea1971b7c83 91 MotD.speed(SPEED-0.3);
MDevolution 0:9cabfa16c820 92 }
MDevolution 0:9cabfa16c820 93 }
MDevolution 0:9cabfa16c820 94 void Sinistra(int curv){
MDevolution 0:9cabfa16c820 95 if(curv == 0){
MDevolution 0:9cabfa16c820 96 MotS.speed(SPEED-0.1);
MDevolution 0:9cabfa16c820 97 MotD.speed(SPEED);
MDevolution 0:9cabfa16c820 98 }else if(curv == 1){
dfalanga 1:dea1971b7c83 99 MotS.speed(SPEED-0.3);
MDevolution 0:9cabfa16c820 100 MotD.speed(SPEED+0.1);
MDevolution 0:9cabfa16c820 101 }
MDevolution 0:9cabfa16c820 102 }
MDevolution 0:9cabfa16c820 103 void Inversione(){
MDevolution 0:9cabfa16c820 104 MotS.speed(-0.35);
MDevolution 0:9cabfa16c820 105 MotD.speed(0.35);
MDevolution 0:9cabfa16c820 106 }
MDevolution 0:9cabfa16c820 107
MDevolution 0:9cabfa16c820 108 //Funzione potenza utilizzata per la conversione da binario a decimale
MDevolution 0:9cabfa16c820 109 int pow(int a, int ex){
MDevolution 0:9cabfa16c820 110 if(ex==0)
MDevolution 0:9cabfa16c820 111 return 1;
MDevolution 0:9cabfa16c820 112 if(ex==1)
MDevolution 0:9cabfa16c820 113 return a;
MDevolution 0:9cabfa16c820 114
MDevolution 0:9cabfa16c820 115 int c=a;
MDevolution 0:9cabfa16c820 116 for(; ex!=1; ex=ex-1){
MDevolution 0:9cabfa16c820 117 a=a*c;
MDevolution 0:9cabfa16c820 118 }
MDevolution 0:9cabfa16c820 119 return a;
MDevolution 0:9cabfa16c820 120 }
MDevolution 0:9cabfa16c820 121
MDevolution 0:9cabfa16c820 122 //Lettura sensore e conversione dello stato da binario->decimale
MDevolution 0:9cabfa16c820 123 int LetturaSensore(){
MDevolution 0:9cabfa16c820 124 int data = 0;
MDevolution 0:9cabfa16c820 125 for(int i=NUMSENS-1; i>=0; i=i-1)
MDevolution 0:9cabfa16c820 126 data = data + sensd[i]*pow(2,NUMSENS-1-i);
MDevolution 0:9cabfa16c820 127 return data;
MDevolution 0:9cabfa16c820 128 }