DFFTeam / Mbed 2 deprecated LineFollower_4

Smart car linefollower 1.0

Dependencies:   Motor mbed

main.cpp@2:b5a8e835fb14, 2016-12-05 (annotated)

Committer:
MDevolution
Date:
Mon Dec 05 10:39:52 2016 +0000
Revision:
2:b5a8e835fb14
Parent:
0:9cabfa16c820 
Versione con led

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
MDevolution 2:b5a8e835fb14 7 #define SPEED 0.5
MDevolution 0:9cabfa16c820 8
MDevolution 0:9cabfa16c820 9 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 10 // Inizializzazione dei Pin //
MDevolution 0:9cabfa16c820 11 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 12
MDevolution 2:b5a8e835fb14 13 Motor MotS(D9, D10, D12); //pwm (E1), fwd (1A), rev (1B) motore sinistro
MDevolution 0:9cabfa16c820 14 Motor MotD(D6, D7, D8); //pwm (E2), fwd (2A), rev (2B) motore destro
MDevolution 0:9cabfa16c820 15 DigitalIn start(PC_13);
MDevolution 0:9cabfa16c820 16 DigitalIn sensd[] = {D2, D3, D4, D5}; //array dei sensori digitali, da sinistra a destra
MDevolution 0:9cabfa16c820 17 AnalogIn sensa[] = {A0, A1, A2, A3}; //array dei sensori analogici, da sinistra a destra
MDevolution 2:b5a8e835fb14 18 PwmOut led[] = {D11, D14, D15}; //array dei sensori digitali, da sinistra a destra
MDevolution 0:9cabfa16c820 19
MDevolution 0:9cabfa16c820 20 int LetturaSensore(void);
MDevolution 0:9cabfa16c820 21 void Avanti(void);
MDevolution 0:9cabfa16c820 22 void Destra(int curv);
MDevolution 0:9cabfa16c820 23 void Sinistra(int curv);
MDevolution 0:9cabfa16c820 24 void Inversione(void);
MDevolution 2:b5a8e835fb14 25 void ledrgb(int st);
MDevolution 0:9cabfa16c820 26
MDevolution 0:9cabfa16c820 27 Serial pc(USBTX, USBRX);
MDevolution 0:9cabfa16c820 28
MDevolution 0:9cabfa16c820 29 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 30
MDevolution 0:9cabfa16c820 31 int stato = 0;
MDevolution 0:9cabfa16c820 32
MDevolution 0:9cabfa16c820 33 int main()
MDevolution 2:b5a8e835fb14 34 {
MDevolution 2:b5a8e835fb14 35 // Inizializzazione PWM
MDevolution 2:b5a8e835fb14 36 for(int i=0; i<=2; i++)
MDevolution 2:b5a8e835fb14 37 led[i].period(0.01);
MDevolution 2:b5a8e835fb14 38
MDevolution 2:b5a8e835fb14 39 wait(1);
MDevolution 2:b5a8e835fb14 40
MDevolution 2:b5a8e835fb14 41 // Inizializzazione macchinina
MDevolution 2:b5a8e835fb14 42 for(int i=0; i<3; i++){
MDevolution 2:b5a8e835fb14 43 for(int j=0; j<3; j++){
MDevolution 2:b5a8e835fb14 44 led[0] = 0;
MDevolution 2:b5a8e835fb14 45 led[1] = 0;
MDevolution 2:b5a8e835fb14 46 led[2] = 0;
MDevolution 0:9cabfa16c820 47
MDevolution 2:b5a8e835fb14 48 led[j] = 1;
MDevolution 2:b5a8e835fb14 49 wait_ms(150);
MDevolution 2:b5a8e835fb14 50 }
MDevolution 2:b5a8e835fb14 51 }
MDevolution 2:b5a8e835fb14 52
MDevolution 2:b5a8e835fb14 53 // Pulsante di Start
MDevolution 2:b5a8e835fb14 54 while(start==1){
MDevolution 2:b5a8e835fb14 55 ledrgb(stato);
MDevolution 2:b5a8e835fb14 56 }
MDevolution 2:b5a8e835fb14 57
MDevolution 2:b5a8e835fb14 58 // Ciclo principale
MDevolution 0:9cabfa16c820 59 while(1) {
MDevolution 0:9cabfa16c820 60 stato = LetturaSensore();
MDevolution 0:9cabfa16c820 61 //pc.printf("stato %i\r\n", stato);
MDevolution 0:9cabfa16c820 62
MDevolution 0:9cabfa16c820 63 // andare avanti
MDevolution 0:9cabfa16c820 64 if(stato==6) {
MDevolution 0:9cabfa16c820 65 Avanti();
MDevolution 0:9cabfa16c820 66 }
MDevolution 0:9cabfa16c820 67 // Girare a sinistra
MDevolution 0:9cabfa16c820 68 else if(stato==12||stato==14) {
MDevolution 0:9cabfa16c820 69 Sinistra(0);
MDevolution 0:9cabfa16c820 70 }
MDevolution 0:9cabfa16c820 71 // Girare a destra
MDevolution 0:9cabfa16c820 72 else if(stato==3||stato==7) {
MDevolution 0:9cabfa16c820 73 Destra(0);
MDevolution 0:9cabfa16c820 74 }
MDevolution 0:9cabfa16c820 75 // Girare stretta a sinistra
MDevolution 0:9cabfa16c820 76 else if(stato==8) {
MDevolution 0:9cabfa16c820 77 Sinistra(1);
MDevolution 0:9cabfa16c820 78 }
MDevolution 0:9cabfa16c820 79 // Girare stretta a destra
MDevolution 0:9cabfa16c820 80 else if(stato==1) {
MDevolution 0:9cabfa16c820 81 Destra(1);
MDevolution 0:9cabfa16c820 82 }
MDevolution 2:b5a8e835fb14 83 // Inversione
MDevolution 0:9cabfa16c820 84 else if(stato==15) {
MDevolution 2:b5a8e835fb14 85 wait_ms(100);
MDevolution 0:9cabfa16c820 86 if(stato==15){
MDevolution 0:9cabfa16c820 87 Inversione();
MDevolution 0:9cabfa16c820 88 }
MDevolution 0:9cabfa16c820 89 }
MDevolution 2:b5a8e835fb14 90 ledrgb(stato);
MDevolution 0:9cabfa16c820 91 }
MDevolution 0:9cabfa16c820 92 }
MDevolution 0:9cabfa16c820 93
MDevolution 0:9cabfa16c820 94 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 95 // Funzioni //
MDevolution 0:9cabfa16c820 96 //----------------------------------------------------------------------------//
MDevolution 0:9cabfa16c820 97 void Avanti(){
MDevolution 0:9cabfa16c820 98 MotS.speed(SPEED);
MDevolution 0:9cabfa16c820 99 MotD.speed(SPEED);
MDevolution 0:9cabfa16c820 100 }
MDevolution 0:9cabfa16c820 101 void Destra(int curv){
MDevolution 0:9cabfa16c820 102 if(curv == 0){
MDevolution 0:9cabfa16c820 103 MotS.speed(SPEED);
MDevolution 0:9cabfa16c820 104 MotD.speed(SPEED-0.1);
MDevolution 0:9cabfa16c820 105 }else if(curv == 1){
MDevolution 0:9cabfa16c820 106 MotS.speed(SPEED+0.1);
MDevolution 2:b5a8e835fb14 107 MotD.speed(SPEED-0.3);
MDevolution 0:9cabfa16c820 108 }
MDevolution 0:9cabfa16c820 109 }
MDevolution 0:9cabfa16c820 110 void Sinistra(int curv){
MDevolution 0:9cabfa16c820 111 if(curv == 0){
MDevolution 0:9cabfa16c820 112 MotS.speed(SPEED-0.1);
MDevolution 0:9cabfa16c820 113 MotD.speed(SPEED);
MDevolution 0:9cabfa16c820 114 }else if(curv == 1){
MDevolution 2:b5a8e835fb14 115 MotS.speed(SPEED-0.3);
MDevolution 0:9cabfa16c820 116 MotD.speed(SPEED+0.1);
MDevolution 0:9cabfa16c820 117 }
MDevolution 0:9cabfa16c820 118 }
MDevolution 0:9cabfa16c820 119 void Inversione(){
MDevolution 0:9cabfa16c820 120 MotS.speed(-0.35);
MDevolution 0:9cabfa16c820 121 MotD.speed(0.35);
MDevolution 0:9cabfa16c820 122 }
MDevolution 0:9cabfa16c820 123
MDevolution 0:9cabfa16c820 124 //Funzione potenza utilizzata per la conversione da binario a decimale
MDevolution 0:9cabfa16c820 125 int pow(int a, int ex){
MDevolution 0:9cabfa16c820 126 if(ex==0)
MDevolution 0:9cabfa16c820 127 return 1;
MDevolution 0:9cabfa16c820 128 if(ex==1)
MDevolution 0:9cabfa16c820 129 return a;
MDevolution 0:9cabfa16c820 130
MDevolution 0:9cabfa16c820 131 int c=a;
MDevolution 0:9cabfa16c820 132 for(; ex!=1; ex=ex-1){
MDevolution 0:9cabfa16c820 133 a=a*c;
MDevolution 0:9cabfa16c820 134 }
MDevolution 0:9cabfa16c820 135 return a;
MDevolution 0:9cabfa16c820 136 }
MDevolution 0:9cabfa16c820 137
MDevolution 0:9cabfa16c820 138 //Lettura sensore e conversione dello stato da binario->decimale
MDevolution 0:9cabfa16c820 139 int LetturaSensore(){
MDevolution 0:9cabfa16c820 140 int data = 0;
MDevolution 0:9cabfa16c820 141 for(int i=NUMSENS-1; i>=0; i=i-1)
MDevolution 0:9cabfa16c820 142 data = data + sensd[i]*pow(2,NUMSENS-1-i);
MDevolution 0:9cabfa16c820 143 return data;
MDevolution 2:b5a8e835fb14 144 }
MDevolution 2:b5a8e835fb14 145
MDevolution 2:b5a8e835fb14 146 void ledrgb(int st){
MDevolution 2:b5a8e835fb14 147 if (st == 0){
MDevolution 2:b5a8e835fb14 148 led[0] = 1;
MDevolution 2:b5a8e835fb14 149 led[1] = 0;
MDevolution 2:b5a8e835fb14 150 led[2] = 1;
MDevolution 2:b5a8e835fb14 151 }else{
MDevolution 2:b5a8e835fb14 152 led[0] = sensa[0];
MDevolution 2:b5a8e835fb14 153 led[1] = (sensa[1]+sensa[2])/2;
MDevolution 2:b5a8e835fb14 154 led[2] = sensa[3];
MDevolution 2:b5a8e835fb14 155 }
MDevolution 0:9cabfa16c820 156 }