Radar detector de obstáculos con sensor ultrasonido y motor paso a paso
Dependencies: mbed mbed-STM32F103C8T6 USBDevice_STM32F103 TextLCD HCSR04
main.cpp@0:29c68dc30a1e, 2021-11-18 (annotated)
- Committer:
- michjpr
- Date:
- Thu Nov 18 03:25:19 2021 +0000
- Revision:
- 0:29c68dc30a1e
Redar deteccion de obstaculos con ultrasonido y motor paso a paso
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
michjpr | 0:29c68dc30a1e | 1 | #include "stm32f103c8t6.h" |
michjpr | 0:29c68dc30a1e | 2 | #include "USBSerial.h" |
michjpr | 0:29c68dc30a1e | 3 | #include "mbed.h" |
michjpr | 0:29c68dc30a1e | 4 | //#include "HCSR04.h" |
michjpr | 0:29c68dc30a1e | 5 | #include "hcsr04.h" |
michjpr | 0:29c68dc30a1e | 6 | #include "TextLCD.h" |
michjpr | 0:29c68dc30a1e | 7 | Serial pc(USBTX,USBRX); |
michjpr | 0:29c68dc30a1e | 8 | HCSR04 ultra(PA_0,PA_1); |
michjpr | 0:29c68dc30a1e | 9 | //BusOut motor(PA_4,PA_5,PA_6,PA_7); |
michjpr | 0:29c68dc30a1e | 10 | DigitalOut a(PA_4); |
michjpr | 0:29c68dc30a1e | 11 | DigitalOut a_(PA_5); |
michjpr | 0:29c68dc30a1e | 12 | DigitalOut b(PA_6); |
michjpr | 0:29c68dc30a1e | 13 | DigitalOut b_(PA_7); |
michjpr | 0:29c68dc30a1e | 14 | int Velocidad = 10000; |
michjpr | 0:29c68dc30a1e | 15 | I2C i2c_lcd(PB_9,PB_8); // SDA, SCL |
michjpr | 0:29c68dc30a1e | 16 | TextLCD_I2C lcd(&i2c_lcd,0x4E, TextLCD::LCD16x2, TextLCD::HD44780); |
michjpr | 0:29c68dc30a1e | 17 | unsigned int dist; |
michjpr | 0:29c68dc30a1e | 18 | float range=0; |
michjpr | 0:29c68dc30a1e | 19 | int centimetros=0; |
michjpr | 0:29c68dc30a1e | 20 | int distancias[2][24]; |
michjpr | 0:29c68dc30a1e | 21 | int menord=10000; |
michjpr | 0:29c68dc30a1e | 22 | int menora=180; |
michjpr | 0:29c68dc30a1e | 23 | int tiempo=10000; |
michjpr | 0:29c68dc30a1e | 24 | BusOut Anodos(PA_8, PA_9, PA_10, PA_11, PA_12, PA_15, PB_3); |
michjpr | 0:29c68dc30a1e | 25 | BusOut Catodos(PB_4,PB_5); |
michjpr | 0:29c68dc30a1e | 26 | Ticker ticker1; |
michjpr | 0:29c68dc30a1e | 27 | int numeroSeg[10]={0b0111111, 0b0000110, 0b1011011, 0b1001111, 0b1100110, 0b1101101, 0b1111101, 0b0000111, 0b1111111, 0b1101111}; |
michjpr | 0:29c68dc30a1e | 28 | int numeroCat[2]={0b01, 0b10}; |
michjpr | 0:29c68dc30a1e | 29 | int menorvec[2]; |
michjpr | 0:29c68dc30a1e | 30 | int contadorsegmentos=0; |
michjpr | 0:29c68dc30a1e | 31 | int i=0; |
michjpr | 0:29c68dc30a1e | 32 | void rotate(){ |
michjpr | 0:29c68dc30a1e | 33 | if(i>3){ |
michjpr | 0:29c68dc30a1e | 34 | i=0; } |
michjpr | 0:29c68dc30a1e | 35 | else if(i<0){ |
michjpr | 0:29c68dc30a1e | 36 | i=3; } |
michjpr | 0:29c68dc30a1e | 37 | if(i==0){ |
michjpr | 0:29c68dc30a1e | 38 | a=1; |
michjpr | 0:29c68dc30a1e | 39 | a_=0; |
michjpr | 0:29c68dc30a1e | 40 | b=0; |
michjpr | 0:29c68dc30a1e | 41 | b_=1; } |
michjpr | 0:29c68dc30a1e | 42 | else if(i==1){ |
michjpr | 0:29c68dc30a1e | 43 | a=1; |
michjpr | 0:29c68dc30a1e | 44 | a_=0; |
michjpr | 0:29c68dc30a1e | 45 | b=1; |
michjpr | 0:29c68dc30a1e | 46 | b_=0; } |
michjpr | 0:29c68dc30a1e | 47 | else if(i==2){ |
michjpr | 0:29c68dc30a1e | 48 | a=0; |
michjpr | 0:29c68dc30a1e | 49 | a_=1; |
michjpr | 0:29c68dc30a1e | 50 | b=1; |
michjpr | 0:29c68dc30a1e | 51 | b_=0; } |
michjpr | 0:29c68dc30a1e | 52 | else if(i==3){ |
michjpr | 0:29c68dc30a1e | 53 | a=0; |
michjpr | 0:29c68dc30a1e | 54 | a_=1; |
michjpr | 0:29c68dc30a1e | 55 | b=0; |
michjpr | 0:29c68dc30a1e | 56 | b_=1; } |
michjpr | 0:29c68dc30a1e | 57 | wait(0.01); |
michjpr | 0:29c68dc30a1e | 58 | } |
michjpr | 0:29c68dc30a1e | 59 | void cw(uint16_t step){ |
michjpr | 0:29c68dc30a1e | 60 | for(int count=0;count<step;count++){ |
michjpr | 0:29c68dc30a1e | 61 | i++; |
michjpr | 0:29c68dc30a1e | 62 | rotate(); |
michjpr | 0:29c68dc30a1e | 63 | } |
michjpr | 0:29c68dc30a1e | 64 | } |
michjpr | 0:29c68dc30a1e | 65 | |
michjpr | 0:29c68dc30a1e | 66 | void Radar(){ |
michjpr | 0:29c68dc30a1e | 67 | lcd.setBacklight(TextLCD::LightOff);//LightOff, LightOn |
michjpr | 0:29c68dc30a1e | 68 | lcd.setCursor(TextLCD::CurOff_BlkOff);//CurOff_BlkOff, CurOn_BlkOff, CurOff_BlkOn, CurOn_BlkOn |
michjpr | 0:29c68dc30a1e | 69 | confSysClock(); |
michjpr | 0:29c68dc30a1e | 70 | USBSerial usbSerial(0x1f00, 0x2012, 0x0001, false); |
michjpr | 0:29c68dc30a1e | 71 | for(int i=0;i<24;i++){ |
michjpr | 0:29c68dc30a1e | 72 | ultra.start(); |
michjpr | 0:29c68dc30a1e | 73 | wait_ms(500); |
michjpr | 0:29c68dc30a1e | 74 | dist=ultra.get_dist_cm(); |
michjpr | 0:29c68dc30a1e | 75 | cw(170);//170 |
michjpr | 0:29c68dc30a1e | 76 | lcd.cls (); |
michjpr | 0:29c68dc30a1e | 77 | lcd.locate(0,0); |
michjpr | 0:29c68dc30a1e | 78 | lcd.printf("Fuck you men"); |
michjpr | 0:29c68dc30a1e | 79 | lcd.locate(0,1); |
michjpr | 0:29c68dc30a1e | 80 | lcd.printf("Gonorrea"); |
michjpr | 0:29c68dc30a1e | 81 | distancias[1][i]=i*(360/24); |
michjpr | 0:29c68dc30a1e | 82 | distancias[0][i]=dist; |
michjpr | 0:29c68dc30a1e | 83 | } |
michjpr | 0:29c68dc30a1e | 84 | for(int i=0;i<24;i++){ |
michjpr | 0:29c68dc30a1e | 85 | if(distancias[0][i]<menord){ |
michjpr | 0:29c68dc30a1e | 86 | menord=distancias[0][i]; |
michjpr | 0:29c68dc30a1e | 87 | menora=distancias[1][i]; |
michjpr | 0:29c68dc30a1e | 88 | } |
michjpr | 0:29c68dc30a1e | 89 | usbSerial.printf(" [%icm ,", distancias[0][i]); |
michjpr | 0:29c68dc30a1e | 90 | usbSerial.printf(" %i]", distancias[1][i]); |
michjpr | 0:29c68dc30a1e | 91 | } |
michjpr | 0:29c68dc30a1e | 92 | usbSerial.printf(" menor distancia %icm,\r\n", menord); |
michjpr | 0:29c68dc30a1e | 93 | usbSerial.printf(" angulo %i.\r\n", menora); |
michjpr | 0:29c68dc30a1e | 94 | lcd.cls (); |
michjpr | 0:29c68dc30a1e | 95 | lcd.locate(0,0); |
michjpr | 0:29c68dc30a1e | 96 | lcd.printf("menor distancia cm:%ld",menord ); |
michjpr | 0:29c68dc30a1e | 97 | wait(5); |
michjpr | 0:29c68dc30a1e | 98 | menorvec[0]=menord/100; |
michjpr | 0:29c68dc30a1e | 99 | menorvec[1]=(menord%100)/10; |
michjpr | 0:29c68dc30a1e | 100 | // menorvec[2]=menord%10; |
michjpr | 0:29c68dc30a1e | 101 | } |
michjpr | 0:29c68dc30a1e | 102 | void segmentos(){ |
michjpr | 0:29c68dc30a1e | 103 | |
michjpr | 0:29c68dc30a1e | 104 | Catodos=numeroCat[contadorsegmentos]; |
michjpr | 0:29c68dc30a1e | 105 | Anodos=numeroSeg[menorvec[contadorsegmentos]]; |
michjpr | 0:29c68dc30a1e | 106 | contadorsegmentos+=1; |
michjpr | 0:29c68dc30a1e | 107 | if (contadorsegmentos>2){ |
michjpr | 0:29c68dc30a1e | 108 | contadorsegmentos=0; |
michjpr | 0:29c68dc30a1e | 109 | } |
michjpr | 0:29c68dc30a1e | 110 | } |
michjpr | 0:29c68dc30a1e | 111 | int main() |
michjpr | 0:29c68dc30a1e | 112 | { |
michjpr | 0:29c68dc30a1e | 113 | ticker1.attach(&segmentos, 0.002); |
michjpr | 0:29c68dc30a1e | 114 | Catodos=0b00; |
michjpr | 0:29c68dc30a1e | 115 | |
michjpr | 0:29c68dc30a1e | 116 | confSysClock(); |
michjpr | 0:29c68dc30a1e | 117 | USBSerial usbSerial(0x1f00, 0x2012, 0x0001, false); |
michjpr | 0:29c68dc30a1e | 118 | |
michjpr | 0:29c68dc30a1e | 119 | while(1) { |
michjpr | 0:29c68dc30a1e | 120 | Radar(); |
michjpr | 0:29c68dc30a1e | 121 | } |
michjpr | 0:29c68dc30a1e | 122 | } |