ProgettoFinaleProRCplus

Dependencies:   HC-SR04 Motor mbed

Committer:
Viktor5
Date:
Sun Dec 04 15:15:57 2016 +0000
Revision:
0:a962eb4d6d61
Progetto Finale

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Viktor5 0:a962eb4d6d61 1 #include "mbed.h"
Viktor5 0:a962eb4d6d61 2 #include "Motor.h"
Viktor5 0:a962eb4d6d61 3 #include "HCSR04.h"
Viktor5 0:a962eb4d6d61 4
Viktor5 0:a962eb4d6d61 5 Motor mSx(D10, D8, D9); //pwm (E1), fwd (1A), rev (1B)
Viktor5 0:a962eb4d6d61 6 Motor mDx(D5, D6, D7); //pwm (E2), fwd (2A), rev (2B)
Viktor5 0:a962eb4d6d61 7
Viktor5 0:a962eb4d6d61 8 HCSR04 sensor(D11, D12); //echo pin, trig pin
Viktor5 0:a962eb4d6d61 9
Viktor5 0:a962eb4d6d61 10 AnalogIn luxSx(A2); //PhotoR Sx
Viktor5 0:a962eb4d6d61 11 AnalogIn luxFw(A1); //PhotoR Fw
Viktor5 0:a962eb4d6d61 12 AnalogIn luxDx(A0); //PhotoR Dx
Viktor5 0:a962eb4d6d61 13
Viktor5 0:a962eb4d6d61 14 //DigitalOut ledSx(D2);
Viktor5 0:a962eb4d6d61 15 //DigitalOut ledFw(D3);
Viktor5 0:a962eb4d6d61 16 //DigitalOut ledDx(D4);
Viktor5 0:a962eb4d6d61 17
Viktor5 0:a962eb4d6d61 18 Serial pc (SERIAL_TX, SERIAL_RX);
Viktor5 0:a962eb4d6d61 19
Viktor5 0:a962eb4d6d61 20 RawSerial hc05(D13, D14);
Viktor5 0:a962eb4d6d61 21
Viktor5 0:a962eb4d6d61 22 Ticker time_up;
Viktor5 0:a962eb4d6d61 23
Viktor5 0:a962eb4d6d61 24 //DigitalIn button(PC_13);
Viktor5 0:a962eb4d6d61 25
Viktor5 0:a962eb4d6d61 26 float inv;
Viktor5 0:a962eb4d6d61 27 float vel;
Viktor5 0:a962eb4d6d61 28 float delta;
Viktor5 0:a962eb4d6d61 29 float dist;
Viktor5 0:a962eb4d6d61 30 float soglia;
Viktor5 0:a962eb4d6d61 31 char a;
Viktor5 0:a962eb4d6d61 32
Viktor5 0:a962eb4d6d61 33 void forward(float v) //v rta 0 e 1
Viktor5 0:a962eb4d6d61 34 {
Viktor5 0:a962eb4d6d61 35 mDx.speed(v);//DX
Viktor5 0:a962eb4d6d61 36 mSx.speed(v); //SX
Viktor5 0:a962eb4d6d61 37
Viktor5 0:a962eb4d6d61 38 //ledSx=0;
Viktor5 0:a962eb4d6d61 39 //ledFw=1;
Viktor5 0:a962eb4d6d61 40 //ledDx=0;
Viktor5 0:a962eb4d6d61 41
Viktor5 0:a962eb4d6d61 42 pc.printf("Fw");
Viktor5 0:a962eb4d6d61 43 }
Viktor5 0:a962eb4d6d61 44
Viktor5 0:a962eb4d6d61 45 void turnDx (float v, float t) //t tra 0 e 1
Viktor5 0:a962eb4d6d61 46 {
Viktor5 0:a962eb4d6d61 47 mDx.speed(v*t);//DX
Viktor5 0:a962eb4d6d61 48 mSx.speed(v); //SX
Viktor5 0:a962eb4d6d61 49
Viktor5 0:a962eb4d6d61 50 //ledSx=0;
Viktor5 0:a962eb4d6d61 51 //ledFw=0;
Viktor5 0:a962eb4d6d61 52 //ledDx=1;
Viktor5 0:a962eb4d6d61 53
Viktor5 0:a962eb4d6d61 54 pc.printf("Dx");
Viktor5 0:a962eb4d6d61 55 }
Viktor5 0:a962eb4d6d61 56 void turnSx (float v, float t)
Viktor5 0:a962eb4d6d61 57 {
Viktor5 0:a962eb4d6d61 58 mDx.speed(v);//DX
Viktor5 0:a962eb4d6d61 59 mSx.speed(v*t); //SX
Viktor5 0:a962eb4d6d61 60
Viktor5 0:a962eb4d6d61 61 //ledSx=1;
Viktor5 0:a962eb4d6d61 62 //ledFw=0;
Viktor5 0:a962eb4d6d61 63 //ledDx=0;
Viktor5 0:a962eb4d6d61 64
Viktor5 0:a962eb4d6d61 65 pc.printf("Sx");
Viktor5 0:a962eb4d6d61 66 }
Viktor5 0:a962eb4d6d61 67
Viktor5 0:a962eb4d6d61 68 void check_lux()
Viktor5 0:a962eb4d6d61 69 {
Viktor5 0:a962eb4d6d61 70 float Sx=luxSx.read();
Viktor5 0:a962eb4d6d61 71 float Fw=luxFw.read();
Viktor5 0:a962eb4d6d61 72 float Dx=luxDx.read();
Viktor5 0:a962eb4d6d61 73
Viktor5 0:a962eb4d6d61 74 dist=50; //provvisorio
Viktor5 0:a962eb4d6d61 75 //dist=sensor.getDistance_cm(); //definitivo
Viktor5 0:a962eb4d6d61 76 //**************
Viktor5 0:a962eb4d6d61 77
Viktor5 0:a962eb4d6d61 78 if(dist>soglia)
Viktor5 0:a962eb4d6d61 79 {
Viktor5 0:a962eb4d6d61 80 inv=1;
Viktor5 0:a962eb4d6d61 81 }
Viktor5 0:a962eb4d6d61 82 else
Viktor5 0:a962eb4d6d61 83 {
Viktor5 0:a962eb4d6d61 84 inv=-1;
Viktor5 0:a962eb4d6d61 85 }
Viktor5 0:a962eb4d6d61 86
Viktor5 0:a962eb4d6d61 87 delta=Sx-Dx;
Viktor5 0:a962eb4d6d61 88
Viktor5 0:a962eb4d6d61 89 if(Fw>Sx&Fw>Dx&dist>soglia)
Viktor5 0:a962eb4d6d61 90 {
Viktor5 0:a962eb4d6d61 91 forward(vel);
Viktor5 0:a962eb4d6d61 92 }
Viktor5 0:a962eb4d6d61 93 else if (delta>0)
Viktor5 0:a962eb4d6d61 94 {
Viktor5 0:a962eb4d6d61 95 turnSx(vel,inv*(1-delta));
Viktor5 0:a962eb4d6d61 96 }
Viktor5 0:a962eb4d6d61 97 else
Viktor5 0:a962eb4d6d61 98 {
Viktor5 0:a962eb4d6d61 99 turnDx(vel,inv*(delta-1));
Viktor5 0:a962eb4d6d61 100 }
Viktor5 0:a962eb4d6d61 101 }
Viktor5 0:a962eb4d6d61 102
Viktor5 0:a962eb4d6d61 103 void OnOff()
Viktor5 0:a962eb4d6d61 104 {
Viktor5 0:a962eb4d6d61 105 if(hc05.readable()) // se e’¨ stato ricevuto un carattere
Viktor5 0:a962eb4d6d61 106 {
Viktor5 0:a962eb4d6d61 107 a=hc05.getc(); // assegna il carattere ricevuto alla var a
Viktor5 0:a962eb4d6d61 108 switch(a)
Viktor5 0:a962eb4d6d61 109 {
Viktor5 0:a962eb4d6d61 110 case ('s'): //Start
Viktor5 0:a962eb4d6d61 111 time_up.attach(&check_lux, 0.2);
Viktor5 0:a962eb4d6d61 112 pc.printf("Start %c\n\r",a);
Viktor5 0:a962eb4d6d61 113 break;
Viktor5 0:a962eb4d6d61 114 case ('d'): //Stop
Viktor5 0:a962eb4d6d61 115 time_up.detach();
Viktor5 0:a962eb4d6d61 116 pc.printf("Stop %c\n\r",a);
Viktor5 0:a962eb4d6d61 117 break;
Viktor5 0:a962eb4d6d61 118 case ('a'): //accelera
Viktor5 0:a962eb4d6d61 119 if(vel<=1)
Viktor5 0:a962eb4d6d61 120 {
Viktor5 0:a962eb4d6d61 121 vel+=0.1;
Viktor5 0:a962eb4d6d61 122 }
Viktor5 0:a962eb4d6d61 123 break;
Viktor5 0:a962eb4d6d61 124 case ('f'): //frena
Viktor5 0:a962eb4d6d61 125 if(vel>0)
Viktor5 0:a962eb4d6d61 126 {
Viktor5 0:a962eb4d6d61 127 vel-=0.1;
Viktor5 0:a962eb4d6d61 128 }
Viktor5 0:a962eb4d6d61 129 break;
Viktor5 0:a962eb4d6d61 130 default:
Viktor5 0:a962eb4d6d61 131 break;
Viktor5 0:a962eb4d6d61 132 }
Viktor5 0:a962eb4d6d61 133 }
Viktor5 0:a962eb4d6d61 134 }
Viktor5 0:a962eb4d6d61 135
Viktor5 0:a962eb4d6d61 136
Viktor5 0:a962eb4d6d61 137
Viktor5 0:a962eb4d6d61 138
Viktor5 0:a962eb4d6d61 139 int main()
Viktor5 0:a962eb4d6d61 140 {
Viktor5 0:a962eb4d6d61 141 inv=1; //curva sul posto 1=NO
Viktor5 0:a962eb4d6d61 142 vel=0.9; //velocità base
Viktor5 0:a962eb4d6d61 143 delta=0; //
Viktor5 0:a962eb4d6d61 144 dist=50; //
Viktor5 0:a962eb4d6d61 145 soglia=30; //soglia sensore ditanza
Viktor5 0:a962eb4d6d61 146
Viktor5 0:a962eb4d6d61 147 sensor.setRanges(1, 150);
Viktor5 0:a962eb4d6d61 148
Viktor5 0:a962eb4d6d61 149 pc.baud(9600); // setto il baud rate della porta seriale pc
Viktor5 0:a962eb4d6d61 150 hc05.baud(9600); // setto il baud rate della porta rawserial hc05
Viktor5 0:a962eb4d6d61 151
Viktor5 0:a962eb4d6d61 152
Viktor5 0:a962eb4d6d61 153 time_up.attach(&check_lux, 0.2); //provvisorio
Viktor5 0:a962eb4d6d61 154 //hc05.attach(&OnOff, RawSerial::RxIrq); //definitivo
Viktor5 0:a962eb4d6d61 155 //***************
Viktor5 0:a962eb4d6d61 156
Viktor5 0:a962eb4d6d61 157 while(1)
Viktor5 0:a962eb4d6d61 158 {
Viktor5 0:a962eb4d6d61 159 //
Viktor5 0:a962eb4d6d61 160 }
Viktor5 0:a962eb4d6d61 161 }