ProgettoFinaleProRCplus
Dependencies: HC-SR04 Motor mbed
main.cpp
- Committer:
- Viktor5
- Date:
- 2016-12-04
- Revision:
- 0:a962eb4d6d61
File content as of revision 0:a962eb4d6d61:
#include "mbed.h" #include "Motor.h" #include "HCSR04.h" Motor mSx(D10, D8, D9); //pwm (E1), fwd (1A), rev (1B) Motor mDx(D5, D6, D7); //pwm (E2), fwd (2A), rev (2B) HCSR04 sensor(D11, D12); //echo pin, trig pin AnalogIn luxSx(A2); //PhotoR Sx AnalogIn luxFw(A1); //PhotoR Fw AnalogIn luxDx(A0); //PhotoR Dx //DigitalOut ledSx(D2); //DigitalOut ledFw(D3); //DigitalOut ledDx(D4); Serial pc (SERIAL_TX, SERIAL_RX); RawSerial hc05(D13, D14); Ticker time_up; //DigitalIn button(PC_13); float inv; float vel; float delta; float dist; float soglia; char a; void forward(float v) //v rta 0 e 1 { mDx.speed(v);//DX mSx.speed(v); //SX //ledSx=0; //ledFw=1; //ledDx=0; pc.printf("Fw"); } void turnDx (float v, float t) //t tra 0 e 1 { mDx.speed(v*t);//DX mSx.speed(v); //SX //ledSx=0; //ledFw=0; //ledDx=1; pc.printf("Dx"); } void turnSx (float v, float t) { mDx.speed(v);//DX mSx.speed(v*t); //SX //ledSx=1; //ledFw=0; //ledDx=0; pc.printf("Sx"); } void check_lux() { float Sx=luxSx.read(); float Fw=luxFw.read(); float Dx=luxDx.read(); dist=50; //provvisorio //dist=sensor.getDistance_cm(); //definitivo //************** if(dist>soglia) { inv=1; } else { inv=-1; } delta=Sx-Dx; if(Fw>Sx&Fw>Dx&dist>soglia) { forward(vel); } else if (delta>0) { turnSx(vel,inv*(1-delta)); } else { turnDx(vel,inv*(delta-1)); } } void OnOff() { if(hc05.readable()) // se e’¨ stato ricevuto un carattere { a=hc05.getc(); // assegna il carattere ricevuto alla var a switch(a) { case ('s'): //Start time_up.attach(&check_lux, 0.2); pc.printf("Start %c\n\r",a); break; case ('d'): //Stop time_up.detach(); pc.printf("Stop %c\n\r",a); break; case ('a'): //accelera if(vel<=1) { vel+=0.1; } break; case ('f'): //frena if(vel>0) { vel-=0.1; } break; default: break; } } } int main() { inv=1; //curva sul posto 1=NO vel=0.9; //velocità base delta=0; // dist=50; // soglia=30; //soglia sensore ditanza sensor.setRanges(1, 150); pc.baud(9600); // setto il baud rate della porta seriale pc hc05.baud(9600); // setto il baud rate della porta rawserial hc05 time_up.attach(&check_lux, 0.2); //provvisorio //hc05.attach(&OnOff, RawSerial::RxIrq); //definitivo //*************** while(1) { // } }