├── .gitignore ├── .gitmodules ├── 3d_model ├── README.md └── stl_files │ ├── Part Studio - Arandela M2 -editada-.stl │ ├── Part Studio - Arandela M3 -editada-.stl │ ├── Part Studio - Bracket Motor L.stl │ ├── Part Studio - Bracket Motor R.stl │ ├── Part Studio - Bracket Ventilador.stl │ ├── Part Studio - Cover Sensor.stl │ ├── Part Studio - Rueda.stl │ ├── Part Studio - Spur gear (22 teeth).stl │ └── Part Studio - Ventilador_fdm.stl ├── LICENSE ├── README.md ├── ZoroBot3.code-workspace ├── images ├── ZoroBot3_3d_model.png ├── ZoroBot3_finish_1.png ├── ZoroBot3_finish_2.png ├── ZoroBot3_finish_3.png ├── ZoroBot3_rev2_3d_model.png ├── ZoroBot3_rev2_3d_model_b.png ├── ZoroBot3_rev2_finish_1.jpg ├── ZoroBot3_rev2_finish_2.jpg └── ZoroBot3_rev2_finish_3.jpg ├── pcb_files ├── bom │ ├── .gitignore │ └── index.html ├── gerbers │ ├── .gitignore │ ├── ZoroBot3.zip │ ├── bom.csv │ ├── designators.csv │ ├── netlist.ipc │ └── positions.csv └── kicad_project │ ├── .gitignore │ ├── Encoders.kicad_sch │ ├── MPU.kicad_sch │ ├── ZoroBot3.kicad_dru │ ├── ZoroBot3.kicad_pcb │ ├── ZoroBot3.kicad_prl │ ├── ZoroBot3.kicad_pro │ ├── ZoroBot3.kicad_sch │ ├── fabrication-toolkit-options.json │ ├── fp-info-cache │ ├── sensores.kicad_sch │ └── sym-lib-table └── source_code ├── .clang-format ├── .gitignore ├── .travis.yml ├── boards └── custom_stm32f405rgt6.json ├── include ├── battery.h ├── buttons.h ├── calibrations.h ├── config.h ├── constants.h ├── control.h ├── debug.h ├── delay.h ├── eeprom.h ├── encoders.h ├── floodfill.h ├── floodfill_weigths.h ├── handwall.h ├── leds.h ├── lsm6dsr.h ├── macroarray.h ├── maze.h ├── menu.h ├── menu_configs.h ├── menu_run.h ├── motors.h ├── move.h ├── rc5.h ├── sensors.h ├── setup.h ├── timetrial.h ├── usart.h └── utils.h ├── lib ├── .gitignore └── lsm6dsr_reg │ ├── lsm6dsr_reg.c │ └── lsm6dsr_reg.h ├── platformio.ini └── src ├── battery.c ├── buttons.c ├── calibrations.c ├── config.c ├── control.c ├── debug.c ├── delay.c ├── eeprom.c ├── encoders.c ├── floodfill.c ├── floodfill_weigths.c ├── handwall.c ├── leds.c ├── lsm6dsr.c ├── macroarray.c ├── main.c ├── maze.c ├── menu.c ├── menu_configs.c ├── menu_run.c ├── motors.c ├── move.c ├── rc5.c ├── sensors.c ├── setup.c ├── timetrial.c ├── usart.c └── utils.c /.gitignore: -------------------------------------------------------------------------------- 1 | ZoroBot3Haki.code-workspace 2 | .pio 3 | .pioenvs 4 | .piolibdeps 5 | .vscode 6 | .vscode/* 7 | desktop.ini 8 | *.skb -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pcb_files/kicad_project/custom_libraries"] 2 | path = pcb_files/kicad_project/custom_libraries 3 | url = https://github.com/OPRobots/KiCADLibraries.git 4 | -------------------------------------------------------------------------------- /3d_model/README.md: -------------------------------------------------------------------------------- 1 | # ZoroBot3 2 | [![ZoroBot3](../images/ZoroBot3_rev2_3d_model_b.png "ZoroBot3")](https://cad.onshape.com/documents/c2349433f4c043eb0ce1efdc/w/394e4193cf2128d0e3f2c628/e/d0ea661264a18ad6b8f6ed22?renderMode=0&uiState=682c9debc352a975bc39e300) 3 | El diseño de ZoroBot3 se encuentra disponible en [Onshape](https://cad.onshape.com/documents/c2349433f4c043eb0ce1efdc/w/394e4193cf2128d0e3f2c628/e/d0ea661264a18ad6b8f6ed22?renderMode=0&uiState=682c9debc352a975bc39e300), donde están diseñados todos los componentes del robot, incluyendo el chasis, la PCB y los engranajes. 4 | -------------------------------------------------------------------------------- /3d_model/stl_files/Part Studio - Arandela M2 -editada-.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/3d_model/stl_files/Part Studio - Arandela M2 -editada-.stl -------------------------------------------------------------------------------- /3d_model/stl_files/Part Studio - Arandela M3 -editada-.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/3d_model/stl_files/Part Studio - Arandela M3 -editada-.stl -------------------------------------------------------------------------------- /3d_model/stl_files/Part Studio - Bracket Motor L.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/3d_model/stl_files/Part Studio - Bracket Motor L.stl -------------------------------------------------------------------------------- /3d_model/stl_files/Part Studio - Bracket Motor R.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/3d_model/stl_files/Part Studio - Bracket Motor R.stl -------------------------------------------------------------------------------- /3d_model/stl_files/Part Studio - Bracket Ventilador.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/3d_model/stl_files/Part Studio - Bracket Ventilador.stl -------------------------------------------------------------------------------- /3d_model/stl_files/Part Studio - Cover Sensor.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/3d_model/stl_files/Part Studio - Cover Sensor.stl -------------------------------------------------------------------------------- /3d_model/stl_files/Part Studio - Rueda.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/3d_model/stl_files/Part Studio - Rueda.stl -------------------------------------------------------------------------------- /3d_model/stl_files/Part Studio - Spur gear (22 teeth).stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/3d_model/stl_files/Part Studio - Spur gear (22 teeth).stl -------------------------------------------------------------------------------- /3d_model/stl_files/Part Studio - Ventilador_fdm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/3d_model/stl_files/Part Studio - Ventilador_fdm.stl -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ZoroBot3 2 | Tercera versión de nuestro primer robot. Un robot micromouse de alto rendimiento con STM32F4, encoders magneticos de alta resolución, succión y muchos leds molones. 3 | 4 | ![ZoroBot3](./images/ZoroBot3_rev2_finish_3.jpg "ZoroBot3") 5 | 6 | ## Hardware 7 | - **Microcontrolador**: STM32F405RGT6 @168MHz 8 | - **Driver de motores**: MP6551 @20kHz 9 | - **Giroscopio**: LSM6DSRTR 4000dps 10 | - **Encoders**: AS5145B-HSST 11 | - **Mosfets**: AO3400 (A09T) Tanto para conmutar los emisores de los sensores como para la succión 12 | - **Regulador**: CN3903 + LDO ME611C33M5G 13 | - **Receptor IR 38kHz**: TSSP77038TR 14 | - **Bateria**: LiPo 2S 260mAh 35-70C Turnigy nano-tech 15 | - **Sensores**: 16 | - 4 Emisores IR SFH-4550 17 | - 4 Receptores IR ST-1KL3A 18 | 19 | - **Tracción**: 20 | - 2x Motores Coreless 8520 7.4v genéricos (AliExpress) 21 | - Goma de ruedas Kyosho K.MZW39-30 22 | - Chasis de PCB con soportes de motores y ventilador en PLA 23 | - Piñones de motores 8T 0.5M 24 | - Engranajes de las ruedas 40T 0.5M 25 | - Engranajes de los encoders 22T 0.5M impresos en resina 26 | - 4x Rodamientos MR63ZZ (Ruedas) 27 | - 2x Rodamientos MR52ZZ (Encoders) 28 | - 2x Imán radial 6x2.5mm 29 | 30 | ![ZoroBot3 Chasis](./images/ZoroBot3_rev2_3d_model.png "ZoroBot3 - Chasis") 31 | 32 | ## Software 33 | - Programado en VSCode y PlatformIO con LibOpenCM3. 34 | - Todos los valores analógicos se leen mediante **DMA** y son procesados cada 1ms. 35 | - Los sensores se leen mediante una máquina de estados a 16kHz, enciendiendo y apagando los emisores para **filtrar la luz ambiente**. 36 | - El valor de los encoders se lee en **cuadratura por hardware** mediante dos timers. 37 | - El MPU se lee mediante SPI. 38 | - El bucle principal de control, del que constan los PID de velocidad lineal, velocidad angular, control frontal y control lateral se ejecuta cada 1ms. 39 | - Se realiza un reseteo de posición por périda de pared lateral para mejorar la navegación. 40 | - Las curvas se realizan mediante perfiles de giro con **aceleración senoidal** para mayor suavidad. 41 | - Dispone de programas para seguimiento de pared derecha/izquierda, exploración y resolución mediante floodfill basado en pesos (más información en [TimeBased FloodFill Simulator](https://github.com/OPRobots/TimeBased-FloodFill-Simulator)) y hardcodeo de movimientos. 42 | 43 | ## Victoria de ZoroBot3 en la OSHWDem2024 44 | https://github.com/user-attachments/assets/b749b48b-d331-48f7-bc95-2def68d67b7c 45 | -------------------------------------------------------------------------------- /ZoroBot3.code-workspace: -------------------------------------------------------------------------------- 1 | { 2 | "folders": [ 3 | { 4 | "path": "." 5 | }, 6 | { 7 | "path": "source_code" 8 | } 9 | ], 10 | "settings": { 11 | "files.associations": { 12 | "*.tpl": "html", 13 | "*.h": "c", 14 | "*.c": "c", 15 | } 16 | } 17 | } -------------------------------------------------------------------------------- /images/ZoroBot3_3d_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/images/ZoroBot3_3d_model.png -------------------------------------------------------------------------------- /images/ZoroBot3_finish_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/images/ZoroBot3_finish_1.png -------------------------------------------------------------------------------- /images/ZoroBot3_finish_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/images/ZoroBot3_finish_2.png -------------------------------------------------------------------------------- /images/ZoroBot3_finish_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/images/ZoroBot3_finish_3.png -------------------------------------------------------------------------------- /images/ZoroBot3_rev2_3d_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/images/ZoroBot3_rev2_3d_model.png -------------------------------------------------------------------------------- /images/ZoroBot3_rev2_3d_model_b.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/images/ZoroBot3_rev2_3d_model_b.png -------------------------------------------------------------------------------- /images/ZoroBot3_rev2_finish_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/images/ZoroBot3_rev2_finish_1.jpg -------------------------------------------------------------------------------- /images/ZoroBot3_rev2_finish_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/images/ZoroBot3_rev2_finish_2.jpg -------------------------------------------------------------------------------- /images/ZoroBot3_rev2_finish_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/images/ZoroBot3_rev2_finish_3.jpg -------------------------------------------------------------------------------- /pcb_files/bom/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/pcb_files/bom/.gitignore -------------------------------------------------------------------------------- /pcb_files/gerbers/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/pcb_files/gerbers/.gitignore -------------------------------------------------------------------------------- /pcb_files/gerbers/ZoroBot3.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/pcb_files/gerbers/ZoroBot3.zip -------------------------------------------------------------------------------- /pcb_files/gerbers/bom.csv: -------------------------------------------------------------------------------- 1 | Designator,Footprint,Quantity,Value,LCSC Part # 2 | "AD8418AWBZ_1, AD8418AWBZ_2",SOP8,2,~, 3 | "AS5145B_D1, AS5145B_I1",Encoder_AS5145B,2,AS5145B-HSST, 4 | AUX_CONN1,PinHeader_1x03_P2.54mm_Vertical,1,Conn_01x03_Pin, 5 | BATT1,JST_XH_B3B-XH-A_1x03_P2.50mm_Vertical,1,Conn_01x03_Pin, 6 | "BTN1, BTN2, BTN3, BTN_RST1",Generic_SmallButton_6x3,4,~, 7 | C1,0402,1,4.7uF, 8 | "C10, C11, C17, C2, C20, C21, C24, C25, C27, C28, C29, C33, C35, C36, C37, C38, C4, C40, C7, CR1, CR2, CR5, CS2, CS4, CS6, CS8",0402,26,0.1uF, 9 | "C12, C19",0402,2,2.2uF, 10 | "C13, C15",0402,2,20pF, 11 | "C18, C22, C32, C34, CR3",0402,5,1uF, 12 | "C23, C3, C30, C41, C42, C44, C45, C46, C47, C48, C49, C50, C51, C52, C6, C9, CS1, CS3, CS5, CS7",0402,20,10uF, 13 | "C26, C39",0402,2,0.01uF, 14 | "C31, C5, C8",0805,3,47uF, 15 | C43,0805,1,10uF, 16 | "CONN_SEN_FRONT_D1, CONN_SEN_FRONT_I1, CONN_SEN_PA_D1, CONN_SEN_PA_I1",ZoroBot3_Sensors,4,Conn_01x04_Socket, 17 | "CR4, CR7",0603,2,0.1uF, 18 | CR6,1206,1,47uF, 19 | "D1, D10, D2, D3, D4, D5, D6, D7, D8, D9",0805,10,LED, 20 | "D11, D12",0402,2,LED, 21 | D13,0805,1,D, 22 | "DATA_MPU1, DATA_MPU2, DATA_MPU3",ZoroBot3_Data_MPU,3,Conn_01x04_Socket, 23 | DC-DC1,Texas_HTSOP-8-1EP_3.9x4.9mm_P1.27mm_EP2.95x4.9mm_Mask2.4x3.1mm_ThermalVias,1,~, 24 | DEIXAME_QUE_PENSE_1,LQFP-64_10x10mm_P0.5mm,1,STM32F405RGT6, 25 | "ENC_D1, ENC_I1",ZoroBot3_PCBmain_Encoder,2,Conn_01x05_Socket, 26 | "ENC_D_CONN1, ENC_I_CONN1",ZoroBot3_PCBmodule_encoder,2,Conn_01x05_Pin, 27 | IR_RECIVER1,IR_SENSOR_SMD_TSSP77038ETR,1,~, 28 | JP2,SolderJumper-2_P1.3mm_Open_RoundedPad1.0x1.5mm,1,SolderJumper_2_Open, 29 | JP3,Generic_simple_1x_SMD_pin,1,Conn_01x01_Pin, 30 | L1,L_Wuerth_HCM-7050,1,L, 31 | "LED_IR1, LED_IR2, LED_IR3, LED_IR4",LED_D5.0mm_Clear,4,SFH-4550, 32 | LSM6DSR1,Bosch_LGA-14_3x2.5mm_P0.5mm,1,~, 33 | MIC1,SOT-23-5_HandSoldering,1,~, 34 | "MOSFET-N1, MOSFET-N2, MOSFET-N3, MOSFET-N4",SOT-23,4,AO3400 (A09T), 35 | "Mot_D1, Mot_I1, Mot_S1",Connector_motor_1.25_Hirose_DF13-02P-1.25DSA_1x02_P1.25mm_Vertical,3,Conn_01x02_Socket, 36 | "PH1, PH2",PH-MP6551GQB-P,2,~, 37 | "POWER_MPU1, POWER_MPU2, POWER_MPU3",ZoroBot3_Power_MPU,3,Conn_01x02_Socket, 38 | POWER_SWITCH1,Generic_Swich_ONOFF,1,SW_DPDT_x2, 39 | Q1,SOT-23,1,NMOS, 40 | Q2,DFN-8-1EP_3x3mm_P0.65mm_EP1.55x2.4mm,1,AON7423, 41 | "Q3, Q4, Q5, Q6",LED_D5.0mm_IRBlack,4,ST-1KL3A, 42 | "R1, R10, R14, R15, R16, R2, R22, R3, R4, R5, R6, R7, R8, R9",0402,14,330, 43 | "R11, R17, R19, R21, RS11, RS12, RS5, RS6",0402,8,47K, 44 | "R12, R13",2512,2,0.05, 45 | R18,0402,1,200K, 46 | "R20, R23, R28",0402,3,1K, 47 | "R24, R30",0402,2,100K, 48 | "R25, R27",0402,2,10K, 49 | R26,0402,1,20K, 50 | R29,0402,1,45K, 51 | R31,0402,1,60k, 52 | RGB1,LED_RGB_5050-6,1,LED_RGB, 53 | "RS1, RS2, RS7, RS8",0805,4,150, 54 | "RS10, RS3, RS4, RS9",0805,4,6.8K, 55 | "SEN_FRONT_D1, SEN_FRONT_I1, SEN_PARED_D1, SEN_PARED_I1",ZoroBot3_Main_sensors,4,Conn_01x04_Socket, 56 | "U1, U2",InvenSense_QFN-24_3x3mm_P0.4mm,2,MPU-6500, 57 | USB1,USB-C ST-Link USART,1,~, 58 | Y1,Crystal_SMD_TXC_7M-4Pin_3.2x2.5mm,1,Crystal_GND24, 59 | -------------------------------------------------------------------------------- /pcb_files/gerbers/designators.csv: -------------------------------------------------------------------------------- 1 | AD8418AWBZ_1:1 2 | AD8418AWBZ_2:1 3 | AS5145B_D1:1 4 | AS5145B_I1:1 5 | AUX_CONN1:1 6 | BATT1:1 7 | BTN1:1 8 | BTN2:1 9 | BTN3:1 10 | BTN_RST1:1 11 | C1:1 12 | C10:1 13 | C11:1 14 | C12:1 15 | C13:1 16 | C15:1 17 | C17:1 18 | C18:1 19 | C19:1 20 | C2:1 21 | C20:1 22 | C21:1 23 | C22:1 24 | C23:1 25 | C24:1 26 | C25:1 27 | C26:1 28 | C27:1 29 | C28:1 30 | C29:1 31 | C3:1 32 | C30:1 33 | C31:1 34 | C32:1 35 | C33:1 36 | C34:1 37 | C35:1 38 | C36:1 39 | C37:1 40 | C38:1 41 | C39:1 42 | C4:1 43 | C40:1 44 | C41:1 45 | C42:1 46 | C43:1 47 | C44:1 48 | C45:1 49 | C46:1 50 | C47:1 51 | C48:1 52 | C49:1 53 | C5:1 54 | C50:1 55 | C51:1 56 | C52:1 57 | C6:1 58 | C7:1 59 | C8:1 60 | C9:1 61 | CONN_SEN_FRONT_D1:1 62 | CONN_SEN_FRONT_I1:1 63 | CONN_SEN_PA_D1:1 64 | CONN_SEN_PA_I1:1 65 | CR1:1 66 | CR2:1 67 | CR3:1 68 | CR4:1 69 | CR5:1 70 | CR6:1 71 | CR7:1 72 | CS1:1 73 | CS2:1 74 | CS3:1 75 | CS4:1 76 | CS5:1 77 | CS6:1 78 | CS7:1 79 | CS8:1 80 | D1:1 81 | D10:1 82 | D11:1 83 | D12:1 84 | D13:1 85 | D2:1 86 | D3:1 87 | D4:1 88 | D5:1 89 | D6:1 90 | D7:1 91 | D8:1 92 | D9:1 93 | DATA_MPU1:1 94 | DATA_MPU2:1 95 | DATA_MPU3:1 96 | DC-DC1:1 97 | DEIXAME_QUE_PENSE_1:1 98 | ENC_D1:1 99 | ENC_D_CONN1:1 100 | ENC_I1:1 101 | ENC_I_CONN1:1 102 | G***:4 103 | IR_RECIVER1:1 104 | JP2:1 105 | JP3:1 106 | L1:1 107 | LED_IR1:1 108 | LED_IR2:1 109 | LED_IR3:1 110 | LED_IR4:1 111 | LSM6DSR1:1 112 | MIC1:1 113 | MOSFET-N1:1 114 | MOSFET-N2:1 115 | MOSFET-N3:1 116 | MOSFET-N4:1 117 | Mot_D1:1 118 | Mot_I1:1 119 | Mot_S1:1 120 | PH1:1 121 | PH2:1 122 | POWER_MPU1:1 123 | POWER_MPU2:1 124 | POWER_MPU3:1 125 | POWER_SWITCH1:1 126 | Q1:1 127 | Q2:1 128 | Q3:1 129 | Q4:1 130 | Q5:1 131 | Q6:1 132 | R1:1 133 | R10:1 134 | R11:1 135 | R12:1 136 | R13:1 137 | R14:1 138 | R15:1 139 | R16:1 140 | R17:1 141 | R18:1 142 | R19:1 143 | R2:1 144 | R20:1 145 | R21:1 146 | R22:1 147 | R23:1 148 | R24:1 149 | R25:1 150 | R26:1 151 | R27:1 152 | R28:1 153 | R29:1 154 | R3:1 155 | R30:1 156 | R31:1 157 | R4:1 158 | R5:1 159 | R6:1 160 | R7:1 161 | R8:1 162 | R9:1 163 | RGB1:1 164 | RS1:1 165 | RS10:1 166 | RS11:1 167 | RS12:1 168 | RS2:1 169 | RS3:1 170 | RS4:1 171 | RS5:1 172 | RS6:1 173 | RS7:1 174 | RS8:1 175 | RS9:1 176 | SEN_FRONT_D1:1 177 | SEN_FRONT_I1:1 178 | SEN_PARED_D1:1 179 | SEN_PARED_I1:1 180 | U1:1 181 | U2:1 182 | USB1:1 183 | Y1:1 184 | kibuzzard-6779A98A:1 185 | kibuzzard-6779A9B1:1 186 | kibuzzard-677A7C8F:1 187 | kibuzzard-677A7C9A:1 188 | kibuzzard-677A7CA3:1 189 | kibuzzard-677A7CAF:1 190 | kibuzzard-677A7CBB:1 191 | kibuzzard-677A7D71:1 192 | kibuzzard-677A7D7B:1 193 | kibuzzard-677A7D86:1 194 | kibuzzard-677A7E2E:1 195 | kibuzzard-677A7E3B:1 196 | -------------------------------------------------------------------------------- /pcb_files/gerbers/positions.csv: -------------------------------------------------------------------------------- 1 | Designator,Mid X,Mid Y,Rotation,Layer 2 | AD8418AWBZ_1,66.089469,47.846292,90.0,top 3 | AD8418AWBZ_2,35.976969,48.096292,270.0,top 4 | AS5145B_D1,79.264053,47.548792,270.0,top 5 | AS5145B_I1,22.611206,47.541824,270.0,top 6 | AUX_CONN1,70.924469,7.446292,90.0,top 7 | BATT1,62.939469,25.501292,0.0,top 8 | BTN1,78.889469,17.996292,0.0,top 9 | BTN2,82.339469,13.696292,90.0,top 10 | BTN3,78.889469,9.396292,0.0,top 11 | BTN_RST1,50.939469,18.096292,0.0,top 12 | C1,29.339469,22.621292,0.0,top 13 | C10,43.3,24.338333,0.0,top 14 | C11,41.889469,19.896292,0.0,top 15 | C12,41.889469,20.896292,0.0,top 16 | C13,26.864469,17.321292,90.0,top 17 | C15,26.864469,15.121292,270.0,top 18 | C17,25.739469,11.521292,270.0,top 19 | C18,24.814469,11.521292,270.0,top 20 | C19,41.489469,7.596292,90.0,top 21 | C2,29.339469,21.696292,0.0,top 22 | C20,42.456135,7.596292,90.0,top 23 | C21,28.839469,6.896292,270.0,top 24 | C22,27.914469,6.896292,270.0,top 25 | C23,23.889469,11.521292,270.0,top 26 | C24,29.764469,6.896292,270.0,top 27 | C25,49.831969,42.971292,180.0,top 28 | C26,52.031969,42.971292,0.0,top 29 | C27,50.964469,49.071292,0.0,top 30 | C28,37.339469,43.096292,180.0,top 31 | C29,64.789469,43.046292,0.0,top 32 | C3,26.789469,56.996292,7.0,top 33 | C30,70.939469,13.946292,0.0,top 34 | C31,73.039469,12.746292,90.0,top 35 | C32,79.270164,43.828792,0.0,bottom 36 | C33,79.270164,44.778792,0.0,bottom 37 | C34,22.589816,44.771824,0.0,bottom 38 | C35,22.589816,43.821824,0.0,bottom 39 | C36,16.739469,44.998792,270.0,top 40 | C37,85.244469,44.857683,270.0,top 41 | C38,85.244469,47.040182,90.0,top 42 | C39,16.739469,47.198792,90.0,top 43 | C4,27.139469,63.071292,277.0,top 44 | C40,10.671969,45.996292,90.0,top 45 | C41,43.3,29.06,0.0,top 46 | C42,43.3,28.101668,0.0,top 47 | C43,49.52,21.46,90.0,top 48 | C44,70.939469,11.096292,0.0,top 49 | C45,70.939469,12.996292,0.0,top 50 | C46,70.939469,12.046292,0.0,top 51 | C47,73.339469,23.346292,90.0,top 52 | C48,71.284468,23.346292,90.0,top 53 | C49,72.311968,23.346292,90.0,top 54 | C5,55.27,16.1,90.0,top 55 | C50,69.229468,23.346292,90.0,top 56 | C51,70.256968,23.346292,90.0,top 57 | C52,70.939469,10.146292,0.0,top 58 | C6,43.3,27.18,0.0,top 59 | C7,43.3,26.254998,0.0,top 60 | C8,52.35,21.45,90.0,top 61 | C9,43.3,25.296664,0.0,top 62 | CONN_SEN_FRONT_D1,90.263998,60.371292,0.0,top 63 | CONN_SEN_FRONT_I1,11.544469,31.586291,180.0,top 64 | CONN_SEN_PA_D1,90.274998,31.556292,180.0,top 65 | CONN_SEN_PA_I1,11.599469,60.381292,0.0,top 66 | CR1,52.089469,15.946292,0.0,top 67 | CR2,61.439469,13.196292,90.0,top 68 | CR3,55.739469,20.646292,180.0,top 69 | CR4,74.639469,20.646292,270.0,top 70 | CR5,65.689469,13.196292,90.0,top 71 | CR6,59.314469,15.496292,90.0,top 72 | CR7,74.639469,17.646292,270.0,top 73 | CS1,91.559468,29.461292,180.0,bottom 74 | CS2,91.559468,28.541292,180.0,bottom 75 | CS3,10.31913,63.381292,0.0,bottom 76 | CS4,10.324469,62.456292,0.0,bottom 77 | CS5,88.984469,63.396292,0.0,bottom 78 | CS6,88.984469,62.471292,0.0,bottom 79 | CS7,12.87447,28.591291,180.0,bottom 80 | CS8,12.874469,29.516291,180.0,bottom 81 | D1,35.03,77.18,0.0,top 82 | D10,66.91,77.18,180.0,top 83 | D11,78.889469,13.696292,180.0,top 84 | D12,44.389467,7.596292,270.0,top 85 | D13,57.214469,15.946292,270.0,top 86 | D2,37.08,80.0,0.0,top 87 | D3,39.59,82.47,0.0,top 88 | D4,42.23,84.41,0.0,top 89 | D5,45.58,85.36,0.0,top 90 | D6,56.36,85.36,180.0,top 91 | D7,59.71,84.41,180.0,top 92 | D8,62.35,82.47,180.0,top 93 | D9,64.86,80.0,180.0,top 94 | DATA_MPU1,58.139469,45.946292,0.0,top 95 | DATA_MPU2,88.126969,50.114902,90.0,top 96 | DATA_MPU3,13.704469,50.096292,90.0,top 97 | DC-DC1,63.239469,18.246292,90.0,top 98 | DEIXAME_QUE_PENSE_1,34.939469,14.696292,270.0,top 99 | ENC_D1,82.457469,26.942058,0.0,top 100 | ENC_D_CONN1,79.264053,40.978792,0.0,top 101 | ENC_I1,19.396469,26.942052,180.0,top 102 | ENC_I_CONN1,22.611206,40.959824,0.0,top 103 | IR_RECIVER1,21.189469,15.446292,90.0,top 104 | JP3,88.111969,43.436292,180.0,top 105 | L1,69.889469,18.296292,0.0,top 106 | LED_IR1,90.288077,20.336551,180.0,top 107 | LED_IR2,11.579808,71.601034,0.0,top 108 | LED_IR3,90.249383,71.591033,0.0,top 109 | LED_IR4,11.56603,20.37155,180.0,top 110 | LSM6DSR1,88.179469,46.053792,0.0,top 111 | MIC1,68.039469,12.046292,90.0,top 112 | MOSFET-N1,89.321968,23.606292,90.0,bottom 113 | MOSFET-N2,12.574469,68.331292,270.0,bottom 114 | MOSFET-N3,91.22197,68.321292,270.0,bottom 115 | MOSFET-N4,10.624469,23.691291,90.0,bottom 116 | Mot_D1,66.639469,55.45,90.0,top 117 | Mot_I1,35.239469,55.45,270.0,top 118 | Mot_S1,25.189469,59.496292,187.0,top 119 | PH1,54.42,26.02,0.0,top 120 | PH2,47.49,26.05125,0.0,top 121 | POWER_MPU1,43.739469,45.971292,0.0,top 122 | POWER_MPU2,88.11197,41.886292,90.0,top 123 | POWER_MPU3,13.671969,41.917682,90.0,top 124 | POWER_SWITCH1,61.639469,8.631292,0.0,top 125 | Q1,20.689469,58.896292,7.0,top 126 | Q2,58.614468,19.946292,180.0,top 127 | Q3,90.294468,26.881292,0.0,top 128 | Q4,11.614469,65.056292,180.0,top 129 | Q5,90.249469,65.046292,180.0,top 130 | Q6,11.60947,26.916292,0.0,top 131 | R1,40.139469,39.846292,90.0,top 132 | R10,61.739469,39.846292,90.0,top 133 | R11,18.164469,59.196292,277.0,top 134 | R12,61.439469,47.846292,90.0,top 135 | R13,40.589469,48.096292,270.0,top 136 | R14,51.889469,39.846292,90.0,top 137 | R15,50.939469,39.846292,90.0,top 138 | R16,49.989469,39.846292,90.0,top 139 | R17,29.349469,23.561292,180.0,top 140 | R18,76.039469,13.696292,0.0,top 141 | R19,76.041969,12.746292,0.0,top 142 | R2,41.089469,39.846292,90.0,top 143 | R20,76.039469,11.796292,0.0,top 144 | R21,49.889469,15.946292,180.0,top 145 | R22,76.034469,14.646292,0.0,top 146 | R23,43.422801,7.591092,270.0,top 147 | R24,76.036969,15.596292,0.0,top 148 | R25,46.36,21.0875,270.0,top 149 | R26,46.36,18.88,270.0,top 150 | R27,55.739469,19.683792,0.0,top 151 | R28,55.739469,18.721292,180.0,top 152 | R29,63.569469,13.196292,90.0,top 153 | R3,42.039469,39.846292,90.0,top 154 | R30,62.499469,13.196292,90.0,top 155 | R31,64.639469,13.196292,90.0,top 156 | R4,42.989469,39.846292,90.0,top 157 | R5,43.939469,39.846292,90.0,top 158 | R6,57.939469,39.846292,90.0,top 159 | R7,58.889469,39.846292,90.0,top 160 | R8,59.839469,39.846292,90.0,top 161 | R9,60.789469,39.846292,90.0,top 162 | RGB1,50.939469,87.6,0.0,top 163 | RS1,90.288078,18.331292,0.0,bottom 164 | RS10,13.27447,23.641293,270.0,bottom 165 | RS11,91.534469,62.946292,0.0,bottom 166 | RS12,10.324469,29.041291,180.0,bottom 167 | RS2,11.579808,73.606292,180.0,bottom 168 | RS3,92.009468,23.606292,270.0,bottom 169 | RS4,9.874469,68.331292,90.0,bottom 170 | RS5,89.019468,29.001293,180.0,bottom 171 | RS6,12.899469,62.906292,0.0,bottom 172 | RS7,90.249383,73.596292,180.0,bottom 173 | RS8,11.566031,18.366291,0.0,bottom 174 | RS9,88.534469,68.321292,90.0,bottom 175 | SEN_FRONT_D1,79.339469,63.546292,173.0,top 176 | SEN_FRONT_I1,22.489469,63.546292,187.0,top 177 | SEN_PARED_D1,34.289469,86.846292,128.2,top 178 | SEN_PARED_I1,67.489469,86.796292,231.8,top 179 | U1,50.939469,45.983156,180.0,top 180 | U2,13.689469,45.996292,270.0,top 181 | USB1,50.939469,9.446292,0.0,top 182 | Y1,24.564469,16.171292,90.0,top 183 | -------------------------------------------------------------------------------- /pcb_files/kicad_project/.gitignore: -------------------------------------------------------------------------------- 1 | *-backups 2 | production 3 | _autosave* 4 | *.lck -------------------------------------------------------------------------------- /pcb_files/kicad_project/ZoroBot3.kicad_dru: -------------------------------------------------------------------------------- 1 | (version 1) 2 | # Custom Design Rules (DRC) for KiCAD 7.0 (Stored in '.kicad_dru' file). 3 | # 4 | # Matching JLCPCB capabilities: https://jlcpcb.com/capabilities/pcb-capabilities 5 | # 6 | # KiCad documentation: https://docs.kicad.org/master/id/pcbnew/pcbnew_advanced.html#custom_design_rules 7 | # 8 | # Inspiration 9 | # - https://gist.github.com/darkxst/f713268e5469645425eed40115fb8b49 (with comments) 10 | # - https://gist.github.com/denniskupec/e163d13b0a64c2044bd259f64659485e (with comments) 11 | 12 | # TODO new rule: NPTH pads. 13 | # Inner diameter of pad should be 0.4-0.5 mm larger than NPTH drill diameter. 14 | # JLCPCB: "We make NPTH via dry sealing film process, if customer would like a NPTH but around with pad/copper, our engineer will dig out around pad/copper about 0.2mm-0.25mm, otherwise the metal potion will be flowed into the hole and it becomes a PTH. (there will be no copper dig out optimization for single board)." 15 | 16 | # TODO: new rule for plated slots: min diameter/width 0.5mm 17 | # JLCPCB: "The minimum plated slot width is 0.5mm, which is drawn with a pad." 18 | 19 | # TODO new rule: non-plated slots: min diameter/width 1.0mm 20 | # JLCPCB: "The minimum Non-Plated Slot Width is 1.0mm, please draw the slot outline in the mechanical layer(GML or GKO)"" 21 | 22 | (rule "Track width, outer layer (1oz copper)" 23 | (layer outer) 24 | (condition "A.Type == 'track'") 25 | (constraint track_width (min 0.127mm)) 26 | ) 27 | 28 | (rule "Track spacing, outer layer (1oz copper)" 29 | (layer outer) 30 | (condition "A.Type == 'track' && B.Type == A.Type") 31 | (constraint clearance (min 0.127mm)) 32 | ) 33 | 34 | (rule "Track width, inner layer" 35 | (layer inner) 36 | (condition "A.Type == 'track'") 37 | (constraint track_width (min 0.09mm)) 38 | ) 39 | 40 | (rule "Track spacing, inner layer" 41 | (layer inner) 42 | (condition "A.Type == 'track' && B.Type == A.Type") 43 | (constraint clearance (min 0.09mm)) 44 | ) 45 | 46 | (rule "Silkscreen text" 47 | (layer "?.Silkscreen") 48 | (condition "A.Type == 'Text' || A.Type == 'Text Box'") 49 | (constraint text_thickness (min 0.15mm)) 50 | (constraint text_height (min 1mm)) 51 | ) 52 | 53 | (rule "Pad to Silkscreen" 54 | (layer outer) 55 | (condition "A.Type == 'pad' && B.Layer == '?.Silkscreen'") 56 | (constraint silk_clearance (min 0.15mm)) 57 | ) 58 | 59 | (rule "Edge (routed) to track clearance" 60 | (condition "A.Type == 'track'") 61 | (constraint edge_clearance (min 0.3mm)) 62 | ) 63 | 64 | #(rule "Edge (v-cut) to track clearance" 65 | # (condition "A.Type == 'track'") 66 | # (constraint edge_clearance (min 0.4mm)) 67 | #) 68 | 69 | # JLCPCB restrictions ambiguous: 70 | # Illustration: 0.2 mm, 1&2 layer: 0.3 mm, multilayer: "(0.15mm more costly)" 71 | # This rule handles diameter minimum and maximum for ALL holes. 72 | # Other specialized rules handle restrictions (e.g. Via, PTH, NPTH) 73 | (rule "Hole diameter" 74 | (constraint hole_size (min 0.2mm) (max 6.3mm)) 75 | ) 76 | 77 | (rule "Hole (NPTH) diameter" 78 | (layer outer) 79 | (condition "!A.isPlated()") 80 | (constraint hole_size (min 0.5mm)) 81 | ) 82 | 83 | # TODO: Hole to board edge ≥ 1 mm. Min. board size 10 × 10 mm 84 | (rule "Hole (castellated) diameter" 85 | (layer outer) 86 | (condition "A.Type == 'pad' && A.Fabrication_Property == 'Castellated pad'") 87 | (constraint hole_size (min 0.6mm)) 88 | ) 89 | 90 | # JLCPCB: "Via diameter should be 0.1mm(0.15mm preferred) larger than Via hole size" (illustration shows diameters for both dimensions) 91 | # JLCPCB: PTH: "The annular ring size will be enlarged to 0.15mm in production." 92 | (rule "Annular ring width (via and PTH)" 93 | (layer outer) 94 | (condition "A.isPlated()") 95 | (constraint annular_width (min 0.075mm)) 96 | ) 97 | 98 | (rule "Clearance: hole to hole (perimeter), different nets" 99 | (layer outer) 100 | (condition "A.Net != B.Net") 101 | (constraint hole_to_hole (min 0.5mm)) 102 | ) 103 | 104 | (rule "Clearance: hole to hole (perimeter), same net" 105 | (layer outer) 106 | (condition "A.Net == B.Net") 107 | (constraint hole_to_hole (min 0.254mm)) 108 | ) 109 | 110 | (rule "Clearance: track to NPTH hole (perimeter)" 111 | # (condition "A.Pad_Type == 'NPTH, mechanical' && B.Type == 'track' && A.Net != B.Net") 112 | (condition "!A.isPlated() && B.Type == 'track' && A.Net != B.Net") 113 | (constraint hole_clearance (min 0.254mm)) 114 | ) 115 | 116 | (rule "Clearance: track to PTH hole perimeter" 117 | (condition "A.isPlated() && B.Type == 'track' && A.Net != B.Net") 118 | (constraint hole_clearance (min 0.33mm)) 119 | ) 120 | 121 | # TODO: try combining with rule "Clearance: PTH to track, different nets" 122 | (rule "Clearance: track to pad" 123 | (condition "A.Type == 'pad' && B.Type == 'track' && A.Net != B.Net") 124 | (constraint clearance (min 0.2mm)) 125 | ) 126 | 127 | (rule "Clearance: pad/via to pad/via" 128 | (layer outer) 129 | # (condition "(A.Type == 'Pad' || A.Type == 'Via') && (B.Type == 'Pad' || B.Type == 'Via') && A.Net != B.Net") 130 | (condition "A.isPlated() && B.isPlated() && A.Net != B.Net") 131 | (constraint clearance (min 0.127mm)) 132 | ) 133 | 134 | -------------------------------------------------------------------------------- /pcb_files/kicad_project/ZoroBot3.kicad_prl: -------------------------------------------------------------------------------- 1 | { 2 | "board": { 3 | "active_layer": 0, 4 | "active_layer_preset": "", 5 | "auto_track_width": true, 6 | "hidden_netclasses": [], 7 | "hidden_nets": [], 8 | "high_contrast_mode": 0, 9 | "net_color_mode": 1, 10 | "opacity": { 11 | "images": 0.6, 12 | "pads": 1.0, 13 | "tracks": 1.0, 14 | "vias": 1.0, 15 | "zones": 0.6 16 | }, 17 | "selection_filter": { 18 | "dimensions": true, 19 | "footprints": true, 20 | "graphics": true, 21 | "keepouts": true, 22 | "lockedItems": true, 23 | "otherItems": true, 24 | "pads": true, 25 | "text": true, 26 | "tracks": true, 27 | "vias": true, 28 | "zones": true 29 | }, 30 | "visible_items": [ 31 | 0, 32 | 1, 33 | 2, 34 | 3, 35 | 4, 36 | 5, 37 | 8, 38 | 9, 39 | 10, 40 | 11, 41 | 12, 42 | 13, 43 | 15, 44 | 16, 45 | 17, 46 | 18, 47 | 19, 48 | 20, 49 | 21, 50 | 22, 51 | 23, 52 | 24, 53 | 25, 54 | 26, 55 | 27, 56 | 28, 57 | 29, 58 | 30, 59 | 32, 60 | 33, 61 | 34, 62 | 35, 63 | 37, 64 | 39, 65 | 40 66 | ], 67 | "visible_layers": "ffcffef_ffffffff", 68 | "zone_display_mode": 0 69 | }, 70 | "git": { 71 | "repo_password": "", 72 | "repo_type": "", 73 | "repo_username": "", 74 | "ssh_key": "" 75 | }, 76 | "meta": { 77 | "filename": "ZoroBot3.kicad_prl", 78 | "version": 3 79 | }, 80 | "project": { 81 | "files": [] 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /pcb_files/kicad_project/ZoroBot3.kicad_pro: -------------------------------------------------------------------------------- 1 | { 2 | "board": { 3 | "3dviewports": [], 4 | "design_settings": { 5 | "defaults": { 6 | "apply_defaults_to_fp_fields": false, 7 | "apply_defaults_to_fp_shapes": false, 8 | "apply_defaults_to_fp_text": false, 9 | "board_outline_line_width": 0.1, 10 | "copper_line_width": 0.2, 11 | "copper_text_italic": false, 12 | "copper_text_size_h": 1.5, 13 | "copper_text_size_v": 1.5, 14 | "copper_text_thickness": 0.3, 15 | "copper_text_upright": false, 16 | "courtyard_line_width": 0.05, 17 | "dimension_precision": 4, 18 | "dimension_units": 3, 19 | "dimensions": { 20 | "arrow_length": 1270000, 21 | "extension_offset": 500000, 22 | "keep_text_aligned": true, 23 | "suppress_zeroes": false, 24 | "text_position": 0, 25 | "units_format": 1 26 | }, 27 | "fab_line_width": 0.1, 28 | "fab_text_italic": false, 29 | "fab_text_size_h": 1.0, 30 | "fab_text_size_v": 1.0, 31 | "fab_text_thickness": 0.15, 32 | "fab_text_upright": false, 33 | "other_line_width": 0.15, 34 | "other_text_italic": false, 35 | "other_text_size_h": 1.0, 36 | "other_text_size_v": 1.0, 37 | "other_text_thickness": 0.15, 38 | "other_text_upright": false, 39 | "pads": { 40 | "drill": 0.762, 41 | "height": 1.524, 42 | "width": 1.524 43 | }, 44 | "silk_line_width": 0.15, 45 | "silk_text_italic": false, 46 | "silk_text_size_h": 1.0, 47 | "silk_text_size_v": 1.0, 48 | "silk_text_thickness": 0.15, 49 | "silk_text_upright": false, 50 | "zones": { 51 | "min_clearance": 0.22 52 | } 53 | }, 54 | "diff_pair_dimensions": [ 55 | { 56 | "gap": 0.0, 57 | "via_gap": 0.0, 58 | "width": 0.0 59 | } 60 | ], 61 | "drc_exclusions": [], 62 | "meta": { 63 | "version": 2 64 | }, 65 | "rule_severities": { 66 | "annular_width": "error", 67 | "clearance": "error", 68 | "connection_width": "warning", 69 | "copper_edge_clearance": "error", 70 | "copper_sliver": "warning", 71 | "courtyards_overlap": "error", 72 | "diff_pair_gap_out_of_range": "error", 73 | "diff_pair_uncoupled_length_too_long": "error", 74 | "drill_out_of_range": "error", 75 | "duplicate_footprints": "warning", 76 | "extra_footprint": "warning", 77 | "footprint": "error", 78 | "footprint_symbol_mismatch": "warning", 79 | "footprint_type_mismatch": "ignore", 80 | "hole_clearance": "error", 81 | "hole_near_hole": "error", 82 | "holes_co_located": "warning", 83 | "invalid_outline": "error", 84 | "isolated_copper": "warning", 85 | "item_on_disabled_layer": "error", 86 | "items_not_allowed": "error", 87 | "length_out_of_range": "error", 88 | "lib_footprint_issues": "warning", 89 | "lib_footprint_mismatch": "warning", 90 | "malformed_courtyard": "error", 91 | "microvia_drill_out_of_range": "error", 92 | "missing_courtyard": "ignore", 93 | "missing_footprint": "warning", 94 | "net_conflict": "warning", 95 | "npth_inside_courtyard": "ignore", 96 | "padstack": "warning", 97 | "pth_inside_courtyard": "ignore", 98 | "shorting_items": "error", 99 | "silk_edge_clearance": "warning", 100 | "silk_over_copper": "warning", 101 | "silk_overlap": "warning", 102 | "skew_out_of_range": "error", 103 | "solder_mask_bridge": "error", 104 | "starved_thermal": "error", 105 | "text_height": "warning", 106 | "text_thickness": "warning", 107 | "through_hole_pad_without_hole": "error", 108 | "too_many_vias": "error", 109 | "track_dangling": "warning", 110 | "track_width": "error", 111 | "tracks_crossing": "error", 112 | "unconnected_items": "error", 113 | "unresolved_variable": "error", 114 | "via_dangling": "warning", 115 | "zones_intersect": "error" 116 | }, 117 | "rules": { 118 | "max_error": 0.005, 119 | "min_clearance": 0.0, 120 | "min_connection": 0.0, 121 | "min_copper_edge_clearance": 0.0, 122 | "min_hole_clearance": 0.25, 123 | "min_hole_to_hole": 0.25, 124 | "min_microvia_diameter": 0.2, 125 | "min_microvia_drill": 0.1, 126 | "min_resolved_spokes": 1, 127 | "min_silk_clearance": 0.0, 128 | "min_text_height": 0.8, 129 | "min_text_thickness": 0.08, 130 | "min_through_hole_diameter": 0.3, 131 | "min_track_width": 0.0, 132 | "min_via_annular_width": 0.1, 133 | "min_via_diameter": 0.5, 134 | "solder_mask_clearance": 0.0, 135 | "solder_mask_min_width": 0.0, 136 | "solder_mask_to_copper_clearance": 0.005, 137 | "use_height_for_length_calcs": true 138 | }, 139 | "teardrop_options": [ 140 | { 141 | "td_onpadsmd": true, 142 | "td_onroundshapesonly": false, 143 | "td_ontrackend": false, 144 | "td_onviapad": true 145 | } 146 | ], 147 | "teardrop_parameters": [ 148 | { 149 | "td_allow_use_two_tracks": true, 150 | "td_curve_segcount": 0, 151 | "td_height_ratio": 1.0, 152 | "td_length_ratio": 0.5, 153 | "td_maxheight": 2.0, 154 | "td_maxlen": 1.0, 155 | "td_on_pad_in_zone": false, 156 | "td_target_name": "td_round_shape", 157 | "td_width_to_size_filter_ratio": 0.9 158 | }, 159 | { 160 | "td_allow_use_two_tracks": true, 161 | "td_curve_segcount": 0, 162 | "td_height_ratio": 1.0, 163 | "td_length_ratio": 0.5, 164 | "td_maxheight": 2.0, 165 | "td_maxlen": 1.0, 166 | "td_on_pad_in_zone": false, 167 | "td_target_name": "td_rect_shape", 168 | "td_width_to_size_filter_ratio": 0.9 169 | }, 170 | { 171 | "td_allow_use_two_tracks": true, 172 | "td_curve_segcount": 0, 173 | "td_height_ratio": 1.0, 174 | "td_length_ratio": 0.5, 175 | "td_maxheight": 2.0, 176 | "td_maxlen": 1.0, 177 | "td_on_pad_in_zone": false, 178 | "td_target_name": "td_track_end", 179 | "td_width_to_size_filter_ratio": 0.9 180 | } 181 | ], 182 | "track_widths": [ 183 | 0.0 184 | ], 185 | "tuning_pattern_settings": { 186 | "diff_pair_defaults": { 187 | "corner_radius_percentage": 80, 188 | "corner_style": 1, 189 | "max_amplitude": 1.0, 190 | "min_amplitude": 0.2, 191 | "single_sided": false, 192 | "spacing": 1.0 193 | }, 194 | "diff_pair_skew_defaults": { 195 | "corner_radius_percentage": 80, 196 | "corner_style": 1, 197 | "max_amplitude": 1.0, 198 | "min_amplitude": 0.2, 199 | "single_sided": false, 200 | "spacing": 0.6 201 | }, 202 | "single_track_defaults": { 203 | "corner_radius_percentage": 80, 204 | "corner_style": 1, 205 | "max_amplitude": 1.0, 206 | "min_amplitude": 0.2, 207 | "single_sided": false, 208 | "spacing": 0.6 209 | } 210 | }, 211 | "via_dimensions": [ 212 | { 213 | "diameter": 0.0, 214 | "drill": 0.0 215 | } 216 | ], 217 | "zones_allow_external_fillets": false 218 | }, 219 | "ipc2581": { 220 | "dist": "", 221 | "distpn": "", 222 | "internal_id": "", 223 | "mfg": "", 224 | "mpn": "" 225 | }, 226 | "layer_presets": [], 227 | "viewports": [] 228 | }, 229 | "boards": [], 230 | "cvpcb": { 231 | "equivalence_files": [] 232 | }, 233 | "erc": { 234 | "erc_exclusions": [], 235 | "meta": { 236 | "version": 0 237 | }, 238 | "pin_map": [ 239 | [ 240 | 0, 241 | 0, 242 | 0, 243 | 0, 244 | 0, 245 | 0, 246 | 1, 247 | 0, 248 | 0, 249 | 0, 250 | 0, 251 | 2 252 | ], 253 | [ 254 | 0, 255 | 0, 256 | 0, 257 | 1, 258 | 0, 259 | 0, 260 | 1, 261 | 0, 262 | 0, 263 | 2, 264 | 2, 265 | 2 266 | ], 267 | [ 268 | 0, 269 | 0, 270 | 0, 271 | 0, 272 | 0, 273 | 0, 274 | 1, 275 | 0, 276 | 0, 277 | 0, 278 | 1, 279 | 2 280 | ], 281 | [ 282 | 0, 283 | 1, 284 | 0, 285 | 0, 286 | 0, 287 | 0, 288 | 1, 289 | 1, 290 | 0, 291 | 1, 292 | 1, 293 | 2 294 | ], 295 | [ 296 | 0, 297 | 0, 298 | 0, 299 | 0, 300 | 0, 301 | 0, 302 | 1, 303 | 0, 304 | 0, 305 | 0, 306 | 0, 307 | 2 308 | ], 309 | [ 310 | 0, 311 | 0, 312 | 0, 313 | 0, 314 | 0, 315 | 0, 316 | 0, 317 | 0, 318 | 0, 319 | 0, 320 | 0, 321 | 2 322 | ], 323 | [ 324 | 1, 325 | 1, 326 | 1, 327 | 1, 328 | 1, 329 | 0, 330 | 1, 331 | 1, 332 | 1, 333 | 1, 334 | 1, 335 | 2 336 | ], 337 | [ 338 | 0, 339 | 0, 340 | 0, 341 | 1, 342 | 0, 343 | 0, 344 | 1, 345 | 0, 346 | 0, 347 | 0, 348 | 0, 349 | 2 350 | ], 351 | [ 352 | 0, 353 | 0, 354 | 0, 355 | 0, 356 | 0, 357 | 0, 358 | 1, 359 | 0, 360 | 0, 361 | 2, 362 | 2, 363 | 2 364 | ], 365 | [ 366 | 0, 367 | 2, 368 | 0, 369 | 1, 370 | 0, 371 | 0, 372 | 1, 373 | 0, 374 | 2, 375 | 0, 376 | 0, 377 | 2 378 | ], 379 | [ 380 | 0, 381 | 2, 382 | 1, 383 | 1, 384 | 0, 385 | 0, 386 | 1, 387 | 0, 388 | 2, 389 | 0, 390 | 0, 391 | 2 392 | ], 393 | [ 394 | 2, 395 | 2, 396 | 2, 397 | 2, 398 | 2, 399 | 2, 400 | 2, 401 | 2, 402 | 2, 403 | 2, 404 | 2, 405 | 2 406 | ] 407 | ], 408 | "rule_severities": { 409 | "bus_definition_conflict": "error", 410 | "bus_entry_needed": "error", 411 | "bus_to_bus_conflict": "error", 412 | "bus_to_net_conflict": "error", 413 | "conflicting_netclasses": "error", 414 | "different_unit_footprint": "error", 415 | "different_unit_net": "error", 416 | "duplicate_reference": "error", 417 | "duplicate_sheet_names": "error", 418 | "endpoint_off_grid": "warning", 419 | "extra_units": "error", 420 | "global_label_dangling": "warning", 421 | "hier_label_mismatch": "error", 422 | "label_dangling": "error", 423 | "lib_symbol_issues": "warning", 424 | "missing_bidi_pin": "warning", 425 | "missing_input_pin": "warning", 426 | "missing_power_pin": "error", 427 | "missing_unit": "warning", 428 | "multiple_net_names": "warning", 429 | "net_not_bus_member": "warning", 430 | "no_connect_connected": "warning", 431 | "no_connect_dangling": "warning", 432 | "pin_not_connected": "error", 433 | "pin_not_driven": "error", 434 | "pin_to_pin": "error", 435 | "power_pin_not_driven": "error", 436 | "similar_labels": "warning", 437 | "simulation_model_issue": "ignore", 438 | "unannotated": "error", 439 | "unit_value_mismatch": "error", 440 | "unresolved_variable": "error", 441 | "wire_dangling": "error" 442 | } 443 | }, 444 | "libraries": { 445 | "pinned_footprint_libs": [], 446 | "pinned_symbol_libs": [] 447 | }, 448 | "meta": { 449 | "filename": "ZoroBot3.kicad_pro", 450 | "version": 1 451 | }, 452 | "net_settings": { 453 | "classes": [ 454 | { 455 | "bus_width": 12, 456 | "clearance": 0.18, 457 | "diff_pair_gap": 0.25, 458 | "diff_pair_via_gap": 0.25, 459 | "diff_pair_width": 0.2, 460 | "line_style": 0, 461 | "microvia_diameter": 0.3, 462 | "microvia_drill": 0.1, 463 | "name": "Default", 464 | "pcb_color": "rgba(0, 0, 0, 0.000)", 465 | "schematic_color": "rgba(0, 0, 0, 0.000)", 466 | "track_width": 0.18, 467 | "via_diameter": 0.6, 468 | "via_drill": 0.35, 469 | "wire_width": 6 470 | } 471 | ], 472 | "meta": { 473 | "version": 3 474 | }, 475 | "net_colors": null, 476 | "netclass_assignments": null, 477 | "netclass_patterns": [] 478 | }, 479 | "pcbnew": { 480 | "last_paths": { 481 | "gencad": "", 482 | "idf": "", 483 | "netlist": "", 484 | "plot": "G:/.shortcut-targets-by-id/1AN4BZZtHHbl9MeGnbDwb2W9vTeww5E2o/OPRobots.org/01. ROBOTS/05. LABERINTO/ZoroBot3/Rebuild_PCB/ZoroBot3/", 485 | "pos_files": "", 486 | "specctra_dsn": "", 487 | "step": "G:/.shortcut-targets-by-id/1AN4BZZtHHbl9MeGnbDwb2W9vTeww5E2o/OPRobots.org/01. ROBOTS/05. LABERINTO/ZoroBot3/Rebuild_PCB/ZoroBot3_intento3.step", 488 | "svg": "", 489 | "vrml": "" 490 | }, 491 | "page_layout_descr_file": "" 492 | }, 493 | "schematic": { 494 | "annotate_start_num": 0, 495 | "bom_export_filename": "", 496 | "bom_fmt_presets": [], 497 | "bom_fmt_settings": { 498 | "field_delimiter": ",", 499 | "keep_line_breaks": false, 500 | "keep_tabs": false, 501 | "name": "CSV", 502 | "ref_delimiter": ",", 503 | "ref_range_delimiter": "", 504 | "string_delimiter": "\"" 505 | }, 506 | "bom_presets": [], 507 | "bom_settings": { 508 | "exclude_dnp": false, 509 | "fields_ordered": [ 510 | { 511 | "group_by": false, 512 | "label": "Reference", 513 | "name": "Reference", 514 | "show": true 515 | }, 516 | { 517 | "group_by": true, 518 | "label": "Value", 519 | "name": "Value", 520 | "show": true 521 | }, 522 | { 523 | "group_by": false, 524 | "label": "Datasheet", 525 | "name": "Datasheet", 526 | "show": true 527 | }, 528 | { 529 | "group_by": false, 530 | "label": "Footprint", 531 | "name": "Footprint", 532 | "show": true 533 | }, 534 | { 535 | "group_by": false, 536 | "label": "Qty", 537 | "name": "${QUANTITY}", 538 | "show": true 539 | }, 540 | { 541 | "group_by": true, 542 | "label": "DNP", 543 | "name": "${DNP}", 544 | "show": true 545 | }, 546 | { 547 | "group_by": false, 548 | "label": "#", 549 | "name": "${ITEM_NUMBER}", 550 | "show": false 551 | }, 552 | { 553 | "group_by": false, 554 | "label": "Aliexpress", 555 | "name": "Aliexpress", 556 | "show": false 557 | }, 558 | { 559 | "group_by": false, 560 | "label": "Height", 561 | "name": "Height", 562 | "show": false 563 | }, 564 | { 565 | "group_by": false, 566 | "label": "Manufacturer_Name", 567 | "name": "Manufacturer_Name", 568 | "show": false 569 | }, 570 | { 571 | "group_by": false, 572 | "label": "Manufacturer_Part_Number", 573 | "name": "Manufacturer_Part_Number", 574 | "show": false 575 | }, 576 | { 577 | "group_by": false, 578 | "label": "Mouser Part Number", 579 | "name": "Mouser Part Number", 580 | "show": false 581 | }, 582 | { 583 | "group_by": false, 584 | "label": "Mouser Price/Stock", 585 | "name": "Mouser Price/Stock", 586 | "show": false 587 | }, 588 | { 589 | "group_by": false, 590 | "label": "RS Part Number", 591 | "name": "RS Part Number", 592 | "show": false 593 | }, 594 | { 595 | "group_by": false, 596 | "label": "RS Price/Stock", 597 | "name": "RS Price/Stock", 598 | "show": false 599 | }, 600 | { 601 | "group_by": false, 602 | "label": "Sim.Device", 603 | "name": "Sim.Device", 604 | "show": false 605 | }, 606 | { 607 | "group_by": false, 608 | "label": "Sim.Pins", 609 | "name": "Sim.Pins", 610 | "show": false 611 | }, 612 | { 613 | "group_by": false, 614 | "label": "Sim.Type", 615 | "name": "Sim.Type", 616 | "show": false 617 | }, 618 | { 619 | "group_by": false, 620 | "label": "Descripción", 621 | "name": "Description", 622 | "show": false 623 | } 624 | ], 625 | "filter_string": "", 626 | "group_symbols": true, 627 | "name": "", 628 | "sort_asc": true, 629 | "sort_field": "Reference" 630 | }, 631 | "connection_grid_size": 50.0, 632 | "drawing": { 633 | "dashed_lines_dash_length_ratio": 12.0, 634 | "dashed_lines_gap_length_ratio": 3.0, 635 | "default_line_thickness": 6.0, 636 | "default_text_size": 50.0, 637 | "field_names": [], 638 | "intersheets_ref_own_page": false, 639 | "intersheets_ref_prefix": "", 640 | "intersheets_ref_short": false, 641 | "intersheets_ref_show": false, 642 | "intersheets_ref_suffix": "", 643 | "junction_size_choice": 3, 644 | "label_size_ratio": 0.375, 645 | "operating_point_overlay_i_precision": 3, 646 | "operating_point_overlay_i_range": "~A", 647 | "operating_point_overlay_v_precision": 3, 648 | "operating_point_overlay_v_range": "~V", 649 | "overbar_offset_ratio": 1.23, 650 | "pin_symbol_size": 25.0, 651 | "text_offset_ratio": 0.15 652 | }, 653 | "legacy_lib_dir": "", 654 | "legacy_lib_list": [], 655 | "meta": { 656 | "version": 1 657 | }, 658 | "net_format_name": "", 659 | "page_layout_descr_file": "", 660 | "plot_directory": "", 661 | "spice_current_sheet_as_root": false, 662 | "spice_external_command": "spice \"%I\"", 663 | "spice_model_current_sheet_as_root": true, 664 | "spice_save_all_currents": false, 665 | "spice_save_all_dissipations": false, 666 | "spice_save_all_voltages": false, 667 | "subpart_first_id": 65, 668 | "subpart_id_separator": 0 669 | }, 670 | "sheets": [ 671 | [ 672 | "38e8b839-c362-4120-bbf7-582efa82e48d", 673 | "Raíz" 674 | ], 675 | [ 676 | "fe1aa100-f33c-4fed-9a9a-b5cbbb3bba93", 677 | "Encoders" 678 | ], 679 | [ 680 | "9d49d3a6-8e33-4528-aef9-1bba3efb9fbc", 681 | "Giroscopio" 682 | ], 683 | [ 684 | "fa956960-b4bb-4d1a-9d4a-3aa8012b4e3b", 685 | "Sensores" 686 | ] 687 | ], 688 | "text_variables": {} 689 | } 690 | -------------------------------------------------------------------------------- /pcb_files/kicad_project/fabrication-toolkit-options.json: -------------------------------------------------------------------------------- 1 | {"EXTRA_LAYERS": "", "EXTEND_EDGE_CUT": false, "AUTO TRANSLATE": true, "AUTO FILL": true, "EXCLUDE DNP": false} -------------------------------------------------------------------------------- /pcb_files/kicad_project/sym-lib-table: -------------------------------------------------------------------------------- 1 | (sym_lib_table 2 | (version 7) 3 | (lib (name "custom_symbols")(type "KiCad")(uri "${KIPRJMOD}/custom_libraries/custom_symbols.kicad_sym")(options "")(descr "")) 4 | ) 5 | -------------------------------------------------------------------------------- /source_code/.clang-format: -------------------------------------------------------------------------------- 1 | ColumnLimit: 0 2 | IndentCaseLabels: true -------------------------------------------------------------------------------- /source_code/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .pioenvs 3 | .piolibdeps 4 | .vscode 5 | .vscode/* 6 | -------------------------------------------------------------------------------- /source_code/.travis.yml: -------------------------------------------------------------------------------- 1 | # Continuous Integration (CI) is the practice, in software 2 | # engineering, of merging all developer working copies with a shared mainline 3 | # several times a day < https://docs.platformio.org/page/ci/index.html > 4 | # 5 | # Documentation: 6 | # 7 | # * Travis CI Embedded Builds with PlatformIO 8 | # < https://docs.travis-ci.com/user/integration/platformio/ > 9 | # 10 | # * PlatformIO integration with Travis CI 11 | # < https://docs.platformio.org/page/ci/travis.html > 12 | # 13 | # * User Guide for `platformio ci` command 14 | # < https://docs.platformio.org/page/userguide/cmd_ci.html > 15 | # 16 | # 17 | # Please choose one of the following templates (proposed below) and uncomment 18 | # it (remove "# " before each line) or use own configuration according to the 19 | # Travis CI documentation (see above). 20 | # 21 | 22 | 23 | # 24 | # Template #1: General project. Test it using existing `platformio.ini`. 25 | # 26 | 27 | # language: python 28 | # python: 29 | # - "2.7" 30 | # 31 | # sudo: false 32 | # cache: 33 | # directories: 34 | # - "~/.platformio" 35 | # 36 | # install: 37 | # - pip install -U platformio 38 | # - platformio update 39 | # 40 | # script: 41 | # - platformio run 42 | 43 | 44 | # 45 | # Template #2: The project is intended to be used as a library with examples. 46 | # 47 | 48 | # language: python 49 | # python: 50 | # - "2.7" 51 | # 52 | # sudo: false 53 | # cache: 54 | # directories: 55 | # - "~/.platformio" 56 | # 57 | # env: 58 | # - PLATFORMIO_CI_SRC=path/to/test/file.c 59 | # - PLATFORMIO_CI_SRC=examples/file.ino 60 | # - PLATFORMIO_CI_SRC=path/to/test/directory 61 | # 62 | # install: 63 | # - pip install -U platformio 64 | # - platformio update 65 | # 66 | # script: 67 | # - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N 68 | -------------------------------------------------------------------------------- /source_code/boards/custom_stm32f405rgt6.json: -------------------------------------------------------------------------------- 1 | { 2 | "build": { 3 | "core": "stm32", 4 | "cpu": "cortex-m4", 5 | "extra_flags": "-DSTM32F4 -DSTM32F415xx -DSTM32F40_41xxx -DBOARD_discovery_f4", 6 | "f_cpu": "168000000L", 7 | "ldscript": "stm32f405x6.ld", 8 | "mcu": "stm32f415rgt", 9 | "variant": "stm32f415xx" 10 | }, 11 | "debug": { 12 | "default_tools": [ 13 | "stlink" 14 | ], 15 | "jlink_device": "STM32F415RG", 16 | "openocd_target": "stm32f4x", 17 | "svd_path": "STM32F41x.svd" 18 | }, 19 | "frameworks": [ 20 | "libopencm3", 21 | "cmsis", 22 | "spl", 23 | "stm32cube" 24 | ], 25 | "name": "1Bitsy", 26 | "upload": { 27 | "maximum_ram_size": 131072, 28 | "maximum_size": 1048576, 29 | "protocol": "blackmagic", 30 | "protocols": [ 31 | "jlink", 32 | "stlink", 33 | "blackmagic" 34 | ] 35 | }, 36 | "url": "http://1bitsy.org", 37 | "vendor": "1BitSquared" 38 | } -------------------------------------------------------------------------------- /source_code/include/battery.h: -------------------------------------------------------------------------------- 1 | #ifndef __BATTERY_H 2 | #define __BATTERY_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "constants.h" 9 | #include "delay.h" 10 | #include "leds.h" 11 | #include "sensors.h" 12 | #include "setup.h" 13 | 14 | #define BATTERY_VOLTAGE_LOW_PASS_FILTER_ALPHA 0.1 15 | 16 | void update_battery_voltage(void); 17 | float get_battery_voltage(void); 18 | float get_battery_high_limit_voltage(void); 19 | void show_battery_level(void); 20 | 21 | #endif /* __BATTERY_H */ 22 | -------------------------------------------------------------------------------- /source_code/include/buttons.h: -------------------------------------------------------------------------------- 1 | #ifndef __BUTTONS_H 2 | #define __BUTTONS_H 3 | 4 | #include "config.h" 5 | #include "control.h" 6 | #include "delay.h" 7 | #include 8 | #include 9 | #include 10 | 11 | #define MENU_VOLTAGE_LOW_PASS_FILTER_ALPHA 0.1 12 | 13 | void check_buttons(void); 14 | bool get_start_btn(void); 15 | bool get_menu_up_btn(void); 16 | bool get_menu_down_btn(void); 17 | bool get_menu_mode_btn(void); 18 | 19 | void set_debug_btn(bool state); 20 | bool get_debug_btn(void); 21 | 22 | #endif -------------------------------------------------------------------------------- /source_code/include/calibrations.h: -------------------------------------------------------------------------------- 1 | #ifndef CALIBRATIONS_H 2 | #define CALIBRATIONS_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #define CALIBRATE_NONE 0 13 | #define CALIBRATE_GYRO_Z 1 14 | #define CALIBRATE_SIDE_SENSORS_OFFSET 2 15 | #define CALIBRATE_FRONT_SENSORS 3 16 | #define CALIBRATE_STORE_EEPROM 4 17 | 18 | void calibrate_from_config(uint8_t type); 19 | void calibrate_manual_distances(void); 20 | 21 | #endif -------------------------------------------------------------------------------- /source_code/include/config.h: -------------------------------------------------------------------------------- 1 | #ifndef __CONFIG_H 2 | #define __CONFIG_H 3 | 4 | #include 5 | #include 6 | 7 | #define ZOROBOT3_A 1 8 | // #define ZOROBOT3_B 2 9 | // #define ZOROBOT3_C 3 10 | 11 | /** Laberinto */ 12 | #define CELL_DIMENSION 180 13 | #define CELL_DIAGONAL 127.3 14 | #define WALL_WIDTH 12 15 | #define MIDDLE_MAZE_DISTANCE ((CELL_DIMENSION - WALL_WIDTH) / 2.) 16 | #define SENSING_POINT_DISTANCE 0 17 | #define WALL_LOSS_TO_SENSING_POINT_DISTANCE 106 18 | 19 | /** Características Físicas */ 20 | #ifdef ZOROBOT3_A 21 | #define MICROMETERS_PER_TICK 10.494055 22 | #endif 23 | #ifdef ZOROBOT3_B 24 | #define MICROMETERS_PER_TICK 10.494055 25 | #endif 26 | #ifdef ZOROBOT3_C 27 | #define MICROMETERS_PER_TICK 10.494055 28 | #endif 29 | #define ROBOT_FRONT_LENGTH 48.121 30 | #define ROBOT_BACK_LENGTH 40.706 31 | #define ROBOT_WIDTH 70.2 32 | #define WHEELS_SEPARATION 62 33 | #define ROBOT_MIDDLE_WIDTH ((ROBOT_WIDTH / 2.0)) 34 | 35 | /** Movimiento */ 36 | #define MAX_MOTOR_SATURATION_COUNT 20 37 | #define MAX_MOTOR_ANGULAR_SATURATION_COUNT 60 38 | #define KP_LINEAR 0.0003 39 | #define KD_LINEAR 0.0005 40 | 41 | #define KP_ANGULAR 0.01 42 | #define KD_ANGULAR 0.4 43 | 44 | #define KP_SIDE_SENSORS 0.08 45 | #define KI_SIDE_SENSORS 0.00 // 0.002 46 | #define KD_SIDE_SENSORS 0.12 47 | 48 | #define KP_FRONT_SENSORS 0.02 49 | #define KI_FRONT_SENSORS 0.00035 50 | 51 | // #define KP_FRONT_DIAGONAL_SENSORS 0.0001 52 | // #define KI_FRONT_DIAGONAL_SENSORS 0.00001 53 | // #define KD_FRONT_DIAGONAL_SENSORS 0.002 54 | 55 | #define KP_FRONT_DIAGONAL_SENSORS 0.040 56 | #define KI_FRONT_DIAGONAL_SENSORS 0.001//0010 57 | #define KD_FRONT_DIAGONAL_SENSORS 0.040//0010 58 | 59 | /** Sensores */ 60 | #define SENSOR_LOW_PASS_FILTER_ALPHA 0.1 61 | 62 | #define SENSOR_FRONT_CALIBRATION_READINGS 20 63 | #define SENSOR_SIDE_CALIBRATION_READINGS 100 64 | #define SENSOR_FRONT_DETECTION ((CELL_DIMENSION * 1.22)) 65 | #define SENSOR_SIDE_DETECTION ((CELL_DIMENSION * 0.7)) 66 | 67 | /** Control de inicio de competición */ 68 | #define SENSOR_FRONT_DETECTION_START 100 69 | #define SENSOR_START_MIN_MS 350 70 | 71 | /** Divisor de Voltage */ 72 | #ifdef ZOROBOT3_C 73 | #define VOLT_DIV_FACTOR 5.30 74 | #else 75 | #define VOLT_DIV_FACTOR 3.15 76 | #endif 77 | #define BATTERY_2S_HIGH_LIMIT_VOLTAGE 8.4 78 | #define BATTERY_2S_LOW_LIMIT_VOLTAGE 7.4 79 | #define BATTERY_3S_HIGH_LIMIT_VOLTAGE 12.6 80 | #define BATTERY_3S_LOW_LIMIT_VOLTAGE 11.1 81 | 82 | /** Modo RUN */ 83 | #define CONFIG_RUN_RACE 1 84 | #define CONFIG_RUN_DEBUG 0 85 | 86 | void set_all_configs(void); 87 | uint16_t get_config_run(void); 88 | 89 | #endif -------------------------------------------------------------------------------- /source_code/include/constants.h: -------------------------------------------------------------------------------- 1 | #ifndef __CONSTANTS_H 2 | #define __CONSTANTS_H 3 | 4 | /** Laberinto (dimensiones máximas)*/ 5 | #define MAZE_ROWS 16 6 | #define MAZE_COLUMNS 16 7 | #define MAZE_CELLS (MAZE_ROWS * MAZE_COLUMNS) 8 | #define MAZE_MAX_DISTANCE (MAZE_CELLS - 1) 9 | 10 | /** Constantes matemáticas */ 11 | #define PI 3.1415 12 | #define MICROMETERS_PER_MILLIMETER 1000 13 | #define MILLIMETERS_PER_METER 1000 14 | #define MICROSECONDS_PER_SECOND 1000000 15 | 16 | /** Constantes del STM32F4 */ 17 | #define SYSCLK_FREQUENCY_HZ 168000000 18 | #define SYSTICK_FREQUENCY_HZ 1000 19 | #define CONTROL_FREQUENCY_HZ 1000 20 | #define LEDS_MAX_PWM 1024 21 | #define MOTORES_MAX_PWM 1024 22 | #define MOTORES_SATURATION_PWM 1000 23 | #define MOTORES_SATURATION_ANGULAR_SPEED 50 24 | #define ADC_RESOLUTION 4096 25 | #define ADC_LSB (3.3 / ADC_RESOLUTION) 26 | 27 | #endif -------------------------------------------------------------------------------- /source_code/include/control.h: -------------------------------------------------------------------------------- 1 | #ifndef __CONTROL_H 2 | #define __CONTROL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | bool is_race_started(void); 17 | void set_race_started(bool state); 18 | int8_t check_start_run(void); 19 | void set_control_debug(bool state); 20 | 21 | void set_side_sensors_close_correction(bool enabled); 22 | void set_side_sensors_far_correction(bool enabled); 23 | void set_front_sensors_correction(bool enabled); 24 | bool is_front_sensors_correction_enabled(void); 25 | void set_front_sensors_diagonal_correction(bool enabled); 26 | void disable_sensors_correction(void); 27 | void reset_control_errors(void); 28 | void reset_control_speed(void); 29 | void reset_control_all(void); 30 | 31 | void set_target_linear_speed(int32_t linear_speed); 32 | int32_t get_ideal_linear_speed(void); 33 | void set_ideal_angular_speed(float angular_speed); 34 | float get_ideal_angular_speed(void); 35 | 36 | void set_target_fan_speed(int32_t fan_speed, int32_t ms); 37 | 38 | void control_loop(void); 39 | 40 | void keep_z_angle(void); 41 | 42 | #endif -------------------------------------------------------------------------------- /source_code/include/debug.h: -------------------------------------------------------------------------------- 1 | #ifndef __DEBUG_H 2 | #define __DEBUG_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #define DEBUG_NONE 0 19 | #define DEBUG_MACROARRAY 1 20 | #define DEBUG_TYPE_SENSORS_RAW 2 21 | #define DEBUG_TYPE_SENSORS_DISTANCES 3 22 | #define DEBUG_FLOODFILL_MAZE 4 23 | #define DEBUG_MOTORS_CURRENT 5 24 | #define DEBUG_GYRO_DEMO 9 25 | #define DEBUG_FAN_DEMO 10 26 | 27 | bool is_debug_enabled(void); 28 | void debug_from_config(uint8_t type); 29 | void debug_from_main(uint8_t type); 30 | 31 | #endif -------------------------------------------------------------------------------- /source_code/include/delay.h: -------------------------------------------------------------------------------- 1 | #ifndef __DELAY_H 2 | #define __DELAY_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "constants.h" 9 | #include "setup.h" 10 | 11 | void delay(uint32_t ms); 12 | void delay_us(uint32_t us); 13 | void clock_tick(void); 14 | uint32_t get_clock_ticks(void); 15 | uint32_t read_cycle_counter(void); 16 | uint32_t get_us_counter(void); 17 | 18 | #endif -------------------------------------------------------------------------------- /source_code/include/eeprom.h: -------------------------------------------------------------------------------- 1 | #ifndef __EEPROM_H 2 | #define __EEPROM_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define EEPROM_SECTOR 11 14 | #define EEPROM_BASE_ADDRESS (0x080E0000) 15 | 16 | #define DATA_LENGTH (2 + NUM_SENSORES + MAZE_CELLS + MENU_RUN_NUM_MODES + RC5_DATA_LENGTH) 17 | 18 | #define DATA_INDEX_GYRO_Z 0 19 | #define DATA_INDEX_SENSORS_OFFSETS (DATA_INDEX_GYRO_Z + 2) 20 | #define DATA_INDEX_MAZE (DATA_INDEX_SENSORS_OFFSETS + NUM_SENSORES) 21 | #define DATA_INDEX_MENU_RUN (DATA_INDEX_MAZE + MAZE_CELLS) 22 | #define DATA_INDEX_RC5 (DATA_INDEX_MENU_RUN + MENU_RUN_NUM_MODES) 23 | 24 | void eeprom_save(void); 25 | void eeprom_load(void); 26 | void eeprom_clear(void); 27 | void eeprom_backup(void); 28 | void eeprom_restore(void); 29 | void eeprom_set_data(uint16_t index, int16_t *data, uint16_t length); 30 | int16_t *eeprom_get_data(void); 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /source_code/include/encoders.h: -------------------------------------------------------------------------------- 1 | #ifndef __ENCODERS_H 2 | #define __ENCODERS_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "config.h" 10 | #include "constants.h" 11 | #include "setup.h" 12 | 13 | int32_t get_encoder_left_ticks(void); 14 | int32_t get_encoder_right_ticks(void); 15 | 16 | int32_t get_encoder_left_micrometers(void); 17 | int32_t get_encoder_right_micrometers(void); 18 | int32_t get_encoder_avg_micrometers(void); 19 | 20 | int32_t get_encoder_left_millimeters(void); 21 | int32_t get_encoder_right_millimeters(void); 22 | int32_t get_encoder_avg_millimeters(void); 23 | void reset_encoder_avg(void); 24 | 25 | float get_encoder_left_speed(void); 26 | float get_encoder_right_speed(void); 27 | float get_encoder_avg_speed(void); 28 | 29 | float get_encoder_angular_speed(void); 30 | 31 | int32_t max_likelihood_counter_diff(uint16_t now, uint16_t before); 32 | void update_encoder_readings(void); 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /source_code/include/floodfill.h: -------------------------------------------------------------------------------- 1 | #ifndef FLOODFILL_H 2 | #define FLOODFILL_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define VISITED_BIT 1 12 | #define EAST_BIT 2 13 | #define SOUTH_BIT 4 14 | #define WEST_BIT 8 15 | #define NORTH_BIT 16 16 | 17 | #define MAX_TARGETS 10 18 | 19 | enum compass_direction { 20 | TARGET = 0, 21 | EAST = 1, 22 | SOUTH_EAST = 1 - MAZE_COLUMNS, 23 | SOUTH = -MAZE_COLUMNS, 24 | SOUTH_WEST = -1 - MAZE_COLUMNS, 25 | WEST = -1, 26 | NORTH_WEST = -1 + MAZE_COLUMNS, 27 | NORTH = MAZE_COLUMNS, 28 | NORTH_EAST = 1 + MAZE_COLUMNS, 29 | }; 30 | 31 | struct compass_direction_values{ 32 | int8_t EAST; 33 | int8_t SOUTH; 34 | int8_t WEST; 35 | int8_t NORTH; 36 | }; 37 | 38 | enum step_direction { 39 | NONE = -1, 40 | FRONT = 0, 41 | LEFT = 1, 42 | RIGHT = 2, 43 | BACK = 3, 44 | }; 45 | 46 | struct queue_cell { 47 | uint8_t cell; 48 | enum compass_direction direction; 49 | enum compass_direction last_step; 50 | uint8_t count; 51 | }; 52 | 53 | struct cells_queue { 54 | struct queue_cell queue[MAZE_CELLS]; 55 | uint8_t head; 56 | uint8_t tail; 57 | }; 58 | 59 | struct cells_stack { 60 | uint8_t stack[MAX_TARGETS]; 61 | uint8_t size; 62 | }; 63 | 64 | void floodfill_set_time_limit(uint32_t ms); 65 | void floodfill_maze_print(void); 66 | void floodfill_load_maze(void); 67 | void floodfill_start_explore(void); 68 | void floodfill_start_run(void); 69 | void floodfill_loop(void); 70 | 71 | #endif -------------------------------------------------------------------------------- /source_code/include/floodfill_weigths.h: -------------------------------------------------------------------------------- 1 | #ifndef FLOODFILL_WEIGHTS_H 2 | #define FLOODFILL_WEIGHTS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct cell_weigth { 10 | uint16_t speed; 11 | float time; 12 | float total_time; 13 | float penalty; 14 | }; 15 | 16 | uint16_t floodfill_weights_cells_to_max_speed(float distance, uint16_t init_speed, uint16_t max_speed, uint16_t accel); 17 | void floodfill_weights_table(float distance, uint16_t init_speed, uint16_t max_speed, uint16_t accel, uint16_t cells_to_max_speed, struct cell_weigth *weights_out); 18 | 19 | #endif -------------------------------------------------------------------------------- /source_code/include/handwall.h: -------------------------------------------------------------------------------- 1 | #ifndef HANDWALL_C 2 | #define HANDWALL_C 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | 10 | void handwall_use_left_hand(void); 11 | void handwall_use_right_hand(void); 12 | void handwall_set_time_limit(uint32_t ms); 13 | 14 | void handwall_start(void); 15 | void handwall_loop(void); 16 | 17 | #endif -------------------------------------------------------------------------------- /source_code/include/leds.h: -------------------------------------------------------------------------------- 1 | #ifndef __LEDS_H 2 | #define __LEDS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | void set_status_led(bool state); 15 | void toggle_status_led(void); 16 | void warning_status_led(uint32_t ms); 17 | bool is_status_led_on(void); 18 | void set_RGB_color(uint32_t r, uint32_t g, uint32_t b); 19 | void set_RGB_rainbow(void); 20 | void set_RGB_color_while(uint32_t r, uint32_t g, uint32_t b, uint32_t ms); 21 | void blink_RGB_color(uint32_t r, uint32_t g, uint32_t b, uint32_t ms); 22 | void check_leds_while(void); 23 | void set_leds_wave(int ms); 24 | void set_leds_side_sensors(int ms); 25 | void set_leds_front_sensors(int ms); 26 | void set_leds_blink(int ms); 27 | void set_leds_battery_level(float battery_level); 28 | void all_leds_clear(void); 29 | void set_info_led(uint8_t index, bool state); 30 | void set_info_leds(void); 31 | void clear_info_leds(void); 32 | 33 | #endif -------------------------------------------------------------------------------- /source_code/include/lsm6dsr.h: -------------------------------------------------------------------------------- 1 | #ifndef LSM6DSR_H 2 | #define LSM6DSR_H 3 | 4 | #include 5 | 6 | #include "buttons.h" 7 | #include "constants.h" 8 | #include "delay.h" 9 | #include "eeprom.h" 10 | #include "math.h" 11 | #include "motors.h" 12 | #include "setup.h" 13 | #include 14 | 15 | void lsm6dsr_init(void); 16 | uint8_t lsm6dsr_who_am_i(void); 17 | 18 | void lsm6dsr_gyro_z_calibration(void); 19 | void lsm6dsr_load_eeprom(void); 20 | 21 | void lsm6dsr_update(void); 22 | float lsm6dsr_get_gyro_z_raw(void); 23 | float lsm6dsr_get_gyro_z_radps(void); 24 | float lsm6dsr_get_gyro_z_dps(void); 25 | float lsm6dsr_get_gyro_z_degrees(void); 26 | void lsm6dsr_set_gyro_z_degrees(float deg); 27 | 28 | #endif // LSM6DSR_H -------------------------------------------------------------------------------- /source_code/include/macroarray.h: -------------------------------------------------------------------------------- 1 | #ifndef MACROARRAY_H 2 | #define MACROARRAY_H 3 | 4 | #include 5 | #include 6 | 7 | 8 | #include "delay.h" 9 | 10 | #define MACROARRAY_LENGTH 30000 11 | #define MACROARRAY_SEPARATOR "\t" 12 | 13 | void macroarray_store(uint8_t ms, uint16_t float_bits, char **labels, uint8_t size, ...); 14 | void macroarray_print(void); 15 | 16 | #endif -------------------------------------------------------------------------------- /source_code/include/maze.h: -------------------------------------------------------------------------------- 1 | #ifndef MAZE_H 2 | #define MAZE_H 3 | 4 | #include "floodfill.h" 5 | #include "menu_run.h" 6 | 7 | uint16_t maze_get_rows(void); 8 | uint16_t maze_get_columns(void); 9 | uint16_t maze_get_cells(void); 10 | struct cells_stack *maze_get_goals(void); 11 | 12 | #endif -------------------------------------------------------------------------------- /source_code/include/menu.h: -------------------------------------------------------------------------------- 1 | #ifndef MENU_H 2 | #define MENU_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | void menu_handler(void); 12 | void menu_reset(void); 13 | 14 | void menu_rc5_mode_change(void); 15 | void menu_rc5_up(void); 16 | void menu_rc5_down(void); 17 | 18 | #endif -------------------------------------------------------------------------------- /source_code/include/menu_configs.h: -------------------------------------------------------------------------------- 1 | #ifndef __MENU_CONFIGS_H 2 | #define __MENU_CONFIGS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | bool menu_config_handler(void); 12 | void menu_config_reset_values(void); 13 | void menu_config_reset_mode(void); 14 | 15 | #endif -------------------------------------------------------------------------------- /source_code/include/menu_run.h: -------------------------------------------------------------------------------- 1 | #ifndef MENU_RUN_H 2 | #define MENU_RUN_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define MENU_RUN_NUM_MODES 5 9 | 10 | enum speed_strategy { 11 | SPEED_EXPLORE = 0, 12 | SPEED_NORMAL = 1, 13 | SPEED_MEDIUM = 2, 14 | SPEED_FAST = 3, 15 | SPEED_SUPER = 4, 16 | SPEED_HAKI = 5, 17 | }; 18 | 19 | enum maze_type { 20 | MAZE_HOME = 0, 21 | MAZE_COMPETITION = 1, 22 | }; 23 | 24 | enum solve_strategy { 25 | SOLVE_STANDARD = 0, 26 | SOLVE_DIAGONALS = 1, 27 | }; 28 | 29 | enum explore_algorithm { 30 | EXPLORE_HANDWALL = 0, 31 | EXPLORE_FLOODFILL = 1, 32 | EXPLORE_TIME_TRIAL = 2, 33 | }; 34 | 35 | bool menu_run_handler(void); 36 | void menu_run_reset(void); 37 | 38 | void menu_run_load_values(void); 39 | 40 | bool menu_run_can_start(void); 41 | 42 | void menu_run_mode_change(void); 43 | void menu_run_up(void); 44 | void menu_run_down(void); 45 | 46 | int16_t *get_menu_run_values(void); 47 | enum speed_strategy menu_run_get_speed(void); 48 | enum maze_type menu_run_get_maze_type(void); 49 | enum solve_strategy menu_run_get_solve_strategy(void); 50 | enum explore_algorithm menu_run_get_explore_algorithm(void); 51 | 52 | #endif -------------------------------------------------------------------------------- /source_code/include/motors.h: -------------------------------------------------------------------------------- 1 | #ifndef __MOTORS_H 2 | #define __MOTORS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | void set_motors_enable(bool enabled); 17 | void set_motors_speed(float velI, float velD); 18 | void set_motors_brake(void); 19 | void set_motors_pwm(int32_t pwm_left, int32_t pwm_right); 20 | void set_fan_speed(uint8_t vel); 21 | 22 | void reset_motors_saturated(void); 23 | bool is_motor_saturated(void); 24 | uint32_t get_motors_saturated_ms(void); 25 | void set_check_motors_saturated_enabled(bool enabled); 26 | 27 | #endif -------------------------------------------------------------------------------- /source_code/include/move.h: -------------------------------------------------------------------------------- 1 | #ifndef MOVE_H 2 | #define MOVE_H 3 | 4 | #include "control.h" 5 | #include "menu.h" 6 | #include "menu_run.h" 7 | #include "motors.h" 8 | #include 9 | 10 | enum movement { 11 | MOVE_NONE, 12 | MOVE_HOME, 13 | MOVE_START, 14 | MOVE_END, 15 | MOVE_FRONT, 16 | MOVE_LEFT, 17 | MOVE_RIGHT, 18 | MOVE_LEFT_90, 19 | MOVE_RIGHT_90, 20 | MOVE_LEFT_180, 21 | MOVE_RIGHT_180, 22 | MOVE_DIAGONAL, 23 | MOVE_LEFT_TO_45, 24 | MOVE_RIGHT_TO_45, 25 | MOVE_LEFT_TO_135, 26 | MOVE_RIGHT_TO_135, 27 | MOVE_LEFT_45_TO_45, 28 | MOVE_RIGHT_45_TO_45, 29 | MOVE_LEFT_FROM_45, 30 | MOVE_RIGHT_FROM_45, 31 | MOVE_LEFT_FROM_45_180, 32 | MOVE_RIGHT_FROM_45_180, 33 | MOVE_BACK, 34 | MOVE_BACK_WALL, 35 | MOVE_BACK_STOP, 36 | }; 37 | 38 | struct inplace_params { 39 | int16_t start; 40 | int16_t end; 41 | int16_t linear_speed; 42 | float angular_accel; 43 | float max_angular_speed; 44 | uint16_t t_accel; 45 | uint16_t t_max; 46 | int8_t sign; 47 | }; 48 | struct turn_params { 49 | float start; 50 | float end; 51 | int16_t linear_speed; 52 | float max_angular_speed; 53 | float transition; 54 | float arc; 55 | int8_t sign; 56 | }; 57 | 58 | struct linear_accel_params { 59 | int16_t break_accel; 60 | int16_t accel_hard; 61 | int16_t speed_hard; 62 | int16_t accel_soft; 63 | }; 64 | 65 | struct kinematics { 66 | int16_t linear_speed; 67 | struct linear_accel_params linear_accel; 68 | int16_t fan_speed; 69 | struct turn_params *turns; 70 | }; 71 | 72 | bool get_cell_change_toggle_state(void); 73 | bool get_wall_lost_toggle_state(void); 74 | 75 | char *get_movement_string(enum movement movement); 76 | 77 | enum speed_strategy; 78 | void configure_kinematics(enum speed_strategy speed); 79 | struct kinematics get_kinematics(void); 80 | uint16_t get_floodfill_linear_speed(void); 81 | uint16_t get_floodfill_max_linear_speed(void); 82 | uint16_t get_floodfill_accel(void); 83 | 84 | void set_starting_position(void); 85 | int32_t get_current_cell_travelled_distance(void); 86 | 87 | void move_straight(int32_t distance, int32_t speed, bool check_wall_loss, bool stop); 88 | void move_straight_until_front_distance(uint32_t distance, int32_t speed, bool stop); 89 | void move_arc_turn(struct turn_params turn); 90 | void move_inplace_turn(enum movement movement); 91 | void move_inplace_angle(float angle, float rads); 92 | 93 | void run_straight(float distance, float start_offset, float end_offset, uint16_t cells, bool has_begin, int32_t speed, int32_t final_speed); 94 | void run_side(enum movement movement, struct turn_params turn); 95 | void run_diagonal(float distance, float end_offset, uint16_t cells, int32_t speed, int32_t final_speed); 96 | 97 | void move(enum movement movement); 98 | void move_run_sequence(enum movement *sequence_movements); 99 | 100 | #endif -------------------------------------------------------------------------------- /source_code/include/rc5.h: -------------------------------------------------------------------------------- 1 | #ifndef RC5_H 2 | #define RC5_H 3 | 4 | #include 5 | 6 | // #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | // #include 13 | 14 | enum RC5_TRIGGER { 15 | RC5_TRIGGER_FALLING, 16 | RC5_TRIGGER_RISING, 17 | }; 18 | 19 | #define RC5_DATA_LENGTH 5 20 | 21 | void rc5_load_eeprom(void); 22 | void rc5_register(enum RC5_TRIGGER trigger); 23 | 24 | #endif -------------------------------------------------------------------------------- /source_code/include/sensors.h: -------------------------------------------------------------------------------- 1 | #ifndef __SENSORS_H 2 | #define __SENSORS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #define NUM_AUX_ADC_CHANNELS 4 15 | #define AUX_BATTERY_ID 0 16 | #define AUX_CURRENT_LEFT_ID 2 17 | #define AUX_CURRENT_RIGHT_ID 1 18 | #define AUX_MENU_BTN_ID 3 19 | 20 | 21 | #define NUM_SENSORES 4 22 | #define SENSOR_FRONT_LEFT_WALL_ID 0 23 | #define SENSOR_FRONT_RIGHT_WALL_ID 1 24 | #define SENSOR_SIDE_LEFT_WALL_ID 2 25 | #define SENSOR_SIDE_RIGHT_WALL_ID 3 26 | 27 | struct walls { 28 | bool front; 29 | bool left; 30 | bool right; 31 | }; 32 | 33 | struct sensors_distance_calibration { 34 | float a; 35 | float b; 36 | float c; 37 | }; 38 | 39 | void set_sensors_enabled(bool enabled); 40 | bool get_sensors_enabled(void); 41 | 42 | uint8_t *get_aux_adc_channels(void); 43 | uint8_t get_aux_adc_channels_num(void); 44 | volatile uint16_t *get_aux_adc_raw(void); 45 | uint16_t get_aux_raw(uint8_t pos); 46 | 47 | 48 | uint8_t *get_sensors(void); 49 | uint8_t get_sensors_num(void); 50 | void get_sensors_raw(uint16_t *on, uint16_t *off); 51 | void sm_emitter_adc(void); 52 | 53 | void front_sensors_calibration(void); 54 | void side_sensors_calibration(void); 55 | void sensors_load_eeprom(void); 56 | 57 | uint16_t get_sensor_raw(uint8_t pos, bool on); 58 | uint16_t get_sensor_raw_filter(uint8_t pos); 59 | 60 | bool left_wall_detection(void); 61 | bool right_wall_detection(void); 62 | bool front_wall_detection(void); 63 | struct walls get_walls(void); 64 | 65 | void update_sensors_magics(void); 66 | void update_side_sensors_leds(void); 67 | uint16_t get_sensor_filtered(uint8_t pos); 68 | uint16_t get_sensor_linearized(uint8_t pos); 69 | uint16_t get_sensor_distance(uint8_t pos); 70 | uint16_t get_front_wall_distance(void); 71 | 72 | int16_t get_side_sensors_close_error(void); 73 | int16_t get_side_sensors_far_error(void); 74 | int16_t get_side_sensors_error(void); 75 | int16_t get_diagonal_sensors_error(void); 76 | int16_t get_front_sensors_angle_error(void); 77 | int16_t get_front_sensors_diagonal_error(void); 78 | 79 | #endif -------------------------------------------------------------------------------- /source_code/include/setup.h: -------------------------------------------------------------------------------- 1 | #ifndef __SETUP_H 2 | #define __SETUP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include "rc5.h" 20 | #include "usart.h" 21 | 22 | void setup(void); 23 | void setup_spi_high_speed(void); 24 | void setup_spi_low_speed(void); 25 | 26 | #endif -------------------------------------------------------------------------------- /source_code/include/timetrial.h: -------------------------------------------------------------------------------- 1 | #ifndef TIMETRIAL_H 2 | #define TIMETRIAL_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | void timetrial_start(void); 9 | void timetrial_loop(void); 10 | 11 | #endif -------------------------------------------------------------------------------- /source_code/include/usart.h: -------------------------------------------------------------------------------- 1 | #ifndef __USART_H 2 | #define __USART_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | int _write(int file, char *ptr, int len); 12 | 13 | #endif -------------------------------------------------------------------------------- /source_code/include/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef __UTILS_H 2 | #define __UTILS_H 3 | 4 | #include 5 | 6 | float map(float x, float in_min, float in_max, float out_min, float out_max); 7 | float constrain(float x, float min, float max); 8 | 9 | #endif -------------------------------------------------------------------------------- /source_code/lib/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OPRobots/ZoroBot3/8818647d85f36d514160bcd7e03b3843404ece32/source_code/lib/.gitignore -------------------------------------------------------------------------------- /source_code/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | ; [env:disco_f407vg] 12 | ; platform = https://github.com/platformio/platform-ststm32.git 13 | ; board = disco_f407vg 14 | ; framework = libopencm3 15 | ; upload_protocol = stlink 16 | 17 | 18 | ; [env:1bitsy_stm32f415rgt] 19 | ; platform = https://github.com/platformio/platform-ststm32.git 20 | ; board = 1bitsy_stm32f415rgt 21 | ; framework = libopencm3 22 | ; upload_protocol = stlink 23 | 24 | [env:custom_STM32F405RGT6] 25 | platform = ststm32 26 | board = genericSTM32F405RG 27 | board_upload.maximum_size = 917504 28 | framework = libopencm3 29 | upload_protocol = stlink 30 | monitor_speed = 115200 31 | monitor_port = COM5 32 | build_flags = -Wl,-u,_printf_float,-u,_scanf_float 33 | debug_tool = stlink -------------------------------------------------------------------------------- /source_code/src/battery.c: -------------------------------------------------------------------------------- 1 | #include "battery.h" 2 | 3 | static float battery_full_voltage = BATTERY_3S_LOW_LIMIT_VOLTAGE; 4 | static volatile float battery_voltage = 0; 5 | 6 | void update_battery_voltage(void) { 7 | float voltage = get_aux_raw(AUX_BATTERY_ID) * ADC_LSB * VOLT_DIV_FACTOR; 8 | if (battery_voltage == 0) { 9 | battery_voltage = voltage; 10 | } else { 11 | battery_voltage = BATTERY_VOLTAGE_LOW_PASS_FILTER_ALPHA * voltage + (1 - BATTERY_VOLTAGE_LOW_PASS_FILTER_ALPHA) * battery_voltage; 12 | } 13 | } 14 | 15 | float get_battery_voltage(void) { 16 | return battery_voltage; 17 | } 18 | 19 | float get_battery_high_limit_voltage(void) { 20 | return battery_full_voltage; 21 | } 22 | 23 | void show_battery_level(void) { 24 | uint32_t ticksInicio = get_clock_ticks(); 25 | float voltage = get_battery_voltage(); 26 | float battery_level = 0; 27 | if (voltage >= BATTERY_3S_LOW_LIMIT_VOLTAGE && voltage <= BATTERY_3S_HIGH_LIMIT_VOLTAGE) { 28 | battery_full_voltage = BATTERY_3S_HIGH_LIMIT_VOLTAGE; 29 | battery_level = map(voltage, BATTERY_3S_LOW_LIMIT_VOLTAGE, BATTERY_3S_HIGH_LIMIT_VOLTAGE, 0.0f, 100.0f); 30 | } else if (voltage >= BATTERY_2S_LOW_LIMIT_VOLTAGE/* && voltage <= BATTERY_2S_HIGH_LIMIT_VOLTAGE */) { 31 | battery_full_voltage = BATTERY_2S_HIGH_LIMIT_VOLTAGE; 32 | battery_level = map(voltage, BATTERY_2S_LOW_LIMIT_VOLTAGE, BATTERY_2S_HIGH_LIMIT_VOLTAGE, 0.0f, 100.0f); 33 | } 34 | while (get_clock_ticks() < ticksInicio + 750) { 35 | set_leds_battery_level(battery_level); 36 | } 37 | all_leds_clear(); 38 | } 39 | -------------------------------------------------------------------------------- /source_code/src/buttons.c: -------------------------------------------------------------------------------- 1 | #include "buttons.h" 2 | 3 | static uint32_t btn_analog = 0; 4 | 5 | static uint32_t btn_menu_up_ms = 0; 6 | static uint32_t btn_menu_down_ms = 0; 7 | static uint32_t btn_menu_mode_ms = 0; 8 | 9 | static bool debug_btn = false; 10 | 11 | void check_buttons(void) { 12 | btn_analog = get_aux_raw(AUX_MENU_BTN_ID); 13 | 14 | if (btn_analog >= 1300 && btn_analog <= 1700) { 15 | if (btn_menu_up_ms == 0) { 16 | btn_menu_up_ms = get_clock_ticks(); 17 | } 18 | } else { 19 | btn_menu_up_ms = 0; 20 | } 21 | if (btn_analog >= 2300 && btn_analog <= 2700) { 22 | if (btn_menu_mode_ms == 0) { 23 | btn_menu_mode_ms = get_clock_ticks(); 24 | } 25 | } else { 26 | btn_menu_mode_ms = 0; 27 | } 28 | if (btn_analog >= 3700) { 29 | if (btn_menu_down_ms == 0) { 30 | btn_menu_down_ms = get_clock_ticks(); 31 | } 32 | } else { 33 | btn_menu_down_ms = 0; 34 | } 35 | } 36 | 37 | /** 38 | * @brief Obtiene el estado del botón de Menú Arriba 39 | * 40 | * @return bool 41 | */ 42 | bool get_menu_up_btn(void) { 43 | return btn_menu_up_ms > 0 && get_clock_ticks() - btn_menu_up_ms > 50; 44 | } 45 | 46 | /** 47 | * @brief Obtiene el estado del botón de Menú Abajo 48 | * 49 | * @return bool 50 | */ 51 | bool get_menu_down_btn(void) { 52 | return btn_menu_down_ms > 0 && get_clock_ticks() - btn_menu_down_ms > 50; 53 | } 54 | 55 | /** 56 | * @brief Obtiene el estado del botón de Menú Modo 57 | * 58 | * @return bool 59 | */ 60 | bool get_menu_mode_btn(void) { 61 | return btn_menu_mode_ms > 0 && get_clock_ticks() - btn_menu_mode_ms > 50; 62 | } 63 | 64 | void set_debug_btn(bool state){ 65 | debug_btn = state; 66 | } 67 | 68 | bool get_debug_btn(void){ 69 | return debug_btn; 70 | } -------------------------------------------------------------------------------- /source_code/src/calibrations.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | bool calibration_enabled = false; 4 | 5 | static void check_calibration_active(void) { 6 | if (get_menu_mode_btn()) { 7 | while (get_menu_mode_btn()) { 8 | } 9 | calibration_enabled = !calibration_enabled; 10 | } 11 | } 12 | 13 | void calibrate_from_config(uint8_t type) { 14 | if (type != CALIBRATE_NONE) { 15 | check_calibration_active(); 16 | } else { 17 | calibration_enabled = false; 18 | } 19 | if (calibration_enabled) { 20 | clear_info_leds(); 21 | set_RGB_color(0, 125, 0); 22 | delay(2000); 23 | switch (type) { 24 | case CALIBRATE_GYRO_Z: 25 | lsm6dsr_gyro_z_calibration(); 26 | break; 27 | case CALIBRATE_SIDE_SENSORS_OFFSET: 28 | side_sensors_calibration(); 29 | break; 30 | case CALIBRATE_FRONT_SENSORS: 31 | front_sensors_calibration(); 32 | break; 33 | case CALIBRATE_STORE_EEPROM: 34 | eeprom_save(); 35 | break; 36 | } 37 | set_RGB_color(0, 0, 0); 38 | calibration_enabled = false; 39 | menu_config_reset_values(); 40 | } 41 | } 42 | 43 | void calibrate_manual_distances(void) { 44 | if (get_debug_btn()) { 45 | delay(500); 46 | printf("L: %4d FL: %4d FR: %4d R: %4d\n", get_sensor_raw_filter(SENSOR_SIDE_LEFT_WALL_ID), get_sensor_raw_filter(SENSOR_FRONT_LEFT_WALL_ID), get_sensor_raw_filter(SENSOR_FRONT_RIGHT_WALL_ID), get_sensor_raw_filter(SENSOR_SIDE_RIGHT_WALL_ID)); 47 | set_debug_btn(false); 48 | } 49 | } -------------------------------------------------------------------------------- /source_code/src/config.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | uint16_t _run_mode; 4 | 5 | /** 6 | * @brief Establece la configuración de Carrera 7 | * 8 | * @param run_mode CONFIG_RUN_RACE | CONFIG_RUN_DEBUG 9 | */ 10 | static void set_config_run(uint16_t run_mode) { 11 | _run_mode = run_mode; 12 | } 13 | 14 | /** 15 | * @brief Establece todas las configuraciones (CARRERA, TODO: añadir más) en funcion de los Switches 16 | * 17 | */ 18 | void set_all_configs(void) { 19 | 20 | // set_config_run(CONFIG_RUN_RACE); 21 | set_config_run(CONFIG_RUN_DEBUG); 22 | 23 | 24 | } 25 | 26 | /** 27 | * @brief Obtiene la configuración de Carrera 28 | * 29 | * @return uint16_t CONFIG_RUN_RACE | CONFIG_RUN_DEBUG 30 | */ 31 | uint16_t get_config_run(void) { 32 | return _run_mode; 33 | } -------------------------------------------------------------------------------- /source_code/src/control.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static volatile bool race_started = false; 4 | static volatile uint32_t race_finish_ms = 0; 5 | static volatile uint32_t sensor_front_left_start_ms = 0; 6 | static volatile uint32_t sensor_front_right_start_ms = 0; 7 | 8 | static volatile bool control_debug = false; 9 | 10 | static volatile int32_t target_linear_speed = 0; 11 | static volatile int32_t ideal_linear_speed = 0; 12 | static volatile float ideal_angular_speed = 0.0; 13 | 14 | static volatile int32_t target_fan_speed = 0; 15 | static volatile float ideal_fan_speed = 0; 16 | static volatile float fan_speed_accel = 0; 17 | 18 | static volatile float linear_error; 19 | static volatile float last_linear_error; 20 | 21 | static volatile float angular_error; 22 | static volatile float last_angular_error; 23 | 24 | static volatile bool side_sensors_close_correction_enabled = false; 25 | static volatile bool side_sensors_far_correction_enabled = false; 26 | static volatile bool front_sensors_correction_enabled = false; 27 | static volatile bool front_sensors_diagonal_correction_enabled = false; 28 | 29 | static volatile float side_sensors_error; 30 | static volatile float last_side_sensors_error; 31 | static volatile float sum_side_sensors_error; 32 | 33 | static volatile float front_sensors_error; 34 | static volatile float sum_front_sensors_error; 35 | 36 | static volatile float front_sensors_diagonal_error; 37 | static volatile float last_front_sensors_diagonal_error; 38 | static volatile float sum_front_sensors_diagonal_error; 39 | 40 | static volatile float voltage_left; 41 | static volatile float voltage_right; 42 | static volatile int32_t pwm_left; 43 | static volatile int32_t pwm_right; 44 | 45 | /** 46 | * @brief Convierte un valor de voltaje dado a su correspondiente PWM 47 | * 48 | * @param voltage 49 | * @return int32_t PWM a aplicar al motor 50 | */ 51 | static int32_t voltage_to_motor_pwm(float voltage) { 52 | return voltage / /* 8.0 */ get_battery_voltage() * MOTORES_MAX_PWM; 53 | } 54 | 55 | static int32_t percentage_to_fan_pwm(float percentage) { 56 | return percentage > 0 ? (int32_t)constrain((get_battery_high_limit_voltage() / get_battery_voltage()) * percentage, percentage, 100.0f) : 0; 57 | } 58 | 59 | /** 60 | * @brief Actualiza la velocidad lineal ideal en función de la velocidad lineal objetivo y la aceleración 61 | * 62 | */ 63 | static void update_ideal_linear_speed(void) { 64 | if (ideal_linear_speed < target_linear_speed) { 65 | int16_t accel = get_kinematics().linear_accel.accel_soft; 66 | if (get_kinematics().linear_accel.speed_hard == 0 || ideal_linear_speed < get_kinematics().linear_accel.speed_hard) { 67 | accel = get_kinematics().linear_accel.accel_hard; 68 | } 69 | ideal_linear_speed += accel / CONTROL_FREQUENCY_HZ; 70 | if (ideal_linear_speed > target_linear_speed) { 71 | ideal_linear_speed = target_linear_speed; 72 | } 73 | } else if (ideal_linear_speed > target_linear_speed) { 74 | ideal_linear_speed -= get_kinematics().linear_accel.break_accel / CONTROL_FREQUENCY_HZ; 75 | if (ideal_linear_speed < target_linear_speed) { 76 | ideal_linear_speed = target_linear_speed; 77 | } 78 | } 79 | } 80 | 81 | static void update_fan_speed(void) { 82 | if (ideal_fan_speed < target_fan_speed) { 83 | ideal_fan_speed += fan_speed_accel / CONTROL_FREQUENCY_HZ; 84 | if (ideal_fan_speed > target_fan_speed) { 85 | ideal_fan_speed = target_fan_speed; 86 | } 87 | } else if (ideal_fan_speed > target_fan_speed) { 88 | ideal_fan_speed += fan_speed_accel / CONTROL_FREQUENCY_HZ; 89 | if (ideal_fan_speed < target_fan_speed) { 90 | ideal_fan_speed = target_fan_speed; 91 | } 92 | } 93 | } 94 | 95 | static float get_measured_linear_speed(void) { 96 | return (get_encoder_left_speed() + get_encoder_right_speed()) / 2.0f; 97 | } 98 | 99 | static float get_measured_angular_speed(void) { 100 | return -lsm6dsr_get_gyro_z_radps(); 101 | } 102 | 103 | /** 104 | * @brief Comprueba si el robot está en funcionamiento 105 | * 106 | * @return bool 107 | */ 108 | 109 | bool is_race_started(void) { 110 | return race_started; 111 | } 112 | 113 | /** 114 | * @brief Establece el estado actual del robot 115 | * 116 | * @param state Estado actual del robot 117 | */ 118 | 119 | void set_race_started(bool state) { 120 | reset_control_all(); 121 | race_started = state; 122 | if (!state) { 123 | menu_reset(); 124 | race_finish_ms = get_clock_ticks(); 125 | } 126 | } 127 | 128 | void set_control_debug(bool state) { 129 | control_debug = state; 130 | } 131 | 132 | int8_t check_start_run(void) { 133 | if (get_sensor_distance(SENSOR_FRONT_LEFT_WALL_ID) <= SENSOR_FRONT_DETECTION_START) { 134 | if (sensor_front_left_start_ms == 0 && sensor_front_right_start_ms == 0) { 135 | sensor_front_left_start_ms = get_clock_ticks(); 136 | } 137 | } else { 138 | sensor_front_left_start_ms = 0; 139 | } 140 | if (get_sensor_distance(SENSOR_FRONT_RIGHT_WALL_ID) <= SENSOR_FRONT_DETECTION_START) { 141 | if (sensor_front_left_start_ms == 0 && sensor_front_right_start_ms == 0) { 142 | sensor_front_right_start_ms = get_clock_ticks(); 143 | } 144 | } else { 145 | sensor_front_right_start_ms = 0; 146 | } 147 | 148 | if (sensor_front_left_start_ms >= SENSOR_START_MIN_MS || sensor_front_right_start_ms >= SENSOR_START_MIN_MS) { 149 | set_RGB_color(0, 50, 0); 150 | delay(1000); 151 | set_RGB_color(0, 0, 0); 152 | set_race_started(true); 153 | uint8_t sensor = sensor_front_left_start_ms >= SENSOR_START_MIN_MS ? SENSOR_FRONT_LEFT_WALL_ID : SENSOR_FRONT_RIGHT_WALL_ID; 154 | sensor_front_left_start_ms = 0; 155 | sensor_front_right_start_ms = 0; 156 | menu_run_reset(); 157 | return sensor; 158 | } 159 | return -1; 160 | } 161 | 162 | void set_side_sensors_close_correction(bool enabled) { 163 | side_sensors_close_correction_enabled = enabled; 164 | } 165 | 166 | void set_side_sensors_far_correction(bool enabled) { 167 | side_sensors_far_correction_enabled = enabled; 168 | } 169 | 170 | void set_front_sensors_correction(bool enabled) { 171 | front_sensors_correction_enabled = enabled; 172 | } 173 | 174 | bool is_front_sensors_correction_enabled(void) { 175 | return front_sensors_correction_enabled; 176 | } 177 | 178 | void set_front_sensors_diagonal_correction(bool enabled) { 179 | front_sensors_diagonal_correction_enabled = enabled; 180 | } 181 | 182 | void disable_sensors_correction(void) { 183 | set_front_sensors_correction(false); 184 | set_front_sensors_diagonal_correction(false); 185 | set_side_sensors_close_correction(false); 186 | set_side_sensors_far_correction(false); 187 | } 188 | 189 | void reset_control_errors(void) { 190 | sum_side_sensors_error = 0; 191 | last_side_sensors_error = 0; 192 | sum_front_sensors_error = 0; 193 | sum_front_sensors_diagonal_error = 0; 194 | linear_error = 0; 195 | angular_error = 0; 196 | last_linear_error = 0; 197 | last_angular_error = 0; 198 | } 199 | 200 | void reset_control_speed(void) { 201 | target_linear_speed = 0; 202 | ideal_linear_speed = 0; 203 | ideal_angular_speed = 0.0; 204 | voltage_left = 0; 205 | voltage_right = 0; 206 | pwm_left = 0; 207 | pwm_right = 0; 208 | } 209 | 210 | void reset_control_all(void) { 211 | reset_control_errors(); 212 | reset_control_speed(); 213 | reset_motors_saturated(); 214 | reset_encoder_avg(); 215 | } 216 | 217 | void set_target_linear_speed(int32_t linear_speed) { 218 | target_linear_speed = linear_speed; 219 | } 220 | 221 | int32_t get_ideal_linear_speed(void) { 222 | return ideal_linear_speed; 223 | } 224 | 225 | void set_ideal_angular_speed(float angular_speed) { 226 | ideal_angular_speed = angular_speed; 227 | } 228 | 229 | float get_ideal_angular_speed(void) { 230 | return ideal_angular_speed; 231 | } 232 | 233 | void set_target_fan_speed(int32_t fan_speed, int32_t ms) { 234 | target_fan_speed = fan_speed; // percentage_to_fan_pwm(fan_speed); 235 | fan_speed_accel = (fan_speed - ideal_fan_speed) * CONTROL_FREQUENCY_HZ / ms; 236 | } 237 | 238 | /** 239 | * @brief Función de control general del robot 240 | * · Gestiona velocidades, aceleraciones, correcciones, ... 241 | * 242 | */ 243 | void control_loop(void) { 244 | // gpio_set(GPIOB, GPIO13); 245 | // delay_us(100); 246 | // gpio_clear(GPIOB, GPIO13); 247 | // return; 248 | if (is_debug_enabled()) { 249 | return; 250 | } 251 | if (is_motor_saturated() && is_race_started()) { 252 | set_motors_speed(0, 0); 253 | set_fan_speed(0); 254 | if (get_clock_ticks() - get_motors_saturated_ms() < 3000) { 255 | blink_RGB_color(512, 0, 0, 50); 256 | } else { 257 | set_RGB_color(0, 0, 0); 258 | set_race_started(false); 259 | } 260 | return; 261 | } 262 | if (!is_race_started()) { 263 | if (race_finish_ms > 0 && get_clock_ticks() - race_finish_ms <= 3000) { 264 | set_motors_brake(); 265 | } else { 266 | set_motors_speed(0, 0); 267 | set_motors_enable(false); 268 | } 269 | set_fan_speed(0); 270 | return; 271 | } else { 272 | set_motors_enable(true); 273 | } 274 | 275 | update_ideal_linear_speed(); 276 | update_fan_speed(); 277 | set_fan_speed(ideal_fan_speed); 278 | 279 | float linear_voltage = 0; 280 | float angular_voltage = 0; 281 | 282 | last_linear_error = linear_error; 283 | linear_error += ideal_linear_speed - get_measured_linear_speed(); 284 | 285 | last_angular_error = angular_error; 286 | angular_error += ideal_angular_speed - get_measured_angular_speed(); 287 | 288 | // side_sensors_error = 0; 289 | // if (side_sensors_close_correction_enabled) { 290 | // side_sensors_error += get_side_sensors_close_error(); 291 | // sum_side_sensors_error += side_sensors_error; 292 | // } 293 | // if (side_sensors_far_correction_enabled) { 294 | // side_sensors_error += get_side_sensors_far_error(); 295 | // sum_side_sensors_error += side_sensors_error; 296 | // } 297 | 298 | side_sensors_error = 0; 299 | if (side_sensors_close_correction_enabled) { 300 | side_sensors_error += get_side_sensors_error(); 301 | sum_side_sensors_error += side_sensors_error; 302 | } 303 | 304 | if (!side_sensors_close_correction_enabled && !side_sensors_far_correction_enabled) { 305 | sum_side_sensors_error = 0; 306 | last_side_sensors_error = 0; 307 | } 308 | 309 | front_sensors_error = 0; 310 | if (front_sensors_correction_enabled) { 311 | front_sensors_error = get_front_sensors_angle_error(); 312 | sum_front_sensors_error += front_sensors_error; 313 | } 314 | 315 | front_sensors_diagonal_error = 0; 316 | if (front_sensors_diagonal_correction_enabled) { 317 | front_sensors_diagonal_error = get_front_sensors_diagonal_error(); 318 | sum_front_sensors_diagonal_error += front_sensors_diagonal_error; 319 | // if (front_sensors_diagonal_error != 0) { 320 | // set_RGB_color(255, 0, 0); 321 | // } else { 322 | // set_RGB_color(0, 255, 0); 323 | // } 324 | } else { 325 | front_sensors_diagonal_error = 0; 326 | sum_front_sensors_diagonal_error = 0; 327 | last_front_sensors_diagonal_error = 0; 328 | } 329 | 330 | linear_voltage = KP_LINEAR * linear_error + KD_LINEAR * (linear_error - last_linear_error); 331 | 332 | angular_voltage = 333 | KP_ANGULAR * angular_error + KD_ANGULAR * (angular_error - last_angular_error) + 334 | KP_SIDE_SENSORS * side_sensors_error + KI_SIDE_SENSORS * sum_side_sensors_error + KD_SIDE_SENSORS * (side_sensors_error - last_side_sensors_error) + 335 | KP_FRONT_SENSORS * front_sensors_error + KI_FRONT_SENSORS * sum_front_sensors_error + 336 | KP_FRONT_DIAGONAL_SENSORS * front_sensors_diagonal_error + KI_FRONT_DIAGONAL_SENSORS * sum_front_sensors_diagonal_error + KD_FRONT_DIAGONAL_SENSORS * (front_sensors_diagonal_error - last_front_sensors_diagonal_error); 337 | 338 | last_side_sensors_error = side_sensors_error; 339 | last_front_sensors_diagonal_error = front_sensors_diagonal_error; 340 | 341 | // if (get_ideal_linear_speed() > 0) { 342 | // angular_voltage *= get_ideal_linear_speed() / 500.0f; // TODO: definear 500 as explore speed 343 | // } 344 | 345 | voltage_left = linear_voltage + angular_voltage; 346 | voltage_right = linear_voltage - angular_voltage; 347 | pwm_left = voltage_to_motor_pwm(voltage_left); 348 | pwm_right = voltage_to_motor_pwm(voltage_right); 349 | set_motors_pwm(pwm_left, pwm_right); 350 | 351 | if (ideal_linear_speed != 0 || ideal_angular_speed != 0) { 352 | // static char *labels[] = { 353 | // "target_linear_speed", 354 | // "ideal_linear_speed", 355 | // "measured_linear_speed", 356 | // "ideal_angular_speed", 357 | // "measured_angular_speed", 358 | // "side_sensors_error", 359 | // "last_side_sensors_error", 360 | // "encoder_avg_millimeters", 361 | // "battery_voltage"}; 362 | // macroarray_store( 363 | // 0, 364 | // 0b000110001, 365 | // labels, 366 | // 9, 367 | // (int16_t)target_linear_speed, 368 | // (int16_t)ideal_linear_speed, 369 | // (int16_t)(get_measured_linear_speed()), 370 | // (int16_t)(ideal_angular_speed * 100), 371 | // (int16_t)(get_measured_angular_speed() * 100), 372 | // (int16_t)side_sensors_error, 373 | // (int16_t)last_side_sensors_error, 374 | // (int16_t)get_encoder_avg_millimeters(), 375 | // (int16_t)(get_battery_voltage() * 100)); 376 | 377 | // static char *labels[] = { 378 | // "target_linear_speed", 379 | // "ideal_linear_speed", 380 | // "measured_linear_speed", 381 | // "ideal_angular_speed", 382 | // "measured_angular_speed", 383 | // "front_left_distance", 384 | // "front_right_distance", 385 | // "diagonal_error", 386 | // "encoder_avg_millimeters", 387 | // "wall_lost_toggle_state", 388 | // "cell_change_toggle_state"}; 389 | // macroarray_store( 390 | // 2, 391 | // 0b00011000000, 392 | // labels, 393 | // 11, 394 | // (int16_t)target_linear_speed, 395 | // (int16_t)ideal_linear_speed, 396 | // (int16_t)(get_measured_linear_speed()), 397 | // (int16_t)(ideal_angular_speed * 100.0), 398 | // (int16_t)(get_measured_angular_speed() * 100), 399 | // (int16_t)get_sensor_distance(SENSOR_FRONT_LEFT_WALL_ID), 400 | // (int16_t)get_sensor_distance(SENSOR_FRONT_RIGHT_WALL_ID), 401 | // (int16_t)front_sensors_diagonal_error, 402 | // (int16_t)get_encoder_avg_millimeters(), 403 | // (int16_t)get_wall_lost_toggle_state() ? 1 : 0, 404 | // (int16_t)get_cell_change_toggle_state() ? 1 : 0 405 | // ); 406 | 407 | // static char *labels[] = { 408 | // "sl", 409 | // "fl", 410 | // "fr", 411 | // "sr", 412 | // }; 413 | // macroarray_store( 414 | // 0, 415 | // 0b0, 416 | // labels, 417 | // 4, 418 | // (int16_t)get_sensor_distance(SENSOR_SIDE_LEFT_WALL_ID), 419 | // (int16_t)get_sensor_distance(SENSOR_FRONT_LEFT_WALL_ID), 420 | // (int16_t)get_sensor_distance(SENSOR_FRONT_RIGHT_WALL_ID), 421 | // (int16_t)get_sensor_distance(SENSOR_SIDE_RIGHT_WALL_ID)); 422 | } 423 | } 424 | 425 | void keep_z_angle(void) { 426 | float linear_voltage = 0; 427 | float angular_voltage = 0; 428 | last_linear_error = linear_error; 429 | linear_error += ideal_linear_speed - get_measured_linear_speed(); 430 | 431 | last_angular_error = angular_error; 432 | angular_error += ideal_angular_speed - get_measured_angular_speed(); 433 | 434 | angular_voltage = KP_ANGULAR * angular_error + KD_ANGULAR * (angular_error - last_angular_error); 435 | linear_voltage = KP_LINEAR * linear_error + KD_LINEAR * (linear_error - last_linear_error); 436 | voltage_left = linear_voltage + angular_voltage; 437 | voltage_right = linear_voltage - angular_voltage; 438 | pwm_left = voltage_to_motor_pwm(voltage_left); 439 | pwm_right = voltage_to_motor_pwm(voltage_right); 440 | gpio_set(GPIOB, GPIO15); 441 | set_motors_pwm(pwm_left, pwm_right); 442 | } -------------------------------------------------------------------------------- /source_code/src/debug.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | bool debug_enabled = false; 4 | uint32_t last_print_debug = 0; 5 | 6 | uint32_t last_keep_z_angle = 0; 7 | 8 | static void debug_macroarray(void) { 9 | macroarray_print(); 10 | debug_enabled = false; 11 | menu_config_reset_values(); 12 | } 13 | 14 | /** 15 | * @brief Imprime los valores de los sensores sin aplicar ninguna corrección 16 | * 17 | */ 18 | static void debug_sensors_raw(void) { 19 | if (get_clock_ticks() > last_print_debug + 50) { 20 | printf("SL: %4d - %4d = %4d ", get_sensor_raw(SENSOR_SIDE_LEFT_WALL_ID, 1), get_sensor_raw(SENSOR_SIDE_LEFT_WALL_ID, 0), get_sensor_raw_filter(SENSOR_SIDE_LEFT_WALL_ID)); 21 | printf("FL: %4d - %4d = %4d ", get_sensor_raw(SENSOR_FRONT_LEFT_WALL_ID, 1), get_sensor_raw(SENSOR_FRONT_LEFT_WALL_ID, 0), get_sensor_raw_filter(SENSOR_FRONT_LEFT_WALL_ID)); 22 | printf("FR: %4d - %4d = %4d ", get_sensor_raw(SENSOR_FRONT_RIGHT_WALL_ID, 1), get_sensor_raw(SENSOR_FRONT_RIGHT_WALL_ID, 0), get_sensor_raw_filter(SENSOR_FRONT_RIGHT_WALL_ID)); 23 | printf("SR: %4d - %4d = %4d ", get_sensor_raw(SENSOR_SIDE_RIGHT_WALL_ID, 1), get_sensor_raw(SENSOR_SIDE_RIGHT_WALL_ID, 0), get_sensor_raw_filter(SENSOR_SIDE_RIGHT_WALL_ID)); 24 | printf("\n"); 25 | last_print_debug = get_clock_ticks(); 26 | } 27 | } 28 | 29 | static void debug_sensors_distances(void) { 30 | if (get_clock_ticks() > last_print_debug + 50) { 31 | printf("SL: %4d ", get_sensor_distance(SENSOR_SIDE_LEFT_WALL_ID)); 32 | printf("FL: %4d ", get_sensor_distance(SENSOR_FRONT_LEFT_WALL_ID)); 33 | printf("FR: %4d ", get_sensor_distance(SENSOR_FRONT_RIGHT_WALL_ID)); 34 | printf("SR: %4d ", get_sensor_distance(SENSOR_SIDE_RIGHT_WALL_ID)); 35 | printf("\n"); 36 | last_print_debug = get_clock_ticks(); 37 | } 38 | } 39 | 40 | static void debug_floodfill_maze(void) { 41 | floodfill_maze_print(); 42 | debug_enabled = false; 43 | menu_config_reset_values(); 44 | } 45 | 46 | static void check_debug_active(void) { 47 | if (get_menu_mode_btn()) { 48 | while (get_menu_mode_btn()) { 49 | } 50 | debug_enabled = !debug_enabled; 51 | if (!debug_enabled) { 52 | menu_config_reset_values(); 53 | } 54 | } 55 | } 56 | 57 | static void debug_motors_current(void) { 58 | if (get_clock_ticks() > last_print_debug + 50) { 59 | set_motors_enable(true); 60 | set_motors_speed(150, 150); 61 | printf("BA: %4d CI: %4d CD: %4d BO: %4d\n", get_aux_raw(AUX_BATTERY_ID), get_aux_raw(AUX_CURRENT_LEFT_ID), get_aux_raw(AUX_CURRENT_RIGHT_ID), get_aux_raw(AUX_MENU_BTN_ID)); 62 | last_print_debug = get_clock_ticks(); 63 | } 64 | } 65 | 66 | static void debug_gyro_demo(void) { 67 | reset_control_all(); 68 | delay(1000); 69 | do { 70 | if (get_clock_ticks() >= last_keep_z_angle + 1) { 71 | keep_z_angle(); 72 | last_keep_z_angle = get_clock_ticks(); 73 | } 74 | check_debug_active(); 75 | } while (debug_enabled); 76 | set_fan_speed(0); 77 | reset_control_all(); 78 | } 79 | 80 | static void debug_fan_demo(void) { 81 | reset_control_all(); 82 | delay(1000); 83 | set_fan_speed(50); 84 | do { 85 | if (get_clock_ticks() >= last_keep_z_angle + 1) { 86 | keep_z_angle(); 87 | last_keep_z_angle = get_clock_ticks(); 88 | } 89 | check_debug_active(); 90 | } while (debug_enabled); 91 | set_fan_speed(0); 92 | reset_control_all(); 93 | } 94 | 95 | bool is_debug_enabled(void) { 96 | return debug_enabled; 97 | } 98 | 99 | void debug_from_config(uint8_t type) { 100 | if (type != DEBUG_NONE) { 101 | check_debug_active(); 102 | } else { 103 | debug_enabled = false; 104 | } 105 | if (debug_enabled) { 106 | set_sensors_enabled(true); 107 | set_RGB_color(0, 50, 0); 108 | switch (type) { 109 | case DEBUG_MACROARRAY: 110 | debug_macroarray(); 111 | break; 112 | case DEBUG_TYPE_SENSORS_RAW: 113 | debug_sensors_raw(); 114 | break; 115 | case DEBUG_TYPE_SENSORS_DISTANCES: 116 | debug_sensors_distances(); 117 | break; 118 | case DEBUG_FLOODFILL_MAZE: 119 | debug_floodfill_maze(); 120 | break; 121 | case DEBUG_MOTORS_CURRENT: 122 | debug_motors_current(); 123 | break; 124 | case DEBUG_GYRO_DEMO: 125 | debug_gyro_demo(); 126 | break; 127 | case DEBUG_FAN_DEMO: 128 | debug_fan_demo(); 129 | break; 130 | default: 131 | debug_enabled = false; 132 | break; 133 | } 134 | } else { 135 | set_RGB_color(0, 0, 0); 136 | } 137 | } 138 | 139 | void debug_from_main(uint8_t type) { 140 | debug_enabled = true; 141 | set_sensors_enabled(true); 142 | debug_from_config(type); 143 | } -------------------------------------------------------------------------------- /source_code/src/delay.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static volatile uint32_t clock_ticks; 4 | 5 | void clock_tick(void) { 6 | clock_ticks++; 7 | } 8 | 9 | uint32_t read_cycle_counter(void) { 10 | return dwt_read_cycle_counter(); 11 | } 12 | 13 | uint32_t get_us_counter(void){ 14 | return dwt_read_cycle_counter() / (SYSCLK_FREQUENCY_HZ / MICROSECONDS_PER_SECOND); 15 | } 16 | 17 | void delay(uint32_t ms) { 18 | uint32_t awake = clock_ticks + ms; 19 | 20 | while (awake > clock_ticks) { 21 | }; 22 | } 23 | 24 | uint32_t get_clock_ticks(void) { 25 | return clock_ticks; 26 | } 27 | 28 | void delay_us(uint32_t us) { 29 | uint32_t initial_cycles = dwt_read_cycle_counter(); 30 | uint32_t sleep_cycles = (uint32_t)(SYSCLK_FREQUENCY_HZ * ((float)us / (float)MICROSECONDS_PER_SECOND)); 31 | 32 | while (dwt_read_cycle_counter() - initial_cycles <= sleep_cycles) { 33 | }; 34 | } -------------------------------------------------------------------------------- /source_code/src/eeprom.c: -------------------------------------------------------------------------------- 1 | #include "eeprom.h" 2 | 3 | static int16_t eeprom_data[DATA_LENGTH]; 4 | 5 | void eeprom_save(void) { 6 | uint32_t addr = EEPROM_BASE_ADDRESS; 7 | set_status_led(false); 8 | for (uint16_t i = 0; i < 2; i++) { 9 | toggle_status_led(); 10 | delay_us(50000); 11 | toggle_status_led(); 12 | delay_us(50000); 13 | } 14 | 15 | set_status_led(true); 16 | flash_unlock(); 17 | flash_erase_sector(EEPROM_SECTOR, FLASH_CR_PROGRAM_X16); 18 | for (uint16_t i = 0; i < DATA_LENGTH; i++) { 19 | flash_program_word(addr, eeprom_data[i]); 20 | addr += 4; 21 | } 22 | flash_lock(); 23 | set_status_led(false); 24 | } 25 | 26 | void eeprom_load(void) { 27 | uint32_t addr = EEPROM_BASE_ADDRESS; 28 | for (uint16_t i = 0; i < DATA_LENGTH; i++) { 29 | eeprom_data[i] = MMIO32(addr); 30 | addr += 4; 31 | } 32 | 33 | lsm6dsr_load_eeprom(); 34 | sensors_load_eeprom(); 35 | floodfill_load_maze(); 36 | menu_run_load_values(); 37 | rc5_load_eeprom(); 38 | } 39 | 40 | void eeprom_clear(void) { 41 | flash_unlock(); 42 | flash_erase_sector(EEPROM_SECTOR, FLASH_CR_PROGRAM_X16); 43 | flash_lock(); 44 | } 45 | 46 | void eeprom_backup(void) { 47 | printf("int16_t eeprom_backup[DATA_LENGTH] = {"); 48 | for (uint16_t i = 0; i < DATA_LENGTH; i++) { 49 | printf("%d,", eeprom_data[i]); 50 | } 51 | printf("};\n"); 52 | } 53 | 54 | // clang-format off 55 | void eeprom_restore(void){ 56 | int16_t eeprom_backup[DATA_LENGTH] = {1,56,0,0,2,17,520,85,325,45,15,28,4,20,20,6,9,21,17,21,5,2,11,12,4,6,11,10,11,24,16,2,11,10,11,13,7,26,9,2,25,19,25,21,17,19,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,1,}; 57 | 58 | for (uint16_t i = 0; i < DATA_LENGTH; i++) { 59 | eeprom_data[i] = eeprom_backup[i]; 60 | } 61 | } 62 | // clang-format on 63 | 64 | void eeprom_set_data(uint16_t index, int16_t *data, uint16_t length) { 65 | for (uint16_t i = index; i < index + length; i++) { 66 | eeprom_data[i] = data[i - index]; 67 | } 68 | } 69 | 70 | int16_t *eeprom_get_data(void) { 71 | return eeprom_data; 72 | } -------------------------------------------------------------------------------- /source_code/src/encoders.c: -------------------------------------------------------------------------------- 1 | #include "encoders.h" 2 | 3 | /* Difference between the current count and the latest count */ 4 | static volatile int32_t left_diff_ticks; 5 | static volatile int32_t right_diff_ticks; 6 | 7 | /* Total number of counts */ 8 | static volatile int32_t left_total_ticks; 9 | static volatile int32_t right_total_ticks; 10 | 11 | /* Total travelled distance, in micrometers */ 12 | static volatile int32_t left_micrometers; 13 | static volatile int32_t right_micrometers; 14 | static volatile float avg_micrometers; 15 | 16 | /* Total travelled distance, in millimeters */ 17 | static volatile int32_t left_millimeters; 18 | static volatile int32_t right_millimeters; 19 | static volatile float avg_millimeters; 20 | 21 | /* Speed, in millimeters per second */ 22 | static volatile float left_speed; 23 | static volatile float right_speed; 24 | static volatile float angular_speed; 25 | 26 | /** 27 | * @brief Read left motor encoder counter. 28 | */ 29 | static uint16_t read_encoder_left(void) { 30 | return (uint16_t)timer_get_counter(TIM4); 31 | } 32 | 33 | /** 34 | * @brief Read right motor encoder counter. 35 | */ 36 | static uint16_t read_encoder_right(void) { 37 | return (uint16_t)timer_get_counter(TIM3); 38 | } 39 | 40 | int32_t get_encoder_left_ticks(void) { 41 | return left_total_ticks; 42 | } 43 | 44 | int32_t get_encoder_right_ticks(void) { 45 | return right_total_ticks; 46 | } 47 | 48 | /** 49 | * @brief Read left motor encoder travelled distance in micrometers. 50 | */ 51 | int32_t get_encoder_left_micrometers(void) { 52 | return left_micrometers; 53 | } 54 | 55 | /** 56 | * @brief Read right motor encoder travelled distance in micrometers. 57 | */ 58 | int32_t get_encoder_right_micrometers(void) { 59 | return right_micrometers; 60 | } 61 | 62 | /** 63 | * @brief Read the average travelled distance in micrometers. 64 | */ 65 | int32_t get_encoder_avg_micrometers(void) { 66 | return (left_micrometers + right_micrometers) / 2; 67 | } 68 | 69 | /** 70 | * @brief Read left motor encoder travelled distance in millimeters. 71 | */ 72 | int32_t get_encoder_left_millimeters(void) { 73 | return left_millimeters; 74 | } 75 | 76 | /** 77 | * @brief Read right motor encoder travelled distance in millimeters. 78 | */ 79 | int32_t get_encoder_right_millimeters(void) { 80 | return right_millimeters; 81 | } 82 | 83 | /** 84 | * @brief Read the average travelled distance in millimeters. 85 | */ 86 | int32_t get_encoder_avg_millimeters(void) { 87 | return (left_millimeters + right_millimeters) / 2; 88 | } 89 | 90 | void reset_encoder_avg(void) { 91 | left_micrometers = 0; 92 | right_micrometers = 0; 93 | avg_micrometers = 0; 94 | left_millimeters = 0; 95 | right_millimeters = 0; 96 | avg_millimeters = 0; 97 | } 98 | 99 | /** 100 | * @brief Read left motor speed in millimeters per second. 101 | */ 102 | float get_encoder_left_speed(void) { 103 | return left_speed; 104 | } 105 | 106 | /** 107 | * @brief Read right motor speed in millimeters per second. 108 | */ 109 | float get_encoder_right_speed(void) { 110 | return right_speed; 111 | } 112 | 113 | /** 114 | * @brief Read the average motor speed in millimeters per second. 115 | */ 116 | float get_encoder_avg_speed(void) { 117 | return (left_speed + right_speed) / 2.0f; 118 | } 119 | 120 | float get_encoder_angular_speed(void) { 121 | return angular_speed; 122 | } 123 | 124 | /** 125 | * @brief Return the most likely counter difference. 126 | * 127 | * When reading an increasing or decreasing counter caution must be taken to: 128 | * 129 | * - Account for counter overflow. 130 | * - Account for counter direction (increasing or decreasing). 131 | * 132 | * This function assumes the most likely situation is to read the counter fast 133 | * enough to ensure that the direction is defined by the minimal difference 134 | * between the two reads (i.e.: readings are much faster than counter 135 | * overflows). 136 | * 137 | * @param[in] now Counter reading now. 138 | * @param[in] before Counter reading before. 139 | */ 140 | int32_t max_likelihood_counter_diff(uint16_t now, uint16_t before) { 141 | uint16_t forward_diff; 142 | uint16_t backward_diff; 143 | 144 | forward_diff = (uint16_t)(now - before); 145 | backward_diff = (uint16_t)(before - now); 146 | if (forward_diff > backward_diff) 147 | return -(int32_t)backward_diff; 148 | return (int32_t)forward_diff; 149 | } 150 | 151 | /** 152 | * @brief Update encoder readings. 153 | * 154 | * - Read raw encoder counters. 155 | * - Update the count differences (with respect to latest reading). 156 | * - Calculate distance travelled. 157 | * - Calculate speed. 158 | */ 159 | void update_encoder_readings(void) { 160 | static uint16_t last_left_ticks; 161 | static uint16_t last_right_ticks; 162 | 163 | uint16_t left_ticks; 164 | uint16_t right_ticks; 165 | 166 | left_ticks = read_encoder_left(); 167 | right_ticks = read_encoder_right(); 168 | left_diff_ticks = -max_likelihood_counter_diff(left_ticks, last_left_ticks); 169 | right_diff_ticks = max_likelihood_counter_diff(right_ticks, last_right_ticks); 170 | left_total_ticks += left_diff_ticks; 171 | right_total_ticks += right_diff_ticks; 172 | 173 | left_micrometers = (int32_t)(left_total_ticks * MICROMETERS_PER_TICK); 174 | right_micrometers = (int32_t)(right_total_ticks * MICROMETERS_PER_TICK); 175 | left_millimeters = left_micrometers / MICROMETERS_PER_MILLIMETER; 176 | right_millimeters = right_micrometers / MICROMETERS_PER_MILLIMETER; 177 | 178 | left_speed = left_diff_ticks * (MICROMETERS_PER_TICK / MICROMETERS_PER_MILLIMETER) * SYSTICK_FREQUENCY_HZ; 179 | right_speed = right_diff_ticks * (MICROMETERS_PER_TICK / MICROMETERS_PER_MILLIMETER) * SYSTICK_FREQUENCY_HZ; 180 | angular_speed = (float)((right_speed - left_speed) / MILLIMETERS_PER_METER) / ((float)WHEELS_SEPARATION / (float)MILLIMETERS_PER_METER); 181 | 182 | last_left_ticks = left_ticks; 183 | last_right_ticks = right_ticks; 184 | } 185 | -------------------------------------------------------------------------------- /source_code/src/floodfill_weigths.c: -------------------------------------------------------------------------------- 1 | #include "floodfill_weigths.h" 2 | 3 | static float second_degree_equation(float a, float b, float c) { 4 | double delta = b * b - 4 * a * c; 5 | if (delta < 0) { 6 | return -1; 7 | } 8 | return (float)((-b + sqrt(delta)) / (2 * a)); 9 | } 10 | 11 | static float time_penalty(uint16_t speed, uint16_t init_speed, uint16_t accel) { 12 | if (init_speed < speed) { 13 | return (speed - init_speed) / (float)accel; 14 | } else { 15 | return 0.0f; 16 | } 17 | } 18 | 19 | static float time_taken_distance(float distance, uint16_t speed, uint16_t max_speed, uint16_t accel) { 20 | if (speed < max_speed) { 21 | float time_to_max_speed = (max_speed - speed) / (float)accel; 22 | float time_to_distance = second_degree_equation(0.5f * accel, speed, -distance); 23 | if (time_to_distance < time_to_max_speed) { 24 | return time_to_distance; 25 | } else { 26 | float left_distance = distance - (speed * time_to_max_speed) + (0.5f * accel * time_to_max_speed * time_to_max_speed); 27 | return time_to_max_speed + (left_distance / max_speed); 28 | } 29 | } else { 30 | return distance / (float)speed; 31 | } 32 | return 0; 33 | } 34 | 35 | static uint16_t speed_after_time(float time, uint16_t speed, uint16_t max_speed, uint16_t accel) { 36 | if (speed < max_speed) { 37 | float speed_at_time = speed + (accel * time); 38 | return speed_at_time > max_speed ? max_speed : (uint16_t)speed_at_time; 39 | } else { 40 | return max_speed; 41 | } 42 | } 43 | 44 | uint16_t floodfill_weights_cells_to_max_speed(float distance, uint16_t init_speed, uint16_t max_speed, uint16_t accel) { 45 | float time_to_max_speed = (max_speed - init_speed) / (float)accel; 46 | float distance_to_max_speed = (init_speed * time_to_max_speed) + (0.5f * accel * time_to_max_speed * time_to_max_speed); 47 | return (uint16_t)round((distance_to_max_speed / distance)) + 2 + 1; 48 | } 49 | 50 | void floodfill_weights_table(float distance, uint16_t init_speed, uint16_t max_speed, uint16_t accel, uint16_t cells_to_max_speed, struct cell_weigth *weights_out) { 51 | uint16_t speed = init_speed; 52 | float time = distance / speed; 53 | float penalty = 0.0f; 54 | float total_time = time; 55 | weights_out[0].speed = speed; 56 | weights_out[0].time = time; 57 | weights_out[0].total_time = total_time; 58 | weights_out[0].penalty = penalty; 59 | for (uint16_t i = 1; i < cells_to_max_speed; i++) { 60 | time = time_taken_distance(distance, speed, max_speed, accel); 61 | total_time += time; 62 | speed = speed_after_time(time, speed, max_speed, accel); 63 | penalty = time_penalty(speed, init_speed, accel); 64 | weights_out[i].speed = speed; 65 | weights_out[i].time = time; 66 | weights_out[i].total_time = total_time; 67 | weights_out[i].penalty = penalty; 68 | } 69 | 70 | // for (uint16_t i = 0; i < cells_to_max_speed; i++) { 71 | // printf("CELL %d: \n", i + 1); 72 | // printf(" Speed: %d\n", weights_out[i].speed); 73 | // printf(" Time: %.4f\n", weights_out[i].time); 74 | // printf(" Total Time: %.4f\n", weights_out[i].total_time); 75 | // printf(" Penalty: %.4f\n", weights_out[i].penalty); 76 | // printf("\n"); 77 | // } 78 | } -------------------------------------------------------------------------------- /source_code/src/handwall.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static bool use_left_hand = true; 4 | static uint32_t start_ms = 0; 5 | static uint32_t time_limit = 0; 6 | 7 | void handwall_use_left_hand(void) { 8 | use_left_hand = true; 9 | } 10 | 11 | void handwall_use_right_hand(void) { 12 | use_left_hand = false; 13 | } 14 | 15 | void handwall_set_time_limit(uint32_t ms) { 16 | time_limit = ms; 17 | } 18 | 19 | void handwall_start(void) { 20 | start_ms = get_clock_ticks(); 21 | configure_kinematics(menu_run_get_speed()); 22 | clear_info_leds(); 23 | set_RGB_color(0, 0, 0); 24 | set_target_fan_speed(get_kinematics().fan_speed, 400); 25 | delay(500); 26 | move(MOVE_START); 27 | } 28 | 29 | void handwall_loop(void) { 30 | if (time_limit > 0 && get_clock_ticks() - start_ms >= time_limit) { 31 | set_race_started(false); 32 | return; 33 | } 34 | struct walls walls = get_walls(); 35 | set_RGB_color_while(255, 255, 0, 20); 36 | if ((use_left_hand && !walls.left) || (!use_left_hand && walls.right && !walls.left)) { 37 | move(MOVE_LEFT); 38 | } else if ((!use_left_hand && !walls.right) || (use_left_hand && walls.left && !walls.right)) { 39 | move(MOVE_RIGHT); 40 | } else if (!walls.front) { 41 | move(MOVE_FRONT); 42 | } else if (walls.front && walls.left && walls.right) { 43 | move(MOVE_BACK_WALL); 44 | } else { 45 | set_target_linear_speed(0); 46 | set_ideal_angular_speed(0); 47 | warning_status_led(50); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /source_code/src/leds.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static uint32_t lastTicksRainbow = 0; 4 | static uint32_t rainbowRGB[3] = {LEDS_MAX_PWM, 0, 0}; 5 | static int16_t rainbowColorDesc = 0; 6 | static int16_t rainbowColorAsc = 1; 7 | 8 | static uint32_t rgbWhileMs = 0; 9 | 10 | static uint32_t lastTicksWarning = 0; 11 | 12 | static uint32_t lastBlinkRGB = 0; 13 | static bool blinkRGBState = false; 14 | 15 | static uint32_t lastTicksWave = 0; 16 | static int8_t currentStepWave = 1; 17 | static uint8_t currentIndexWave = 0; 18 | 19 | static uint32_t lastTickSideSensors = 0; 20 | static uint32_t currentStepSideSensors = 1; 21 | static uint8_t currentIndexSideSensors = 0; 22 | 23 | static uint32_t lastTickFrontSensors = 0; 24 | static uint32_t currentStepFrontSensors = 1; 25 | static uint8_t currentIndexFrontSensors = 0; 26 | 27 | static uint32_t lastTicksLedsBlink = 0; 28 | 29 | static uint32_t lastTicksWarningBateria = 0; 30 | 31 | void set_status_led(bool state) { 32 | if (state) { 33 | gpio_set(GPIOB, GPIO14); 34 | } else { 35 | gpio_clear(GPIOB, GPIO14); 36 | } 37 | } 38 | 39 | void toggle_status_led(void) { 40 | gpio_toggle(GPIOB, GPIO14); 41 | } 42 | 43 | void warning_status_led(uint32_t ms) { 44 | if (get_clock_ticks() > lastTicksWarning + ms) { 45 | toggle_status_led(); 46 | lastTicksWarning = get_clock_ticks(); 47 | } 48 | } 49 | 50 | bool is_status_led_on(void) { 51 | return gpio_get(GPIOB, GPIO14); 52 | } 53 | 54 | void set_RGB_color(uint32_t r, uint32_t g, uint32_t b) { 55 | timer_set_oc_value(TIM1, TIM_OC3, r); 56 | timer_set_oc_value(TIM1, TIM_OC4, b); 57 | timer_set_oc_value(TIM1, TIM_OC2, g); 58 | rgbWhileMs = 0; 59 | } 60 | 61 | void set_RGB_color_while(uint32_t r, uint32_t g, uint32_t b, uint32_t ms) { 62 | set_RGB_color(r, g, b); 63 | rgbWhileMs = get_clock_ticks() + ms; 64 | } 65 | 66 | void blink_RGB_color(uint32_t r, uint32_t g, uint32_t b, uint32_t ms) { 67 | if (get_clock_ticks() > lastBlinkRGB + ms) { 68 | blinkRGBState = !blinkRGBState; 69 | if (blinkRGBState) { 70 | set_RGB_color(0, 0, 0); 71 | } else { 72 | set_RGB_color(r, g, b); 73 | } 74 | lastBlinkRGB = get_clock_ticks(); 75 | } 76 | } 77 | 78 | void check_leds_while(void) { 79 | if (rgbWhileMs > 0 && get_clock_ticks() > rgbWhileMs) { 80 | set_RGB_color(0, 0, 0); 81 | rgbWhileMs = 0; 82 | } 83 | } 84 | 85 | void set_RGB_rainbow(void) { 86 | if (get_clock_ticks() > lastTicksRainbow + 10) { 87 | lastTicksRainbow = get_clock_ticks(); 88 | rainbowRGB[rainbowColorDesc] -= 20; 89 | rainbowRGB[rainbowColorAsc] += 20; 90 | set_RGB_color(rainbowRGB[0], rainbowRGB[1], rainbowRGB[2]); 91 | if (rainbowRGB[rainbowColorDesc] <= 0 || rainbowRGB[rainbowColorAsc] >= LEDS_MAX_PWM) { 92 | rainbowRGB[rainbowColorDesc] = 0; 93 | rainbowRGB[rainbowColorAsc] = LEDS_MAX_PWM; 94 | set_RGB_color(rainbowRGB[0], rainbowRGB[1], rainbowRGB[2]); 95 | rainbowColorDesc++; 96 | if (rainbowColorDesc > 2) { 97 | rainbowColorDesc = 0; 98 | } 99 | rainbowColorAsc = rainbowColorDesc == 2 ? 0 : rainbowColorDesc + 1; 100 | } 101 | } 102 | } 103 | 104 | void set_leds_wave(int ms) { 105 | if (get_clock_ticks() > lastTicksWave + ms) { 106 | gpio_clear(GPIOC, GPIO4 | GPIO5); 107 | gpio_clear(GPIOB, GPIO0 | GPIO1 | GPIO2); 108 | gpio_clear(GPIOC, GPIO15 | GPIO14 | GPIO13); 109 | gpio_clear(GPIOB, GPIO9 | GPIO8); 110 | 111 | switch (currentIndexWave) { 112 | case 0: 113 | gpio_set(GPIOC, GPIO4); 114 | gpio_set(GPIOC, GPIO5); 115 | break; 116 | case 1: 117 | gpio_set(GPIOB, GPIO0); 118 | gpio_set(GPIOB, GPIO1); 119 | break; 120 | case 2: 121 | gpio_set(GPIOB, GPIO2); 122 | gpio_set(GPIOC, GPIO15); 123 | break; 124 | case 3: 125 | gpio_set(GPIOC, GPIO14); 126 | gpio_set(GPIOC, GPIO13); 127 | break; 128 | case 4: 129 | gpio_set(GPIOB, GPIO9); 130 | gpio_set(GPIOB, GPIO8); 131 | break; 132 | } 133 | 134 | if (currentIndexWave >= 4) { 135 | currentStepWave = -1; 136 | } else if (currentIndexWave <= 0) { 137 | currentStepWave = 1; 138 | } 139 | 140 | currentIndexWave += currentStepWave; 141 | lastTicksWave = get_clock_ticks(); 142 | } 143 | } 144 | 145 | void set_leds_side_sensors(int ms) { 146 | if (get_clock_ticks() > lastTickSideSensors + ms) { 147 | gpio_clear(GPIOC, GPIO4 | GPIO5); 148 | gpio_clear(GPIOB, GPIO0 | GPIO1 | GPIO2); 149 | gpio_clear(GPIOC, GPIO15 | GPIO14 | GPIO13); 150 | gpio_clear(GPIOB, GPIO9 | GPIO8); 151 | 152 | switch (currentIndexSideSensors) { 153 | case 0: 154 | gpio_set(GPIOB, GPIO2); 155 | gpio_set(GPIOC, GPIO15); 156 | break; 157 | case 1: 158 | gpio_set(GPIOB, GPIO1); 159 | gpio_set(GPIOC, GPIO14); 160 | break; 161 | } 162 | 163 | if (currentIndexSideSensors >= 1) { 164 | currentStepSideSensors = -1; 165 | } else if (currentIndexSideSensors <= 0) { 166 | currentStepSideSensors = 1; 167 | } 168 | 169 | currentIndexSideSensors += currentStepSideSensors; 170 | lastTickSideSensors = get_clock_ticks(); 171 | } 172 | } 173 | 174 | void set_leds_front_sensors(int ms) { 175 | if (get_clock_ticks() > lastTickFrontSensors + ms) { 176 | gpio_clear(GPIOC, GPIO4 | GPIO5); 177 | gpio_clear(GPIOB, GPIO0 | GPIO1 | GPIO2); 178 | gpio_clear(GPIOC, GPIO15 | GPIO14 | GPIO13); 179 | gpio_clear(GPIOB, GPIO9 | GPIO8); 180 | 181 | switch (currentIndexFrontSensors) { 182 | case 0: 183 | gpio_set(GPIOC, GPIO4); 184 | gpio_set(GPIOB, GPIO8); 185 | break; 186 | case 1: 187 | gpio_set(GPIOC, GPIO5); 188 | gpio_set(GPIOB, GPIO9); 189 | break; 190 | } 191 | 192 | if (currentIndexFrontSensors >= 1) { 193 | currentStepFrontSensors = -1; 194 | } else if (currentIndexFrontSensors <= 0) { 195 | currentStepFrontSensors = 1; 196 | } 197 | 198 | currentIndexFrontSensors += currentStepFrontSensors; 199 | lastTickFrontSensors = get_clock_ticks(); 200 | } 201 | } 202 | 203 | void set_leds_blink(int ms) { 204 | if (get_clock_ticks() > lastTicksLedsBlink + ms) { 205 | gpio_toggle(GPIOC, GPIO4 | GPIO5); 206 | gpio_toggle(GPIOB, GPIO0 | GPIO1 | GPIO2); 207 | gpio_toggle(GPIOC, GPIO15 | GPIO14 | GPIO13); 208 | gpio_toggle(GPIOB, GPIO9 | GPIO8); 209 | lastTicksLedsBlink = get_clock_ticks(); 210 | } 211 | } 212 | 213 | void set_leds_battery_level(float battery_level) { 214 | if (battery_level <= 10) { 215 | gpio_clear(GPIOB, GPIO0 | GPIO1 | GPIO9 | GPIO8); 216 | gpio_clear(GPIOC, GPIO4 | GPIO5 | GPIO14 | GPIO13); 217 | if (get_clock_ticks() > lastTicksWarningBateria + 50) { 218 | 219 | gpio_toggle(GPIOB, GPIO2); 220 | gpio_toggle(GPIOC, GPIO15); 221 | 222 | lastTicksWarningBateria = get_clock_ticks(); 223 | } 224 | 225 | } else if (battery_level <= 26) { 226 | gpio_clear(GPIOB, GPIO0 | GPIO1 | GPIO9 | GPIO8); 227 | gpio_clear(GPIOC, GPIO4 | GPIO5 | GPIO14 | GPIO13); 228 | 229 | gpio_set(GPIOB, GPIO2); 230 | gpio_set(GPIOC, GPIO15); 231 | 232 | } else if (battery_level <= 42) { 233 | gpio_clear(GPIOB, GPIO0 | GPIO9 | GPIO8); 234 | gpio_clear(GPIOC, GPIO4 | GPIO5 | GPIO13); 235 | 236 | gpio_set(GPIOB, GPIO1 | GPIO2); 237 | gpio_set(GPIOC, GPIO15 | GPIO14); 238 | 239 | } else if (battery_level <= 58) { 240 | gpio_clear(GPIOB, GPIO9 | GPIO8); 241 | gpio_clear(GPIOC, GPIO4 | GPIO5); 242 | 243 | gpio_set(GPIOB, GPIO1 | GPIO2 | GPIO0); 244 | gpio_set(GPIOC, GPIO15 | GPIO14 | GPIO13); 245 | 246 | } else if (battery_level <= 74) { 247 | gpio_clear(GPIOB, GPIO8); 248 | gpio_clear(GPIOC, GPIO4); 249 | 250 | gpio_set(GPIOB, GPIO1 | GPIO2 | GPIO0 | GPIO9); 251 | gpio_set(GPIOC, GPIO15 | GPIO14 | GPIO13 | GPIO5); 252 | } else if (battery_level <= 90) { 253 | gpio_set(GPIOB, GPIO1 | GPIO2 | GPIO0 | GPIO9 | GPIO8); 254 | gpio_set(GPIOC, GPIO15 | GPIO14 | GPIO13 | GPIO5 | GPIO4); 255 | } else { 256 | if (get_clock_ticks() > lastTicksWarningBateria + 50) { 257 | 258 | gpio_toggle(GPIOB, GPIO1 | GPIO2 | GPIO0 | GPIO9 | GPIO8); 259 | gpio_toggle(GPIOC, GPIO15 | GPIO14 | GPIO13 | GPIO5 | GPIO4); 260 | 261 | lastTicksWarningBateria = get_clock_ticks(); 262 | } 263 | } 264 | } 265 | 266 | void all_leds_clear(void) { 267 | set_RGB_color(0, 0, 0); 268 | set_status_led(false); 269 | 270 | gpio_clear(GPIOC, GPIO4 | GPIO5); 271 | gpio_clear(GPIOB, GPIO0 | GPIO1 | GPIO2); 272 | gpio_clear(GPIOC, GPIO15 | GPIO14 | GPIO13); 273 | gpio_clear(GPIOB, GPIO9 | GPIO8); 274 | } 275 | 276 | void set_info_led(uint8_t index, bool state) { 277 | switch (index) { 278 | case 0: 279 | if (state) { 280 | gpio_set(GPIOC, GPIO4); 281 | } else { 282 | gpio_clear(GPIOC, GPIO4); 283 | } 284 | break; 285 | case 1: 286 | if (state) { 287 | gpio_set(GPIOC, GPIO5); 288 | } else { 289 | gpio_clear(GPIOC, GPIO5); 290 | } 291 | break; 292 | case 2: 293 | if (state) { 294 | gpio_set(GPIOB, GPIO0); 295 | } else { 296 | gpio_clear(GPIOB, GPIO0); 297 | } 298 | break; 299 | case 3: 300 | if (state) { 301 | gpio_set(GPIOB, GPIO1); 302 | } else { 303 | gpio_clear(GPIOB, GPIO1); 304 | } 305 | break; 306 | case 4: 307 | if (state) { 308 | gpio_set(GPIOB, GPIO2); 309 | } else { 310 | gpio_clear(GPIOB, GPIO2); 311 | } 312 | break; 313 | case 5: 314 | if (state) { 315 | gpio_set(GPIOC, GPIO15); 316 | } else { 317 | gpio_clear(GPIOC, GPIO15); 318 | } 319 | break; 320 | case 6: 321 | if (state) { 322 | gpio_set(GPIOC, GPIO14); 323 | } else { 324 | gpio_clear(GPIOC, GPIO14); 325 | } 326 | break; 327 | case 7: 328 | if (state) { 329 | gpio_set(GPIOC, GPIO13); 330 | } else { 331 | gpio_clear(GPIOC, GPIO13); 332 | } 333 | break; 334 | case 8: 335 | if (state) { 336 | gpio_set(GPIOB, GPIO9); 337 | } else { 338 | gpio_clear(GPIOB, GPIO9); 339 | } 340 | break; 341 | case 9: 342 | if (state) { 343 | gpio_set(GPIOB, GPIO8); 344 | } else { 345 | gpio_clear(GPIOB, GPIO8); 346 | } 347 | break; 348 | } 349 | } 350 | 351 | void set_info_leds(void) { 352 | gpio_set(GPIOC, GPIO4 | GPIO5); 353 | gpio_set(GPIOB, GPIO0 | GPIO1 | GPIO2); 354 | gpio_set(GPIOC, GPIO15 | GPIO14 | GPIO13); 355 | gpio_set(GPIOB, GPIO9 | GPIO8); 356 | } 357 | 358 | void clear_info_leds(void) { 359 | gpio_clear(GPIOC, GPIO4 | GPIO5); 360 | gpio_clear(GPIOB, GPIO0 | GPIO1 | GPIO2); 361 | gpio_clear(GPIOC, GPIO15 | GPIO14 | GPIO13); 362 | gpio_clear(GPIOB, GPIO9 | GPIO8); 363 | } -------------------------------------------------------------------------------- /source_code/src/lsm6dsr.c: -------------------------------------------------------------------------------- 1 | #include "lsm6dsr.h" 2 | 3 | #define MPU_READ 0x80 4 | #define MPU_WHOAMI 0x0F 5 | 6 | #define CTRL1_XL 0x10 7 | #define CTRL2_G 0x11 8 | #define CTRL3_C 0x12 9 | #define CTRL6_C 0x15 10 | #define CTRL7_G 0x16 11 | #define CTRL8_XL 0x17 12 | #define CTRL9_XL 0x18 13 | 14 | #define OUTZ_L_G 0x26 15 | #define OUTZ_H_G 0x27 16 | 17 | #define GYRO_SENSITIVITY_2000DPS 70.0f 18 | #define GYRO_SENSITIVITY_4000DPS 140.0f 19 | #define MPU_DPS_TO_RADPS (PI / 180) 20 | 21 | static stmdev_ctx_t dev_ctx; 22 | 23 | static volatile float deg_integ; 24 | static volatile float gyro_z_raw; 25 | static bool mpu_updating = false; 26 | static float offset_z = 0; 27 | 28 | static uint8_t lsm6dsr_read_register(uint8_t address) { 29 | uint8_t reading; 30 | 31 | gpio_clear(GPIOA, GPIO15); 32 | spi_send(SPI3, (MPU_READ | address)); 33 | spi_read(SPI3); 34 | spi_send(SPI3, 0x00); 35 | reading = spi_read(SPI3); 36 | gpio_set(GPIOA, GPIO15); 37 | 38 | return reading; 39 | } 40 | 41 | static void lsm6dsr_write_register(uint8_t address, uint8_t value) { 42 | gpio_clear(GPIOA, GPIO15); 43 | spi_send(SPI3, address); 44 | spi_read(SPI3); 45 | spi_send(SPI3, value); 46 | spi_read(SPI3); 47 | gpio_set(GPIOA, GPIO15); 48 | } 49 | 50 | /* #region Librería */ 51 | 52 | static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len) { 53 | handle = handle; 54 | len = len; 55 | lsm6dsr_write_register(reg, *bufp); 56 | return 0; 57 | } 58 | static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len) { 59 | handle = handle; 60 | len = len; 61 | *bufp = lsm6dsr_read_register(reg); 62 | return 0; 63 | } 64 | static void platform_delay(uint32_t ms) { 65 | delay(ms); 66 | } 67 | 68 | /* #endregion */ 69 | 70 | static int16_t lsm6dsr_read_gyro_z_raw(void) { 71 | uint8_t zl = lsm6dsr_read_register(OUTZ_L_G); 72 | uint8_t zh = lsm6dsr_read_register(OUTZ_H_G); 73 | return ((zh << 8) | zl); 74 | } 75 | 76 | void lsm6dsr_init(void) { 77 | 78 | /* Initialize mems driver interface */ 79 | dev_ctx.write_reg = platform_write; 80 | dev_ctx.read_reg = platform_read; 81 | dev_ctx.mdelay = platform_delay; 82 | 83 | /* Restore default configuration */ 84 | lsm6dsr_reset_set(&dev_ctx, PROPERTY_ENABLE); 85 | uint8_t rst; 86 | do { 87 | lsm6dsr_reset_get(&dev_ctx, &rst); 88 | } while (rst); 89 | 90 | /* Disable I3C interface */ 91 | lsm6dsr_i3c_disable_set(&dev_ctx, LSM6DSR_I3C_DISABLE); 92 | /* Enable Block Data Update */ 93 | lsm6dsr_block_data_update_set(&dev_ctx, PROPERTY_DISABLE); 94 | 95 | /* Set Output Data Rate */ 96 | lsm6dsr_xl_data_rate_set(&dev_ctx, LSM6DSR_XL_ODR_12Hz5); 97 | lsm6dsr_gy_data_rate_set(&dev_ctx, LSM6DSR_GY_ODR_1666Hz); 98 | /* Set full scale */ 99 | lsm6dsr_xl_full_scale_set(&dev_ctx, LSM6DSR_2g); 100 | lsm6dsr_gy_full_scale_set(&dev_ctx, LSM6DSR_4000dps); 101 | /* Configure filtering chain(No aux interface) 102 | * Accelerometer - LPF1 + LPF2 path 103 | */ 104 | lsm6dsr_xl_hp_path_on_out_set(&dev_ctx, LSM6DSR_LP_ODR_DIV_100); 105 | lsm6dsr_xl_filter_lp2_set(&dev_ctx, PROPERTY_ENABLE); 106 | 107 | delay(1000); 108 | } 109 | 110 | uint8_t lsm6dsr_who_am_i(void) { 111 | return lsm6dsr_read_register(MPU_WHOAMI); 112 | // uint8_t whoamI = 0; 113 | // lsm6dsr_device_id_get(&dev_ctx, &whoamI); 114 | // return whoamI; 115 | } 116 | 117 | void lsm6dsr_gyro_z_calibration(void) { 118 | mpu_updating = false; 119 | int32_t sum_z = 0; 120 | 121 | offset_z = 0; 122 | for (int i = 0; i < 1000; i++) { 123 | sum_z += lsm6dsr_read_gyro_z_raw(); 124 | delay(10); 125 | set_info_leds(); 126 | } 127 | clear_info_leds(); 128 | 129 | offset_z = sum_z / 1000.0f; 130 | printf("Offset Z: %.4f\n", offset_z); 131 | 132 | int16_t eeprom_data[2] = {offset_z >= 0 ? 1 : 0, (int16_t)(abs(offset_z * 10000))}; 133 | eeprom_set_data(DATA_INDEX_GYRO_Z, eeprom_data, 2); 134 | 135 | delay(100); 136 | mpu_updating = true; 137 | } 138 | 139 | void lsm6dsr_load_eeprom(void) { 140 | int16_t *eeprom_data = eeprom_get_data(); 141 | offset_z = eeprom_data[1] / 10000.0f; 142 | if (eeprom_data[0] == 0) { 143 | offset_z = -offset_z; 144 | } 145 | printf("Offset Z: %.4f\n", offset_z); 146 | mpu_updating = true; 147 | } 148 | 149 | void lsm6dsr_update(void) { 150 | if (mpu_updating) { 151 | float new_gyro_z_raw = lsm6dsr_read_gyro_z_raw(); 152 | // gyro_z_raw = 0.5 * new_gyro_z_raw + (1 - 0.5) * gyro_z_raw; 153 | #ifdef ZOROBOT3_C 154 | gyro_z_raw = new_gyro_z_raw - 4.2290f; 155 | #else 156 | gyro_z_raw = new_gyro_z_raw - offset_z; 157 | #endif 158 | deg_integ = deg_integ - lsm6dsr_get_gyro_z_dps() / SYSTICK_FREQUENCY_HZ; 159 | } 160 | } 161 | 162 | float lsm6dsr_get_gyro_z_raw(void) { 163 | return gyro_z_raw; 164 | } 165 | 166 | float lsm6dsr_get_gyro_z_radps(void) { 167 | return (gyro_z_raw * GYRO_SENSITIVITY_4000DPS / 1000 * MPU_DPS_TO_RADPS); 168 | } 169 | 170 | float lsm6dsr_get_gyro_z_dps(void) { 171 | return (gyro_z_raw * GYRO_SENSITIVITY_4000DPS) / 1000; 172 | } 173 | 174 | float lsm6dsr_get_gyro_z_degrees(void) { 175 | return deg_integ; 176 | } 177 | void lsm6dsr_set_gyro_z_degrees(float deg) { 178 | deg_integ = deg; 179 | } -------------------------------------------------------------------------------- /source_code/src/macroarray.c: -------------------------------------------------------------------------------- 1 | #include "macroarray.h" 2 | 3 | static uint32_t macroarray_last_update_ms = 0; 4 | static uint8_t macroarray_size = 0; 5 | static uint16_t macroarray_float_bits = 0; 6 | static uint16_t macroarray_start = 0; 7 | static uint16_t macroarray_end = 0; 8 | static int16_t macroarray[MACROARRAY_LENGTH]; 9 | static char *macroarray_labels[20]; 10 | 11 | void macroarray_store(uint8_t ms, uint16_t float_bits, char **labels, uint8_t size, ...) { 12 | if (get_clock_ticks() - macroarray_last_update_ms < ms || size > 20) { 13 | return; 14 | } else { 15 | macroarray_last_update_ms = get_clock_ticks(); 16 | } 17 | 18 | if (macroarray_size != size) { 19 | macroarray_start = 0; 20 | macroarray_end = 0; 21 | macroarray_size = size; 22 | macroarray_float_bits = float_bits; 23 | for (uint8_t i = 0; i < size; i++) { 24 | macroarray_labels[i] = labels[i]; 25 | } 26 | } 27 | 28 | if (macroarray_end + size > MACROARRAY_LENGTH) { 29 | macroarray_start = macroarray_end - size; 30 | macroarray_end = 0; 31 | } 32 | 33 | va_list valist; 34 | va_start(valist, size); 35 | for (uint8_t i = 0; i < size; i++) { 36 | macroarray[macroarray_end++] = va_arg(valist, int); 37 | } 38 | va_end(valist); 39 | } 40 | 41 | void macroarray_print(void) { 42 | if (macroarray_size == 0) { 43 | return; 44 | } 45 | uint16_t i = macroarray_start; 46 | uint8_t col = 1; 47 | do { 48 | if (macroarray_float_bits & (1 << (macroarray_size - col))) { 49 | printf(">%s:%.2f", macroarray_labels[col - 1], macroarray[i] / 100.0); 50 | } else { 51 | printf(">%s:%d", macroarray_labels[col - 1], macroarray[i]); 52 | } 53 | printf("\n"); 54 | if (col == macroarray_size) { 55 | col = 1; 56 | i++; 57 | if (i + macroarray_size > MACROARRAY_LENGTH) { 58 | i = 0; 59 | } 60 | } else { 61 | col++; 62 | i++; 63 | } 64 | } while (i != macroarray_end); 65 | } 66 | -------------------------------------------------------------------------------- /source_code/src/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | // #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | void sys_tick_handler(void) { 22 | clock_tick(); 23 | update_encoder_readings(); 24 | update_sensors_magics(); 25 | update_battery_voltage(); 26 | check_leds_while(); 27 | check_buttons(); 28 | lsm6dsr_update(); 29 | } 30 | 31 | int main(void) { 32 | setup(); 33 | eeprom_load(); 34 | show_battery_level(); 35 | 36 | printf("BA: %4d CI: %4d CD: %4d BO: %4d\n", get_aux_raw(AUX_BATTERY_ID), get_aux_raw(AUX_CURRENT_LEFT_ID), get_aux_raw(AUX_CURRENT_RIGHT_ID), get_aux_raw(AUX_MENU_BTN_ID)); 37 | 38 | while (1) { 39 | if (!is_race_started()) { 40 | menu_handler(); 41 | if (!get_sensors_enabled() && menu_run_can_start()) { 42 | set_sensors_enabled(menu_run_can_start()); 43 | delay(200); 44 | } else { 45 | set_sensors_enabled(menu_run_can_start()); 46 | } 47 | if (menu_run_can_start()) { 48 | int8_t sensor_started = check_start_run(); 49 | if (is_race_started()) { 50 | switch (menu_run_get_explore_algorithm()) { 51 | case EXPLORE_HANDWALL: 52 | switch (sensor_started) { 53 | case SENSOR_FRONT_LEFT_WALL_ID: 54 | handwall_use_left_hand(); 55 | handwall_start(); 56 | break; 57 | case SENSOR_FRONT_RIGHT_WALL_ID: 58 | handwall_use_right_hand(); 59 | handwall_start(); 60 | break; 61 | } 62 | break; 63 | case EXPLORE_FLOODFILL: 64 | switch (sensor_started) { 65 | case SENSOR_FRONT_LEFT_WALL_ID: 66 | floodfill_start_run(); 67 | break; 68 | case SENSOR_FRONT_RIGHT_WALL_ID: 69 | floodfill_start_explore(); 70 | break; 71 | } 72 | break; 73 | case EXPLORE_TIME_TRIAL: 74 | timetrial_start(); 75 | break; 76 | default: 77 | set_race_started(false); 78 | break; 79 | } 80 | } 81 | } 82 | } else { 83 | switch (menu_run_get_explore_algorithm()) { 84 | case EXPLORE_HANDWALL: 85 | handwall_loop(); 86 | break; 87 | case EXPLORE_FLOODFILL: 88 | floodfill_loop(); 89 | break; 90 | case EXPLORE_TIME_TRIAL: 91 | timetrial_loop(); 92 | break; 93 | default: 94 | set_race_started(false); 95 | break; 96 | } 97 | } 98 | } 99 | 100 | // ZONA DEBUG TEMPORAL 101 | 102 | while (1) { 103 | // LED WARNING 104 | // warning_status_led(125); 105 | 106 | // LEDS MENU 107 | // set_leds_wave(125); 108 | 109 | // LED RGB 110 | // set_RGB_color(20, 0, 0); 111 | // delay(250); 112 | // set_RGB_color(0, 20, 0); 113 | // delay(250); 114 | // set_RGB_color(0, 0, 20); 115 | // delay(250); 116 | // set_RGB_color(LEDS_MAX_PWM, LEDS_MAX_PWM, LEDS_MAX_PWM); 117 | // set_RGB_rainbow(); 118 | 119 | // MPU 120 | // printf("MPU: 0x%02X\t", lsm6dsr_who_am_i()); 121 | // printf("Z(raw): %6d Z(radps): %6.4f Z(dps): %6.4f Z(deg): %6.4f\t", lsm6dsr_get_gyro_z_raw(), lsm6dsr_get_gyro_z_radps(), lsm6dsr_get_gyro_z_dps(), lsm6dsr_get_gyro_z_degrees()); 122 | // printf("Z(raw): %6d\t", lsm6dsr_get_gyro_z_raw()); 123 | // delay(500); 124 | 125 | // VENTILADOR 126 | // set_fan_speed(50); 127 | 128 | // MOTORES 129 | // gpio_set(GPIOB, GPIO15); 130 | // set_motors_speed(20, 20); 131 | 132 | // SENSORES 133 | // printf("S1: %4d S2: %4d S3: %4d S4: %4d\t", get_sensor_raw(SENSOR_FRONT_LEFT_WALL_ID, true), get_sensor_raw(SENSOR_FRONT_RIGHT_WALL_ID, true), get_sensor_raw(SENSOR_SIDE_LEFT_WALL_ID, true), get_sensor_raw(SENSOR_SIDE_RIGHT_WALL_ID, true)); 134 | 135 | // AUX ANALÓGICOS 136 | // printf("BA: %4d CI: %4d CD: %4d BO: %4d\t", get_aux_raw(AUX_BATTERY_ID), get_aux_raw(AUX_CURRENT_LEFT_ID), get_aux_raw(AUX_CURRENT_RIGHT_ID), get_aux_raw(AUX_MENU_BTN_ID)); 137 | // printf("BA: %.2f\n", get_battery_voltage()); 138 | 139 | // ENCODERS 140 | // printf("L: %ld R: %ld\t", get_encoder_left_millimeters(), get_encoder_right_millimeters()); 141 | 142 | // printf("\n"); 143 | // delay(50); 144 | } 145 | 146 | return 0; 147 | } -------------------------------------------------------------------------------- /source_code/src/maze.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define MAZE_ROWS_HOME 6 4 | #define MAZE_COLUMNS_HOME 6 5 | 6 | #define MAZE_ROWS_COMPETITION 16 7 | #define MAZE_COLUMNS_COMPETITION 16 8 | 9 | static struct cells_stack goals; 10 | 11 | static void add_goal(uint8_t x, uint8_t y) { 12 | goals.stack[goals.size++] = (x - 1) + (y - 1) * maze_get_columns(); 13 | } 14 | 15 | uint16_t maze_get_rows() { 16 | switch (menu_run_get_maze_type()) { 17 | case MAZE_HOME: 18 | return MAZE_ROWS_HOME; 19 | break; 20 | case MAZE_COMPETITION: 21 | return MAZE_ROWS_COMPETITION; 22 | break; 23 | } 24 | return 0; 25 | } 26 | 27 | uint16_t maze_get_columns() { 28 | switch (menu_run_get_maze_type()) { 29 | case MAZE_HOME: 30 | return MAZE_COLUMNS_HOME; 31 | break; 32 | case MAZE_COMPETITION: 33 | return MAZE_COLUMNS_COMPETITION; 34 | break; 35 | } 36 | return 0; 37 | } 38 | 39 | uint16_t maze_get_cells() { 40 | return maze_get_rows() * maze_get_columns(); 41 | } 42 | 43 | struct cells_stack *maze_get_goals(void) { 44 | goals.size = 0; 45 | switch (menu_run_get_maze_type()) { 46 | case MAZE_HOME: 47 | add_goal(6, 6); 48 | add_goal(6, 5); 49 | add_goal(5, 6); 50 | add_goal(5, 5); 51 | break; 52 | case MAZE_COMPETITION: 53 | add_goal(8, 8); 54 | add_goal(8, 9); 55 | add_goal(9, 8); 56 | add_goal(9, 9); 57 | break; 58 | } 59 | 60 | return &goals; 61 | } -------------------------------------------------------------------------------- /source_code/src/menu.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define MENU_RUN 0 4 | #define MENU_CONFIG 1 5 | #define MENU_TYPES 2 6 | 7 | static uint8_t current_menu = MENU_RUN; 8 | 9 | static bool handle_current_menu(void) { 10 | switch (current_menu) { 11 | case MENU_RUN: 12 | return menu_run_handler(); 13 | case MENU_CONFIG: 14 | return menu_config_handler(); 15 | } 16 | return false; 17 | } 18 | 19 | /** 20 | * @brief Controla que menú está activo [MENU_RUN, MENU_CONFIG] 21 | * 22 | */ 23 | void menu_handler(void) { 24 | bool menu_mode_btn_long_pressed = handle_current_menu(); 25 | if (menu_mode_btn_long_pressed) { 26 | menu_config_reset_mode(); 27 | current_menu = (current_menu + 1) % MENU_TYPES; 28 | } 29 | } 30 | 31 | void menu_reset(void) { 32 | current_menu = MENU_RUN; 33 | menu_run_reset(); 34 | } 35 | 36 | void menu_rc5_mode_change(void) { 37 | switch (current_menu) { 38 | case MENU_RUN: 39 | menu_run_mode_change(); 40 | break; 41 | } 42 | } 43 | 44 | void menu_rc5_up(void) { 45 | switch (current_menu) { 46 | case MENU_RUN: 47 | menu_run_up(); 48 | break; 49 | } 50 | } 51 | 52 | void menu_rc5_down(void) { 53 | switch (current_menu) { 54 | case MENU_RUN: 55 | menu_run_down(); 56 | break; 57 | } 58 | } -------------------------------------------------------------------------------- /source_code/src/menu_configs.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define MODE_CALIBRATION 0 4 | #define MODE_DEBUG 1 5 | uint8_t modeConfig = MODE_CALIBRATION; 6 | 7 | #define NUM_MODES 2 8 | 9 | int8_t valueConfig[NUM_MODES] = {0, 0}; 10 | #define NUM_VALUES_CALIBRATION 5 11 | #define NUM_VALUES_DEBUG 10 12 | 13 | /** 14 | * @brief Indicar el tipo de menú que está actualmente activo mediante el led de estado 15 | * 16 | */ 17 | static void handle_menu_config_mode(void) { 18 | switch (modeConfig) { 19 | case MODE_CALIBRATION: 20 | warning_status_led(125); 21 | break; 22 | case MODE_DEBUG: 23 | set_status_led(true); 24 | break; 25 | } 26 | } 27 | 28 | static void handle_menu_config_value(void) { 29 | switch (modeConfig) { 30 | case MODE_CALIBRATION: 31 | set_RGB_color(0, 0, 0); 32 | switch (valueConfig[modeConfig]) { 33 | case CALIBRATE_NONE: 34 | clear_info_leds(); 35 | break; 36 | case CALIBRATE_GYRO_Z: 37 | set_leds_wave(120); 38 | break; 39 | case CALIBRATE_SIDE_SENSORS_OFFSET: 40 | set_leds_side_sensors(120); 41 | break; 42 | case CALIBRATE_FRONT_SENSORS: 43 | set_leds_front_sensors(120); 44 | break; 45 | case CALIBRATE_STORE_EEPROM: 46 | set_leds_blink(250); 47 | break; 48 | } 49 | calibrate_from_config(valueConfig[modeConfig]); 50 | break; 51 | case MODE_DEBUG: 52 | for (uint8_t i = 1; i <= NUM_VALUES_DEBUG; i++) { 53 | set_info_led(i - 1, i == valueConfig[modeConfig]); 54 | } 55 | debug_from_config(valueConfig[modeConfig]); 56 | break; 57 | } 58 | } 59 | 60 | bool menu_config_handler(void) { 61 | handle_menu_config_mode(); 62 | handle_menu_config_value(); 63 | 64 | // Comprueba cambios del modo de configuración 65 | if (valueConfig[modeConfig] == 0) { 66 | if (get_menu_mode_btn()) { 67 | uint32_t ms = get_clock_ticks(); 68 | while (get_menu_mode_btn()) { 69 | if (get_clock_ticks() - ms >= 200) { 70 | warning_status_led(50); 71 | } else { 72 | handle_menu_config_mode(); 73 | } 74 | handle_menu_config_value(); 75 | }; 76 | if (get_clock_ticks() - ms >= 200) { 77 | return true; 78 | } else { 79 | modeConfig = (modeConfig + 1) % NUM_MODES; 80 | delay(50); 81 | } 82 | } 83 | } 84 | 85 | // Comprueba aumento de valor de configuración 86 | if (get_menu_up_btn() && !(modeConfig == MODE_DEBUG && is_debug_enabled())) { 87 | valueConfig[modeConfig]++; 88 | switch (modeConfig) { 89 | case MODE_CALIBRATION: 90 | if (valueConfig[modeConfig] > NUM_VALUES_CALIBRATION) { 91 | valueConfig[modeConfig] = 0; 92 | } 93 | break; 94 | case MODE_DEBUG: 95 | if (valueConfig[modeConfig] > NUM_VALUES_DEBUG) { 96 | valueConfig[modeConfig] = 0; 97 | } 98 | break; 99 | } 100 | clear_info_leds(); 101 | while (get_menu_up_btn()) { 102 | handle_menu_config_value(); 103 | handle_menu_config_mode(); 104 | }; 105 | delay(50); 106 | } 107 | 108 | // Comprueba descenso de valor de configuración 109 | if (get_menu_down_btn() && !(modeConfig == MODE_DEBUG && is_debug_enabled())) { 110 | if (valueConfig[modeConfig] > 0) { 111 | valueConfig[modeConfig]--; 112 | } 113 | 114 | clear_info_leds(); 115 | while (get_menu_down_btn()) { 116 | handle_menu_config_value(); 117 | handle_menu_config_mode(); 118 | }; 119 | delay(50); 120 | } 121 | return false; 122 | } 123 | 124 | void menu_config_reset_values(void) { 125 | valueConfig[MODE_CALIBRATION] = 0; 126 | valueConfig[MODE_DEBUG] = 0; 127 | } 128 | 129 | void menu_config_reset_mode(void) { 130 | modeConfig = MODE_CALIBRATION; 131 | } -------------------------------------------------------------------------------- /source_code/src/menu_run.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define MODE_SPEED 0 4 | #define MODE_RACE 1 5 | #define MODE_MAZE_TYPE 2 6 | #define MODE_SOLVE_STRATEGY 3 7 | #define MODE_EXPLORE_ALGORITHM 4 8 | uint8_t modeRun = MODE_SPEED; 9 | 10 | #define MODE_SPEED_VALUES 6 11 | #define MODE_RACE_VALUES 2 12 | #define MODE_MAZE_TYPE_VALUES 2 13 | #define MODE_EXPLORE_ALGORITHM_VALUES 3 14 | #define MODE_SOLVE_STRATEGY_VALUES 2 15 | 16 | int16_t valueRun[MENU_RUN_NUM_MODES] = {0, 0, 0, 0, 1}; 17 | 18 | uint32_t lastBlinkMs = 0; 19 | bool blinkState = false; 20 | 21 | static void handle_menu_run_values(void) { 22 | if (get_clock_ticks() - lastBlinkMs >= 125) { 23 | lastBlinkMs = get_clock_ticks(); 24 | blinkState = !blinkState; 25 | } 26 | if (modeRun == MODE_SPEED) { 27 | set_RGB_color(0, 0, 0); 28 | 29 | if (valueRun[MODE_SPEED] == MODE_SPEED_VALUES - 1) { 30 | set_info_led(0, blinkState); 31 | set_info_led(1, blinkState); 32 | set_info_led(2, blinkState); 33 | set_info_led(3, blinkState); 34 | set_info_led(4, blinkState); 35 | } else { 36 | for (uint8_t i = 0; i < MODE_SPEED_VALUES - 1; i++) { 37 | if ((valueRun[MODE_SPEED] == i && blinkState)) { 38 | set_info_led(i, true); 39 | } else { 40 | set_info_led(i, false); 41 | } 42 | } 43 | } 44 | } else { 45 | if (valueRun[MODE_SPEED] == MODE_SPEED_VALUES - 1) { 46 | set_info_led(0, true); 47 | set_info_led(1, true); 48 | set_info_led(2, true); 49 | set_info_led(3, true); 50 | set_info_led(4, true); 51 | } else { 52 | for (uint8_t i = 0; i < MODE_SPEED_VALUES - 1; i++) { 53 | set_info_led(i, i == valueRun[MODE_SPEED]); 54 | } 55 | } 56 | } 57 | 58 | if (modeRun == MODE_RACE) { 59 | if (valueRun[modeRun] == 1) { 60 | set_RGB_color(50, 0, 0); 61 | } else { 62 | if (blinkState) { 63 | set_RGB_color(50, 0, 0); 64 | } else { 65 | set_RGB_color(0, 0, 0); 66 | } 67 | } 68 | } 69 | 70 | if (modeRun == MODE_MAZE_TYPE) { 71 | if (valueRun[modeRun] == 1) { 72 | set_RGB_color(0, 50, 0); 73 | } else { 74 | set_RGB_color(0, 0, 0); 75 | } 76 | set_info_led(7, blinkState); 77 | } else { 78 | set_info_led(7, valueRun[MODE_MAZE_TYPE] == 1); 79 | } 80 | 81 | if (modeRun == MODE_SOLVE_STRATEGY) { 82 | if (valueRun[modeRun] == 1) { 83 | set_RGB_color(0, 50, 0); 84 | } else { 85 | set_RGB_color(0, 0, 0); 86 | } 87 | set_info_led(8, blinkState); 88 | } else { 89 | set_info_led(8, valueRun[MODE_SOLVE_STRATEGY] == 1); 90 | } 91 | 92 | if (modeRun == MODE_EXPLORE_ALGORITHM) { 93 | switch (valueRun[modeRun]) { 94 | case EXPLORE_HANDWALL: 95 | set_RGB_color(0, 50, 0); 96 | break; 97 | case EXPLORE_FLOODFILL: 98 | set_RGB_color(0, 0, 50); 99 | break; 100 | case EXPLORE_TIME_TRIAL: 101 | set_RGB_color(50, 0, 50); 102 | break; 103 | default: 104 | set_RGB_color(0, 0, 0); 105 | break; 106 | } 107 | set_info_led(9, blinkState); 108 | } else { 109 | set_info_led(9, valueRun[MODE_EXPLORE_ALGORITHM] == 1); 110 | } 111 | } 112 | 113 | static void handle_menu_run_btn(void) { 114 | if (get_menu_up_btn()) { 115 | while (get_menu_up_btn()) { 116 | handle_menu_run_values(); 117 | } 118 | menu_run_up(); 119 | } 120 | 121 | if (get_menu_down_btn()) { 122 | while (get_menu_down_btn()) { 123 | handle_menu_run_values(); 124 | } 125 | menu_run_down(); 126 | } 127 | } 128 | 129 | bool menu_run_handler(void) { 130 | set_status_led(false); 131 | if (get_menu_mode_btn()) { 132 | uint32_t ms = get_clock_ticks(); 133 | while (get_menu_mode_btn()) { 134 | if (get_clock_ticks() - ms >= 200) { 135 | warning_status_led(50); 136 | } 137 | } 138 | if (get_clock_ticks() - ms >= 200) { 139 | return true; 140 | } else { 141 | menu_run_mode_change(); 142 | } 143 | } 144 | handle_menu_run_btn(); 145 | handle_menu_run_values(); 146 | return false; 147 | } 148 | 149 | void menu_run_reset(void) { 150 | modeRun = MODE_SPEED; 151 | valueRun[MODE_RACE] = 0; 152 | } 153 | 154 | void menu_run_load_values(void) { 155 | int16_t *data = eeprom_get_data(); 156 | for (uint16_t i = DATA_INDEX_MENU_RUN; i < (DATA_INDEX_MENU_RUN + MENU_RUN_NUM_MODES); i++) { 157 | valueRun[i - DATA_INDEX_MENU_RUN] = data[i]; 158 | } 159 | valueRun[MODE_RACE] = 0; 160 | } 161 | 162 | void menu_run_mode_change() { 163 | modeRun = (modeRun + 1) % MENU_RUN_NUM_MODES; 164 | } 165 | 166 | void menu_run_up() { 167 | uint8_t mode_values = 0; 168 | switch (modeRun) { 169 | case MODE_SPEED: 170 | mode_values = MODE_SPEED_VALUES; 171 | break; 172 | case MODE_MAZE_TYPE: 173 | mode_values = MODE_MAZE_TYPE_VALUES; 174 | break; 175 | case MODE_EXPLORE_ALGORITHM: 176 | mode_values = MODE_EXPLORE_ALGORITHM_VALUES; 177 | break; 178 | case MODE_RACE: 179 | mode_values = MODE_RACE_VALUES; 180 | break; 181 | case MODE_SOLVE_STRATEGY: 182 | mode_values = MODE_SOLVE_STRATEGY_VALUES; 183 | break; 184 | } 185 | valueRun[modeRun] = (valueRun[modeRun] + 1) % mode_values; 186 | if (modeRun == MODE_RACE && valueRun[modeRun] == 1) { 187 | set_RGB_color(50, 0, 0); 188 | eeprom_set_data(DATA_INDEX_MENU_RUN, valueRun, MENU_RUN_NUM_MODES); 189 | eeprom_save(); 190 | } 191 | } 192 | 193 | void menu_run_down() { 194 | if (valueRun[modeRun] > 0) { 195 | valueRun[modeRun]--; 196 | } 197 | } 198 | 199 | bool menu_run_can_start(void) { 200 | return modeRun == MODE_RACE && valueRun[MODE_RACE] > 0; 201 | } 202 | 203 | int16_t *get_menu_run_values(void) { 204 | return valueRun; 205 | } 206 | 207 | enum speed_strategy menu_run_get_speed(void) { 208 | return valueRun[MODE_SPEED]; 209 | } 210 | 211 | enum maze_type menu_run_get_maze_type(void) { 212 | return valueRun[MODE_MAZE_TYPE]; 213 | } 214 | 215 | enum solve_strategy menu_run_get_solve_strategy(void) { 216 | return valueRun[MODE_SOLVE_STRATEGY]; 217 | } 218 | 219 | enum explore_algorithm menu_run_get_explore_algorithm(void) { 220 | return valueRun[MODE_EXPLORE_ALGORITHM]; 221 | } -------------------------------------------------------------------------------- /source_code/src/motors.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static bool motors_saturated = false; 4 | static uint32_t motors_saturated_ms = 0; 5 | static uint16_t left_motor_saturation_count = 0; 6 | static uint16_t right_motor_saturation_count = 0; 7 | static uint16_t angular_speed_saturation_count = 0; 8 | 9 | static bool check_motors_saturated_enabled = true; 10 | 11 | static void check_motors_saturated(void) { 12 | if (check_motors_saturated_enabled && (left_motor_saturation_count > MAX_MOTOR_SATURATION_COUNT || right_motor_saturation_count > MAX_MOTOR_SATURATION_COUNT || angular_speed_saturation_count > MAX_MOTOR_ANGULAR_SATURATION_COUNT)) { 13 | if (!motors_saturated) { 14 | motors_saturated_ms = get_clock_ticks(); 15 | } 16 | motors_saturated = true; 17 | } else { 18 | motors_saturated = false; 19 | } 20 | } 21 | 22 | void set_check_motors_saturated_enabled(bool enabled) { 23 | check_motors_saturated_enabled = enabled; 24 | } 25 | 26 | void set_motors_enable(bool enabled) { 27 | if (enabled) { 28 | gpio_set(GPIOB, GPIO15); 29 | } else { 30 | gpio_clear(GPIOB, GPIO15); 31 | } 32 | } 33 | 34 | void set_motors_speed(float velI, float velD) { 35 | float ocI = 0; 36 | float ocD = 0; 37 | 38 | ocI = map(abs(velI), 0, 1000, 0, MOTORES_MAX_PWM); 39 | if (velI > 0) { 40 | timer_set_oc_value(TIM8, TIM_OC4, MOTORES_MAX_PWM - (uint32_t)ocI); 41 | timer_set_oc_value(TIM8, TIM_OC3, MOTORES_MAX_PWM); 42 | } else { 43 | timer_set_oc_value(TIM8, TIM_OC3, MOTORES_MAX_PWM - (uint32_t)ocI); 44 | timer_set_oc_value(TIM8, TIM_OC4, MOTORES_MAX_PWM); 45 | } 46 | 47 | ocD = map(abs(velD), 0, 1000, 0, MOTORES_MAX_PWM); 48 | if (velD > 0) { 49 | timer_set_oc_value(TIM8, TIM_OC2, MOTORES_MAX_PWM - (uint32_t)ocD); 50 | timer_set_oc_value(TIM8, TIM_OC1, MOTORES_MAX_PWM); 51 | } else { 52 | timer_set_oc_value(TIM8, TIM_OC1, MOTORES_MAX_PWM - (uint32_t)ocD); 53 | timer_set_oc_value(TIM8, TIM_OC2, MOTORES_MAX_PWM); 54 | } 55 | // printf("%ld - %ld\n", (uint32_t)ocI, (uint32_t)ocD); 56 | } 57 | 58 | void set_motors_brake(void) { 59 | timer_set_oc_value(TIM8, TIM_OC1, MOTORES_MAX_PWM); 60 | timer_set_oc_value(TIM8, TIM_OC2, MOTORES_MAX_PWM); 61 | timer_set_oc_value(TIM8, TIM_OC3, MOTORES_MAX_PWM); 62 | timer_set_oc_value(TIM8, TIM_OC4, MOTORES_MAX_PWM); 63 | } 64 | 65 | void set_motors_pwm(int32_t pwm_left, int32_t pwm_right) { 66 | if (pwm_left > MOTORES_MAX_PWM) { 67 | pwm_left = MOTORES_MAX_PWM; 68 | } else if (pwm_left < -MOTORES_MAX_PWM) { 69 | pwm_left = -MOTORES_MAX_PWM; 70 | } 71 | 72 | if (abs(pwm_left) >= MOTORES_SATURATION_PWM) { 73 | left_motor_saturation_count++; 74 | } else { 75 | left_motor_saturation_count = 0; 76 | } 77 | 78 | if (pwm_left >= 0) { 79 | timer_set_oc_value(TIM8, TIM_OC4, MOTORES_MAX_PWM - abs(pwm_left)); 80 | timer_set_oc_value(TIM8, TIM_OC3, MOTORES_MAX_PWM); 81 | } else { 82 | timer_set_oc_value(TIM8, TIM_OC3, MOTORES_MAX_PWM - abs(pwm_left)); 83 | timer_set_oc_value(TIM8, TIM_OC4, MOTORES_MAX_PWM); 84 | } 85 | 86 | if (pwm_right > MOTORES_MAX_PWM) { 87 | pwm_right = MOTORES_MAX_PWM; 88 | } else if (pwm_right < -MOTORES_MAX_PWM) { 89 | pwm_right = -MOTORES_MAX_PWM; 90 | } 91 | 92 | if (abs(pwm_right) >= MOTORES_SATURATION_PWM) { 93 | right_motor_saturation_count++; 94 | } else { 95 | right_motor_saturation_count = 0; 96 | } 97 | 98 | if (pwm_right >= 0) { 99 | timer_set_oc_value(TIM8, TIM_OC2, MOTORES_MAX_PWM - abs(pwm_right)); 100 | timer_set_oc_value(TIM8, TIM_OC1, MOTORES_MAX_PWM); 101 | } else { 102 | timer_set_oc_value(TIM8, TIM_OC1, MOTORES_MAX_PWM - abs(pwm_right)); 103 | timer_set_oc_value(TIM8, TIM_OC2, MOTORES_MAX_PWM); 104 | } 105 | 106 | if(abs(get_encoder_angular_speed()) >= MOTORES_SATURATION_ANGULAR_SPEED) { 107 | angular_speed_saturation_count++; 108 | } else { 109 | angular_speed_saturation_count = 0; 110 | } 111 | 112 | check_motors_saturated(); 113 | // printf("%d - %d\n", abs(pwm_left), abs(pwm_right)); 114 | } 115 | 116 | void set_fan_speed(uint8_t vel) { 117 | uint32_t ocF = 0; 118 | if (vel != 0) { 119 | ocF = map(abs(vel), 0, 100, 0, LEDS_MAX_PWM); 120 | } 121 | // printf("%ld\n", ocF); 122 | timer_set_oc_value(TIM1, TIM_OC1, ocF); 123 | } 124 | 125 | void reset_motors_saturated(void) { 126 | motors_saturated = false; 127 | motors_saturated_ms = 0; 128 | left_motor_saturation_count = 0; 129 | right_motor_saturation_count = 0; 130 | check_motors_saturated_enabled = true; 131 | } 132 | 133 | bool is_motor_saturated(void) { 134 | return motors_saturated; 135 | } 136 | 137 | uint32_t get_motors_saturated_ms(void) { 138 | return motors_saturated_ms; 139 | } 140 | -------------------------------------------------------------------------------- /source_code/src/rc5.c: -------------------------------------------------------------------------------- 1 | /** 2 | Basado en https://clearwater.com.au/code/rc5 3 | */ 4 | #include 5 | 6 | enum RC5_TIMMINGS { 7 | MIN_SHORT = 444, 8 | MAX_SHORT = 1333, 9 | MIN_LONG = 1334, 10 | MAX_LONG = 2222 11 | }; 12 | 13 | enum RC5_EVENT { 14 | EVENT_SHORTSPACE = 0, 15 | EVENT_SHORTPULSE = 2, 16 | EVENT_LONGSPACE = 4, 17 | EVENT_LONGPULSE = 6 18 | }; 19 | 20 | enum RC5_STATE { 21 | STATE_START1 = 0, 22 | STATE_MID1 = 1, 23 | STATE_MID0 = 2, 24 | STATE_START0 = 3 25 | }; 26 | 27 | enum RC5_MANAGE { 28 | S2_MASK = 0x1000, 29 | S2_SHIFT = 12, 30 | TOGGLE_MASK = 0x0800, 31 | TOGGLE_SHIFT = 11, 32 | ADDRESS_MASK = 0x7C0, 33 | ADDRESS_SHIFT = 6, 34 | COMMAND_MASK = 0x003F, 35 | COMMAND_SHIFT = 0 36 | }; 37 | 38 | enum RC5_ADDRESS { 39 | ADDRESS_PROG = 0x0B, 40 | ADDRESS_COMP = 0x07, 41 | ADDRESS_MENU = 0x1B, 42 | }; 43 | 44 | enum RC5_CUSTOM_CMD { 45 | CUSTOM_CMD_MENU = 0x0D, 46 | CUSTOM_CMD_MENU_UP = 0x0E, 47 | CUSTOM_CMD_MENU_DOWN = 0x0C, 48 | }; 49 | 50 | static int16_t rc5_stored_data[RC5_DATA_LENGTH]; 51 | enum RC5_DATA_INDEX { 52 | DATA_STOP = 0, 53 | DATA_START = 1, 54 | DATA_MENU = 2, 55 | DATA_MENU_UP = 3, 56 | DATA_MENU_DOWN = 4, 57 | }; 58 | 59 | static const uint8_t trans[] = {0x01, 0x91, 0x9B, 0xFB}; 60 | 61 | static uint8_t state = STATE_MID1; 62 | static uint16_t cmd = 1; 63 | static uint8_t bits = 1; 64 | static uint32_t last_us = 0; 65 | 66 | static void rc5_manage_command(uint16_t message) { 67 | // unsigned char toggle = (message & TOGGLE_MASK) >> TOGGLE_SHIFT; 68 | unsigned char address = (message & ADDRESS_MASK) >> ADDRESS_SHIFT; 69 | unsigned char command = (message & COMMAND_MASK) >> COMMAND_SHIFT; 70 | // printf("RC5: %d %d\n", address, command); 71 | switch (address) { 72 | case ADDRESS_PROG: 73 | rc5_stored_data[DATA_STOP] = command; 74 | rc5_stored_data[DATA_START] = command + DATA_START; 75 | rc5_stored_data[DATA_MENU] = command; 76 | rc5_stored_data[DATA_MENU_UP] = command + 1; 77 | rc5_stored_data[DATA_MENU_DOWN] = command + 2; 78 | eeprom_set_data(DATA_INDEX_RC5, rc5_stored_data, RC5_DATA_LENGTH); 79 | eeprom_save(); 80 | break; 81 | case ADDRESS_COMP: 82 | if (command == rc5_stored_data[DATA_START]) { 83 | if(menu_run_can_start()){ 84 | // set_competicion_iniciando(true); 85 | }else{ 86 | set_debug_btn(true); 87 | } 88 | } else if (command == rc5_stored_data[DATA_STOP]) { 89 | set_race_started(false); 90 | } 91 | break; 92 | case ADDRESS_MENU: 93 | if (command == CUSTOM_CMD_MENU || command == rc5_stored_data[DATA_MENU]) { 94 | menu_rc5_mode_change(); 95 | } else if (command == CUSTOM_CMD_MENU_UP || command == rc5_stored_data[DATA_MENU_UP]) { 96 | menu_rc5_up(); 97 | } else if (command == CUSTOM_CMD_MENU_DOWN || command == rc5_stored_data[DATA_MENU_DOWN]) { 98 | menu_rc5_down(); 99 | } 100 | break; 101 | } 102 | } 103 | 104 | static void reset(void) { 105 | state = STATE_MID1; 106 | cmd = 1; 107 | bits = 1; 108 | last_us = get_us_counter(); 109 | } 110 | 111 | static void rc5_decode_event(enum RC5_EVENT event) { 112 | 113 | unsigned char newState = (trans[state] >> event) & 0x3; 114 | if (newState == state) { 115 | reset(); 116 | } else { 117 | state = newState; 118 | if (newState == STATE_MID0) { 119 | cmd = (cmd << 1); 120 | bits++; 121 | } else if (newState == STATE_MID1) { 122 | cmd = (cmd << 1) + 1; 123 | bits++; 124 | } 125 | } 126 | } 127 | 128 | static void rc5_decode_pulse(enum RC5_TRIGGER trigger, uint32_t elapsed) { 129 | 130 | if (elapsed >= MIN_SHORT && elapsed <= MAX_SHORT) { 131 | rc5_decode_event(trigger == RC5_TRIGGER_FALLING ? EVENT_SHORTSPACE : EVENT_SHORTPULSE); 132 | } else if (elapsed >= MIN_LONG && elapsed <= MAX_LONG) { 133 | rc5_decode_event(trigger == RC5_TRIGGER_FALLING ? EVENT_LONGSPACE : EVENT_LONGPULSE); 134 | } else { 135 | reset(); 136 | } 137 | } 138 | 139 | void rc5_load_eeprom(void) { 140 | int16_t *eeprom_data = eeprom_get_data(); 141 | for (uint16_t i = DATA_INDEX_RC5; i < (DATA_INDEX_RC5 + RC5_DATA_LENGTH); i++) { 142 | rc5_stored_data[i - DATA_INDEX_RC5] = eeprom_data[i]; 143 | } 144 | } 145 | 146 | void rc5_register(enum RC5_TRIGGER trigger) { 147 | uint32_t us = get_us_counter(); 148 | rc5_decode_pulse(trigger, us - last_us); 149 | last_us = us; 150 | 151 | if (bits == 14) { 152 | rc5_manage_command(cmd); 153 | cmd = 0; 154 | bits = 0; 155 | } 156 | } -------------------------------------------------------------------------------- /source_code/src/sensors.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static bool sensors_enabled = false; 4 | 5 | static uint8_t emitter_status = 1; 6 | static uint8_t sensor_index = SENSOR_FRONT_LEFT_WALL_ID; 7 | 8 | static uint8_t aux_adc_channels[NUM_AUX_ADC_CHANNELS] = { 9 | ADC_CHANNEL4, 10 | ADC_CHANNEL5, 11 | ADC_CHANNEL6, 12 | ADC_CHANNEL7, 13 | }; 14 | static volatile uint16_t aux_adc_raw[NUM_AUX_ADC_CHANNELS]; 15 | 16 | static uint8_t sensores[NUM_SENSORES] = { 17 | ADC_CHANNEL10, // DETECTA SENSOR_FRONT_LEFT_WALL_ID - NO CAMBIAR 18 | ADC_CHANNEL13, // DETECTA SENSOR_FRONT_RIGHT_WALL_ID - NO CAMBIAR 19 | ADC_CHANNEL12, // DETECTA SENSOR_SIDE_LEFT_WALL_ID - NO CAMBIAR 20 | ADC_CHANNEL11, // DETECTA SENSOR_SIDE_RIGHT_WALL_ID - NO CAMBIAR 21 | }; 22 | static volatile uint16_t sensors_raw[NUM_SENSORES]; 23 | static volatile uint16_t sensors_off[NUM_SENSORES]; 24 | static volatile uint16_t sensors_on[NUM_SENSORES]; 25 | 26 | volatile uint16_t sensors_distance[NUM_SENSORES]; 27 | int16_t sensors_distance_offset[NUM_SENSORES] = {0, 0, 0, 0}; 28 | 29 | #ifdef ZOROBOT3_A 30 | struct sensors_distance_calibration sensors_distance_calibrations[] = { 31 | [SENSOR_FRONT_LEFT_WALL_ID] = { 32 | .a = 2.881, 33 | .b = 0.333, 34 | .c = -3.307, 35 | }, 36 | [SENSOR_FRONT_RIGHT_WALL_ID] = { 37 | .a = 2.866, 38 | .b = 0.333, 39 | .c = 2.251, 40 | }, 41 | [SENSOR_SIDE_LEFT_WALL_ID] = { 42 | .a = 2.357, 43 | .b = 0.297, 44 | .c = -50.430, 45 | }, 46 | [SENSOR_SIDE_RIGHT_WALL_ID] = { 47 | .a = 2.399, 48 | .b = 0.307, 49 | .c = -4.694, 50 | }, 51 | }; 52 | #endif 53 | 54 | #ifdef ZOROBOT3_B 55 | struct sensors_distance_calibration sensors_distance_calibrations[] = { 56 | [SENSOR_FRONT_LEFT_WALL_ID] = { 57 | .a = 2.915, 58 | .b = 0.346, 59 | .c = 23.996, 60 | }, 61 | [SENSOR_FRONT_RIGHT_WALL_ID] = { 62 | .a = 2.649, 63 | .b = 0.318, 64 | .c = 28.761, 65 | }, 66 | [SENSOR_SIDE_LEFT_WALL_ID] = { 67 | .a = 2.277, 68 | .b = 0.289, 69 | .c = 29.466, 70 | }, 71 | [SENSOR_SIDE_RIGHT_WALL_ID] = { 72 | .a = 2.374, 73 | .b = 0.307, 74 | .c = -12.683, 75 | }, 76 | }; 77 | #endif 78 | 79 | #ifdef ZOROBOT3_C 80 | struct sensors_distance_calibration sensors_distance_calibrations[] = { 81 | [SENSOR_FRONT_LEFT_WALL_ID] = { 82 | .a = 2.717, 83 | .b = 0.321, 84 | .c = 34.784, 85 | }, 86 | [SENSOR_FRONT_RIGHT_WALL_ID] = { 87 | .a = 2.992, 88 | .b = 0.355, 89 | .c = 37.060, 90 | }, 91 | [SENSOR_SIDE_LEFT_WALL_ID] = { 92 | .a = 1.900, 93 | .b = 0.247, 94 | .c = -0.844, 95 | }, 96 | [SENSOR_SIDE_RIGHT_WALL_ID] = { 97 | .a = 2.300, 98 | .b = 0.295, 99 | .c = 35.468, 100 | }, 101 | }; 102 | #endif 103 | 104 | #define LOG_LINEARIZATION_TABLE_STEP 4 105 | #define LOG_LINEARIZATION_TABLE_SIZE (ADC_RESOLUTION / LOG_LINEARIZATION_TABLE_STEP) 106 | 107 | /** 108 | * @brief Tabla para linealización de sensores en base a la función de logaritmo. 109 | * Contiene los valores de logaritmo de los valores entre `1` y `ADC_RESOLUTION - 1`. 110 | * 111 | * ? It uses steps of size `LOG_LINEARIZATION_TABLE_STEP`. 112 | * ? Usa pasos de tamaño `LOG_LINEARIZATION_TABLE_STEP`. 113 | * ? Para evitar resultados inestables, calcula `ln(0)` como `1`. 114 | */ 115 | const float ln_lookup[LOG_LINEARIZATION_TABLE_SIZE] = { 116 | 1., 1.3863, 2.0794, 2.4849, 2.7726, 2.9957, 3.1781, 3.3322, 3.4657, 3.5835, 3.6889, 3.7842, 3.8712, 3.9512, 4.0254, 4.0943, 4.1589, 4.2195, 4.2767, 4.3307, 4.382, 4.4308, 4.4773, 4.5218, 4.5643, 4.6052, 4.6444, 4.6821, 4.7185, 4.7536, 4.7875, 4.8203, 4.852, 4.8828, 4.9127, 4.9416, 4.9698, 4.9972, 5.0239, 5.0499, 5.0752, 5.0999, 5.124, 5.1475, 5.1705, 5.193, 5.2149, 5.2364, 5.2575, 5.2781, 5.2983, 5.3181, 5.3375, 5.3566, 5.3753, 5.3936, 5.4116, 5.4293, 5.4467, 5.4638, 5.4806, 5.4972, 5.5134, 5.5294, 5.5452, 5.5607, 5.5759, 5.591, 5.6058, 5.6204, 5.6348, 5.649, 5.663, 5.6768, 5.6904, 5.7038, 5.717, 5.7301, 5.743, 5.7557, 5.7683, 5.7807, 5.793, 5.8051, 5.8171, 5.8289, 5.8406, 5.8522, 5.8636, 5.8749, 5.8861, 5.8972, 5.9081, 5.9189, 5.9296, 5.9402, 5.9506, 5.961, 5.9713, 5.9814, 5.9915, 6.0014, 6.0113, 6.021, 6.0307, 6.0403, 6.0497, 6.0591, 6.0684, 6.0776, 6.0868, 6.0958, 6.1048, 6.1137, 6.1225, 6.1312, 6.1399, 6.1485, 6.157, 6.1654, 6.1738, 6.1821, 6.1903, 6.1985, 6.2066, 6.2146, 6.2226, 6.2305, 6.2383, 6.2461, 6.2538, 6.2615, 6.2691, 6.2766, 6.2841, 6.2916, 6.2989, 6.3063, 6.3135, 6.3208, 6.3279, 6.3351, 6.3421, 6.3491, 6.3561, 6.363, 6.3699, 6.3767, 6.3835, 6.3902, 6.3969, 6.4036, 6.4102, 6.4167, 6.4232, 6.4297, 6.4362, 6.4425, 6.4489, 6.4552, 6.4615, 6.4677, 6.4739, 6.48, 6.4862, 6.4922, 6.4983, 6.5043, 6.5103, 6.5162, 6.5221, 6.528, 6.5338, 6.5396, 6.5453, 6.5511, 6.5568, 6.5624, 6.5681, 6.5737, 6.5793, 6.5848, 6.5903, 6.5958, 6.6012, 6.6067, 6.612, 6.6174, 6.6227, 6.628, 6.6333, 6.6386, 6.6438, 6.649, 6.6542, 6.6593, 6.6644, 6.6695, 6.6746, 6.6796, 6.6846, 6.6896, 6.6946, 6.6995, 6.7044, 6.7093, 6.7142, 6.719, 6.7238, 6.7286, 6.7334, 6.7382, 6.7429, 6.7476, 6.7523, 6.7569, 6.7616, 6.7662, 6.7708, 6.7754, 6.7799, 6.7845, 6.789, 6.7935, 6.7979, 6.8024, 6.8068, 6.8112, 6.8156, 6.82, 6.8244, 6.8287, 6.833, 6.8373, 6.8416, 6.8459, 6.8501, 6.8544, 6.8586, 6.8628, 6.8669, 6.8711, 6.8752, 6.8794, 6.8835, 6.8876, 6.8916, 6.8957, 6.8997, 6.9037, 6.9078, 6.9117, 6.9157, 6.9197, 6.9236, 6.9276, 6.9315, 6.9354, 6.9393, 6.9431, 6.947, 6.9508, 6.9546, 6.9584, 6.9622, 6.966, 6.9698, 6.9735, 6.9773, 6.981, 6.9847, 6.9884, 6.9921, 6.9958, 6.9994, 7.0031, 7.0067, 7.0103, 7.0139, 7.0175, 7.0211, 7.0246, 7.0282, 7.0317, 7.0353, 7.0388, 7.0423, 7.0458, 7.0493, 7.0527, 7.0562, 7.0596, 7.063, 7.0665, 7.0699, 7.0733, 7.0767, 7.08, 7.0834, 7.0867, 7.0901, 7.0934, 7.0967, 7.1, 7.1033, 7.1066, 7.1099, 7.1131, 7.1164, 7.1196, 7.1229, 7.1261, 7.1293, 7.1325, 7.1357, 7.1389, 7.142, 7.1452, 7.1483, 7.1515, 7.1546, 7.1577, 7.1608, 7.1639, 7.167, 7.1701, 7.1732, 7.1763, 7.1793, 7.1824, 7.1854, 7.1884, 7.1914, 7.1944, 7.1974, 7.2004, 7.2034, 7.2064, 7.2093, 7.2123, 7.2152, 7.2182, 7.2211, 7.224, 7.2269, 7.2298, 7.2327, 7.2356, 7.2385, 7.2414, 7.2442, 7.2471, 7.2499, 7.2528, 7.2556, 7.2584, 7.2612, 7.264, 7.2668, 7.2696, 7.2724, 7.2752, 7.2779, 7.2807, 7.2834, 7.2862, 7.2889, 7.2917, 7.2944, 7.2971, 7.2998, 7.3025, 7.3052, 7.3079, 7.3106, 7.3132, 7.3159, 7.3185, 7.3212, 7.3238, 7.3265, 7.3291, 7.3317, 7.3343, 7.3369, 7.3395, 7.3421, 7.3447, 7.3473, 7.3499, 7.3524, 7.355, 7.3576, 7.3601, 7.3626, 7.3652, 7.3677, 7.3702, 7.3727, 7.3753, 7.3778, 7.3803, 7.3827, 7.3852, 7.3877, 7.3902, 7.3926, 7.3951, 7.3976, 7.4, 7.4025, 7.4049, 7.4073, 7.4097, 7.4122, 7.4146, 7.417, 7.4194, 7.4218, 7.4242, 7.4265, 7.4289, 7.4313, 7.4337, 7.436, 7.4384, 7.4407, 7.4431, 7.4454, 7.4478, 7.4501, 7.4524, 7.4547, 7.457, 7.4593, 7.4616, 7.4639, 7.4662, 7.4685, 7.4708, 7.4731, 7.4753, 7.4776, 7.4799, 7.4821, 7.4844, 7.4866, 7.4889, 7.4911, 7.4933, 7.4955, 7.4978, 7.5, 7.5022, 7.5044, 7.5066, 7.5088, 7.511, 7.5132, 7.5153, 7.5175, 7.5197, 7.5219, 7.524, 7.5262, 7.5283, 7.5305, 7.5326, 7.5348, 7.5369, 7.539, 7.5412, 7.5433, 7.5454, 7.5475, 7.5496, 7.5517, 7.5538, 7.5559, 7.558, 7.5601, 7.5622, 7.5642, 7.5663, 7.5684, 7.5704, 7.5725, 7.5746, 7.5766, 7.5787, 7.5807, 7.5827, 7.5848, 7.5868, 7.5888, 7.5909, 7.5929, 7.5949, 7.5969, 7.5989, 7.6009, 7.6029, 7.6049, 7.6069, 7.6089, 7.6109, 7.6128, 7.6148, 7.6168, 7.6187, 7.6207, 7.6227, 7.6246, 7.6266, 7.6285, 7.6305, 7.6324, 7.6343, 7.6363, 7.6382, 7.6401, 7.642, 7.644, 7.6459, 7.6478, 7.6497, 7.6516, 7.6535, 7.6554, 7.6573, 7.6592, 7.6611, 7.6629, 7.6648, 7.6667, 7.6686, 7.6704, 7.6723, 7.6742, 7.676, 7.6779, 7.6797, 7.6816, 7.6834, 7.6852, 7.6871, 7.6889, 7.6907, 7.6926, 7.6944, 7.6962, 7.698, 7.6998, 7.7017, 7.7035, 7.7053, 7.7071, 7.7089, 7.7107, 7.7124, 7.7142, 7.716, 7.7178, 7.7196, 7.7213, 7.7231, 7.7249, 7.7267, 7.7284, 7.7302, 7.7319, 7.7337, 7.7354, 7.7372, 7.7389, 7.7407, 7.7424, 7.7441, 7.7459, 7.7476, 7.7493, 7.751, 7.7528, 7.7545, 7.7562, 7.7579, 7.7596, 7.7613, 7.763, 7.7647, 7.7664, 7.7681, 7.7698, 7.7715, 7.7732, 7.7749, 7.7765, 7.7782, 7.7799, 7.7816, 7.7832, 7.7849, 7.7866, 7.7882, 7.7899, 7.7915, 7.7932, 7.7948, 7.7965, 7.7981, 7.7998, 7.8014, 7.803, 7.8047, 7.8063, 7.8079, 7.8095, 7.8112, 7.8128, 7.8144, 7.816, 7.8176, 7.8192, 7.8208, 7.8224, 7.824, 7.8256, 7.8272, 7.8288, 7.8304, 7.832, 7.8336, 7.8352, 7.8368, 7.8383, 7.8399, 7.8415, 7.8431, 7.8446, 7.8462, 7.8478, 7.8493, 7.8509, 7.8524, 7.854, 7.8555, 7.8571, 7.8586, 7.8602, 7.8617, 7.8633, 7.8648, 7.8663, 7.8679, 7.8694, 7.8709, 7.8725, 7.874, 7.8755, 7.877, 7.8785, 7.88, 7.8816, 7.8831, 7.8846, 7.8861, 7.8876, 7.8891, 7.8906, 7.8921, 7.8936, 7.8951, 7.8966, 7.898, 7.8995, 7.901, 7.9025, 7.904, 7.9054, 7.9069, 7.9084, 7.9099, 7.9113, 7.9128, 7.9143, 7.9157, 7.9172, 7.9186, 7.9201, 7.9215, 7.923, 7.9244, 7.9259, 7.9273, 7.9288, 7.9302, 7.9316, 7.9331, 7.9345, 7.9359, 7.9374, 7.9388, 7.9402, 7.9417, 7.9431, 7.9445, 7.9459, 7.9473, 7.9487, 7.9501, 7.9516, 7.953, 7.9544, 7.9558, 7.9572, 7.9586, 7.96, 7.9614, 7.9628, 7.9642, 7.9655, 7.9669, 7.9683, 7.9697, 7.9711, 7.9725, 7.9738, 7.9752, 7.9766, 7.978, 7.9793, 7.9807, 7.9821, 7.9834, 7.9848, 7.9862, 7.9875, 7.9889, 7.9902, 7.9916, 7.9929, 7.9943, 7.9956, 7.997, 7.9983, 7.9997, 8.001, 8.0024, 8.0037, 8.005, 8.0064, 8.0077, 8.009, 8.0104, 8.0117, 8.013, 8.0143, 8.0157, 8.017, 8.0183, 8.0196, 8.0209, 8.0222, 8.0236, 8.0249, 8.0262, 8.0275, 8.0288, 8.0301, 8.0314, 8.0327, 8.034, 8.0353, 8.0366, 8.0379, 8.0392, 8.0404, 8.0417, 8.043, 8.0443, 8.0456, 8.0469, 8.0481, 8.0494, 8.0507, 8.052, 8.0533, 8.0545, 8.0558, 8.0571, 8.0583, 8.0596, 8.0609, 8.0621, 8.0634, 8.0646, 8.0659, 8.0671, 8.0684, 8.0697, 8.0709, 8.0722, 8.0734, 8.0746, 8.0759, 8.0771, 8.0784, 8.0796, 8.0809, 8.0821, 8.0833, 8.0846, 8.0858, 8.087, 8.0883, 8.0895, 8.0907, 8.0919, 8.0932, 8.0944, 8.0956, 8.0968, 8.098, 8.0993, 8.1005, 8.1017, 8.1029, 8.1041, 8.1053, 8.1065, 8.1077, 8.1089, 8.1101, 8.1113, 8.1125, 8.1137, 8.1149, 8.1161, 8.1173, 8.1185, 8.1197, 8.1209, 8.1221, 8.1233, 8.1244, 8.1256, 8.1268, 8.128, 8.1292, 8.1304, 8.1315, 8.1327, 8.1339, 8.1351, 8.1362, 8.1374, 8.1386, 8.1397, 8.1409, 8.1421, 8.1432, 8.1444, 8.1455, 8.1467, 8.1479, 8.149, 8.1502, 8.1513, 8.1525, 8.1536, 8.1548, 8.1559, 8.1571, 8.1582, 8.1594, 8.1605, 8.1617, 8.1628, 8.1639, 8.1651, 8.1662, 8.1674, 8.1685, 8.1696, 8.1708, 8.1719, 8.173, 8.1741, 8.1753, 8.1764, 8.1775, 8.1786, 8.1798, 8.1809, 8.182, 8.1831, 8.1842, 8.1854, 8.1865, 8.1876, 8.1887, 8.1898, 8.1909, 8.192, 8.1931, 8.1942, 8.1953, 8.1964, 8.1975, 8.1986, 8.1997, 8.2008, 8.2019, 8.203, 8.2041, 8.2052, 8.2063, 8.2074, 8.2085, 8.2096, 8.2107, 8.2118, 8.2128, 8.2139, 8.215, 8.2161, 8.2172, 8.2182, 8.2193, 8.2204, 8.2215, 8.2226, 8.2236, 8.2247, 8.2258, 8.2268, 8.2279, 8.229, 8.23, 8.2311, 8.2322, 8.2332, 8.2343, 8.2354, 8.2364, 8.2375, 8.2385, 8.2396, 8.2406, 8.2417, 8.2428, 8.2438, 8.2449, 8.2459, 8.247, 8.248, 8.2491, 8.2501, 8.2511, 8.2522, 8.2532, 8.2543, 8.2553, 8.2563, 8.2574, 8.2584, 8.2595, 8.2605, 8.2615, 8.2626, 8.2636, 8.2646, 8.2657, 8.2667, 8.2677, 8.2687, 8.2698, 8.2708, 8.2718, 8.2728, 8.2738, 8.2749, 8.2759, 8.2769, 8.2779, 8.2789, 8.28, 8.281, 8.282, 8.283, 8.284, 8.285, 8.286, 8.287, 8.288, 8.289, 8.29, 8.291, 8.292, 8.293, 8.294, 8.295, 8.296, 8.297, 8.298, 8.299, 8.3, 8.301, 8.302, 8.303, 8.304, 8.305, 8.306, 8.307, 8.308, 8.3089, 8.3099, 8.3109, 8.3119, 8.3129, 8.3139, 8.3148, 8.3158, 8.3168}; 117 | 118 | static volatile int16_t last_front_sensors_angle_error = 0; 119 | 120 | uint8_t *get_aux_adc_channels(void) { 121 | return aux_adc_channels; 122 | } 123 | 124 | uint8_t get_aux_adc_channels_num(void) { 125 | return NUM_AUX_ADC_CHANNELS; 126 | } 127 | 128 | volatile uint16_t *get_aux_adc_raw(void) { 129 | return aux_adc_raw; 130 | } 131 | 132 | uint16_t get_aux_raw(uint8_t pos) { 133 | return aux_adc_raw[pos]; 134 | } 135 | 136 | /** 137 | * @brief Set an specific emitter ON. 138 | * 139 | * @param[in] emitter Emitter type. 140 | */ 141 | static void set_emitter_on(uint8_t emitter) { 142 | switch (emitter) { 143 | case SENSOR_FRONT_LEFT_WALL_ID: 144 | gpio_set(GPIOA, GPIO0); 145 | break; 146 | case SENSOR_FRONT_RIGHT_WALL_ID: 147 | gpio_set(GPIOA, GPIO3); 148 | break; 149 | case SENSOR_SIDE_RIGHT_WALL_ID: 150 | gpio_set(GPIOA, GPIO1); 151 | break; 152 | case SENSOR_SIDE_LEFT_WALL_ID: 153 | gpio_set(GPIOA, GPIO2); 154 | break; 155 | default: 156 | break; 157 | } 158 | } 159 | 160 | /** 161 | * @brief Set an specific emitter OFF. 162 | * 163 | * @param[in] emitter Emitter type. 164 | */ 165 | static void set_emitter_off(uint8_t emitter) { 166 | switch (emitter) { 167 | case SENSOR_FRONT_LEFT_WALL_ID: 168 | gpio_clear(GPIOA, GPIO0); 169 | break; 170 | case SENSOR_FRONT_RIGHT_WALL_ID: 171 | gpio_clear(GPIOA, GPIO3); 172 | break; 173 | case SENSOR_SIDE_RIGHT_WALL_ID: 174 | gpio_clear(GPIOA, GPIO1); 175 | break; 176 | case SENSOR_SIDE_LEFT_WALL_ID: 177 | gpio_clear(GPIOA, GPIO2); 178 | break; 179 | default: 180 | break; 181 | } 182 | } 183 | 184 | void set_sensors_enabled(bool enabled) { 185 | if (!sensors_enabled && enabled) { 186 | emitter_status = 1; 187 | sensor_index = SENSOR_FRONT_LEFT_WALL_ID; 188 | } 189 | sensors_enabled = enabled; 190 | } 191 | 192 | bool get_sensors_enabled(void) { 193 | return sensors_enabled; 194 | } 195 | 196 | void get_sensors_raw(uint16_t *on, uint16_t *off) { 197 | for (uint8_t i = 0; i < NUM_SENSORES; i++) { 198 | on[i] = sensors_on[i]; 199 | off[i] = sensors_off[i]; 200 | } 201 | } 202 | 203 | uint8_t *get_sensors(void) { 204 | return sensores; 205 | } 206 | 207 | uint8_t get_sensors_num(void) { 208 | return NUM_SENSORES; 209 | } 210 | 211 | /** 212 | * @brief Máquina de estados de valores de sensores 213 | * 214 | */ 215 | void sm_emitter_adc(void) { 216 | if (!sensors_enabled) { 217 | gpio_clear(GPIOA, GPIO0); 218 | gpio_clear(GPIOA, GPIO3); 219 | gpio_clear(GPIOA, GPIO1); 220 | gpio_clear(GPIOA, GPIO2); 221 | return; 222 | } 223 | 224 | switch (emitter_status) { 225 | case 1: 226 | sensors_off[sensor_index] = adc_read_injected(ADC2, (sensor_index + 1)); 227 | set_emitter_on(sensor_index); 228 | emitter_status = 2; 229 | break; 230 | case 2: 231 | adc_start_conversion_injected(ADC2); 232 | emitter_status = 3; 233 | break; 234 | case 3: 235 | sensors_on[sensor_index] = adc_read_injected(ADC2, (sensor_index + 1)); 236 | set_emitter_off(sensor_index); 237 | emitter_status = 4; 238 | break; 239 | case 4: 240 | adc_start_conversion_injected(ADC2); 241 | emitter_status = 1; 242 | if (sensor_index == (NUM_SENSORES - 1)) 243 | sensor_index = 0; 244 | else 245 | sensor_index++; 246 | break; 247 | default: 248 | break; 249 | } 250 | } 251 | 252 | uint16_t get_sensor_raw(uint8_t pos, bool on) { 253 | if (pos < NUM_SENSORES) { 254 | return on ? sensors_on[pos] : sensors_off[pos]; 255 | } else { 256 | return 0; 257 | } 258 | } 259 | 260 | uint16_t get_sensor_raw_filter(uint8_t pos) { 261 | if (pos < NUM_SENSORES) { 262 | if (sensors_on[pos] > sensors_off[pos]) { 263 | return sensors_on[pos] - sensors_off[pos]; 264 | } else { 265 | return 0; 266 | } 267 | } else { 268 | return 0; 269 | } 270 | } 271 | 272 | void front_sensors_calibration(void) { 273 | int32_t left_temp = 0; 274 | int16_t left_offset = 0; 275 | int32_t right_temp = 0; 276 | int16_t right_offset = 0; 277 | 278 | set_sensors_enabled(true); 279 | sensors_distance_offset[SENSOR_FRONT_LEFT_WALL_ID] = 0; 280 | sensors_distance_offset[SENSOR_FRONT_RIGHT_WALL_ID] = 0; 281 | delay(5); 282 | 283 | for (int i = 0; i < SENSOR_FRONT_CALIBRATION_READINGS; i++) { 284 | left_temp += sensors_distance[SENSOR_FRONT_LEFT_WALL_ID]; 285 | right_temp += sensors_distance[SENSOR_FRONT_RIGHT_WALL_ID]; 286 | set_leds_wave(35); 287 | delay(5); 288 | } 289 | set_info_leds(); 290 | left_offset = (int16_t)((CELL_DIMENSION - WALL_WIDTH - ROBOT_BACK_LENGTH) - (left_temp / SENSOR_FRONT_CALIBRATION_READINGS)); 291 | right_offset = (int16_t)((CELL_DIMENSION - WALL_WIDTH - ROBOT_BACK_LENGTH) - (right_temp / SENSOR_FRONT_CALIBRATION_READINGS)); 292 | sensors_distance_offset[SENSOR_FRONT_LEFT_WALL_ID] = left_offset; 293 | sensors_distance_offset[SENSOR_FRONT_RIGHT_WALL_ID] = right_offset; 294 | 295 | set_sensors_enabled(false); 296 | delay(500); 297 | clear_info_leds(); 298 | eeprom_set_data(DATA_INDEX_SENSORS_OFFSETS, sensors_distance_offset, NUM_SENSORES); 299 | } 300 | 301 | void side_sensors_calibration(void) { 302 | int32_t left_temp = 0; 303 | int16_t left_offset = 0; 304 | int32_t right_temp = 0; 305 | int16_t right_offset = 0; 306 | 307 | set_sensors_enabled(true); 308 | sensors_distance_offset[SENSOR_SIDE_LEFT_WALL_ID] = 0; 309 | sensors_distance_offset[SENSOR_SIDE_RIGHT_WALL_ID] = 0; 310 | delay(5); 311 | 312 | for (int i = 0; i < SENSOR_SIDE_CALIBRATION_READINGS; i++) { 313 | left_temp += sensors_distance[SENSOR_SIDE_LEFT_WALL_ID]; 314 | right_temp += sensors_distance[SENSOR_SIDE_RIGHT_WALL_ID]; 315 | set_leds_wave(35); 316 | delay(5); 317 | } 318 | set_info_leds(); 319 | left_offset = (int16_t)(MIDDLE_MAZE_DISTANCE - (left_temp / SENSOR_SIDE_CALIBRATION_READINGS)); 320 | right_offset = (int16_t)(MIDDLE_MAZE_DISTANCE - (right_temp / SENSOR_SIDE_CALIBRATION_READINGS)); 321 | sensors_distance_offset[SENSOR_SIDE_LEFT_WALL_ID] = left_offset; 322 | sensors_distance_offset[SENSOR_SIDE_RIGHT_WALL_ID] = right_offset; 323 | 324 | set_sensors_enabled(false); 325 | delay(500); 326 | clear_info_leds(); 327 | eeprom_set_data(DATA_INDEX_SENSORS_OFFSETS, sensors_distance_offset, NUM_SENSORES); 328 | } 329 | 330 | void sensors_load_eeprom(void) { 331 | int16_t *data = eeprom_get_data(); 332 | for (uint16_t i = DATA_INDEX_SENSORS_OFFSETS; i < (DATA_INDEX_SENSORS_OFFSETS + NUM_SENSORES); i++) { 333 | sensors_distance_offset[i - DATA_INDEX_SENSORS_OFFSETS] = data[i]; 334 | } 335 | } 336 | 337 | bool left_wall_detection(void) { 338 | return sensors_distance[SENSOR_SIDE_LEFT_WALL_ID] < SENSOR_SIDE_DETECTION; 339 | } 340 | 341 | bool right_wall_detection(void) { 342 | return sensors_distance[SENSOR_SIDE_RIGHT_WALL_ID] < SENSOR_SIDE_DETECTION; 343 | } 344 | 345 | bool front_wall_detection(void) { 346 | return (sensors_distance[SENSOR_FRONT_LEFT_WALL_ID] < SENSOR_FRONT_DETECTION) || 347 | (sensors_distance[SENSOR_FRONT_RIGHT_WALL_ID] < SENSOR_FRONT_DETECTION); 348 | } 349 | 350 | /** 351 | * @brief Las magias de los sensores vienen dadas por un script de python que calcula los valores ABC de la ecuación que los caracteriza. 352 | * TODO: A mayores, se aplica un offset al valor calculado de las distancias, calibrado mediante menú. 353 | */ 354 | void update_sensors_magics(void) { 355 | if (!sensors_enabled) { 356 | return; 357 | } 358 | for (uint8_t sensor = 0; sensor < NUM_SENSORES; sensor++) { 359 | if (sensors_on[sensor] > sensors_off[sensor]) { 360 | sensors_raw[sensor] = sensors_on[sensor] - sensors_off[sensor]; 361 | 362 | int16_t ln_index = (sensors_raw[sensor] + sensors_distance_calibrations[sensor].c) / 4; 363 | if (ln_index < 0) { 364 | ln_index = 0; 365 | } 366 | float ln = ln_lookup[ln_index]; 367 | int16_t new_sensor_distance = (int16_t)((sensors_distance_calibrations[sensor].a / ln - sensors_distance_calibrations[sensor].b) * 1000.0f); 368 | if (new_sensor_distance < 0) { 369 | new_sensor_distance = 0; 370 | } 371 | 372 | switch (sensor) { 373 | case SENSOR_FRONT_LEFT_WALL_ID: 374 | case SENSOR_FRONT_RIGHT_WALL_ID: 375 | new_sensor_distance += ROBOT_FRONT_LENGTH; 376 | break; 377 | case SENSOR_SIDE_LEFT_WALL_ID: 378 | case SENSOR_SIDE_RIGHT_WALL_ID: 379 | new_sensor_distance += ROBOT_MIDDLE_WIDTH; 380 | break; 381 | } 382 | new_sensor_distance += sensors_distance_offset[sensor]; 383 | // sensors_distance[sensor] = 0.1f * new_sensor_distance + (1 - 0.1f) * sensors_distance[sensor]; 384 | sensors_distance[sensor] = new_sensor_distance; 385 | } 386 | } 387 | } 388 | 389 | void update_side_sensors_leds(void) { 390 | int16_t side_error_leds = get_side_sensors_close_error(); 391 | if (abs(side_error_leds) < 2) { 392 | clear_info_leds(); 393 | } else if (side_error_leds >= 20) { 394 | set_info_led(0, true); 395 | set_info_led(1, false); 396 | set_info_led(2, false); 397 | set_info_led(3, false); 398 | set_info_led(4, false); 399 | set_info_led(5, false); 400 | set_info_led(6, false); 401 | set_info_led(7, false); 402 | } else if (side_error_leds >= 10) { 403 | set_info_led(0, false); 404 | set_info_led(1, true); 405 | set_info_led(2, false); 406 | set_info_led(3, false); 407 | set_info_led(4, false); 408 | set_info_led(5, false); 409 | set_info_led(6, false); 410 | set_info_led(7, false); 411 | } else if (side_error_leds >= 5) { 412 | set_info_led(0, false); 413 | set_info_led(1, false); 414 | set_info_led(2, true); 415 | set_info_led(3, false); 416 | set_info_led(4, false); 417 | set_info_led(5, false); 418 | set_info_led(6, false); 419 | set_info_led(7, false); 420 | } else if (side_error_leds >= 0) { 421 | set_info_led(0, false); 422 | set_info_led(1, false); 423 | set_info_led(2, false); 424 | set_info_led(3, true); 425 | set_info_led(4, false); 426 | set_info_led(5, false); 427 | set_info_led(6, false); 428 | set_info_led(7, false); 429 | } else if (side_error_leds <= -20) { 430 | set_info_led(0, false); 431 | set_info_led(1, false); 432 | set_info_led(2, false); 433 | set_info_led(3, false); 434 | set_info_led(4, false); 435 | set_info_led(5, false); 436 | set_info_led(6, false); 437 | set_info_led(7, true); 438 | } else if (side_error_leds <= -10) { 439 | set_info_led(0, false); 440 | set_info_led(1, false); 441 | set_info_led(2, false); 442 | set_info_led(3, false); 443 | set_info_led(4, false); 444 | set_info_led(5, false); 445 | set_info_led(6, true); 446 | set_info_led(7, false); 447 | } else if (side_error_leds <= -5) { 448 | set_info_led(0, false); 449 | set_info_led(1, false); 450 | set_info_led(2, false); 451 | set_info_led(3, false); 452 | set_info_led(4, false); 453 | set_info_led(5, true); 454 | set_info_led(6, false); 455 | set_info_led(7, false); 456 | } else if (side_error_leds <= 0) { 457 | set_info_led(0, false); 458 | set_info_led(1, false); 459 | set_info_led(2, false); 460 | set_info_led(3, false); 461 | set_info_led(4, true); 462 | set_info_led(5, false); 463 | set_info_led(6, false); 464 | set_info_led(7, false); 465 | } 466 | } 467 | 468 | uint16_t get_sensor_distance(uint8_t pos) { 469 | return sensors_distance[pos]; 470 | } 471 | 472 | uint16_t get_front_wall_distance(void) { 473 | return (sensors_distance[SENSOR_FRONT_LEFT_WALL_ID] + sensors_distance[SENSOR_FRONT_RIGHT_WALL_ID]) / 2; 474 | } 475 | 476 | struct walls get_walls(void) { 477 | struct walls walls; 478 | walls.front = front_wall_detection(); 479 | walls.left = left_wall_detection(); 480 | walls.right = right_wall_detection(); 481 | return walls; 482 | } 483 | 484 | int16_t get_side_sensors_close_error(void) { 485 | int16_t left_error = sensors_distance[SENSOR_SIDE_LEFT_WALL_ID] - MIDDLE_MAZE_DISTANCE; 486 | int16_t right_error = sensors_distance[SENSOR_SIDE_RIGHT_WALL_ID] - MIDDLE_MAZE_DISTANCE; 487 | // printf("\t%4d | %4d = ", left_error, right_error); 488 | if (left_error > 0 && right_error < 0) { 489 | return right_error; 490 | } else if (right_error > 0 && left_error < 0) { 491 | return -left_error; 492 | } 493 | return 0; 494 | } 495 | 496 | int16_t get_side_sensors_far_error(void) { 497 | int16_t left_error = sensors_distance[SENSOR_SIDE_LEFT_WALL_ID] - MIDDLE_MAZE_DISTANCE; 498 | int16_t right_error = sensors_distance[SENSOR_SIDE_RIGHT_WALL_ID] - MIDDLE_MAZE_DISTANCE; 499 | // printf("\t%4d | %4d = ", left_error, right_error); 500 | 501 | if ((left_error > 90 && left_error < 350) && (right_error < 40)) { 502 | return right_error; 503 | } 504 | if ((right_error > 90 && right_error < 350) && (left_error < 40)) { 505 | return -left_error; 506 | } 507 | 508 | return 0; 509 | } 510 | 511 | int16_t get_side_sensors_error(void) { 512 | int16_t left_error = sensors_distance[SENSOR_SIDE_LEFT_WALL_ID] - MIDDLE_MAZE_DISTANCE; 513 | int16_t right_error = sensors_distance[SENSOR_SIDE_RIGHT_WALL_ID] - MIDDLE_MAZE_DISTANCE; 514 | 515 | if (sensors_distance[SENSOR_SIDE_LEFT_WALL_ID] < 90 && sensors_distance[SENSOR_SIDE_RIGHT_WALL_ID] < 90) { 516 | return right_error - left_error; 517 | } else if (sensors_distance[SENSOR_SIDE_LEFT_WALL_ID] < 90) { 518 | return -2 * left_error; 519 | } else if (sensors_distance[SENSOR_SIDE_RIGHT_WALL_ID] < 90) { 520 | return 2 * right_error; 521 | } else { 522 | return 0; 523 | } 524 | } 525 | 526 | int16_t get_diagonal_sensors_error(void) { 527 | bool left_correction = sensors_distance[SENSOR_FRONT_LEFT_WALL_ID] < sensors_distance[SENSOR_FRONT_RIGHT_WALL_ID]; 528 | 529 | if (left_correction && sensors_distance[SENSOR_FRONT_LEFT_WALL_ID] < 320) { 530 | return -(sensors_distance[SENSOR_FRONT_LEFT_WALL_ID] - 320); 531 | } else if (!left_correction && sensors_distance[SENSOR_FRONT_RIGHT_WALL_ID] < 320) { 532 | return sensors_distance[SENSOR_FRONT_RIGHT_WALL_ID] - 320; 533 | } 534 | return 0; 535 | } 536 | 537 | int16_t get_front_sensors_angle_error(void) { 538 | if (!front_wall_detection()) { 539 | last_front_sensors_angle_error = 0; 540 | return 0; 541 | } 542 | int16_t error = sensors_distance[SENSOR_FRONT_LEFT_WALL_ID] - sensors_distance[SENSOR_FRONT_RIGHT_WALL_ID]; 543 | // error = 0.1 * error + (1 - 0.1) * last_front_sensors_angle_error; 544 | // last_front_sensors_angle_error = error; 545 | return error; 546 | } 547 | 548 | int16_t get_front_sensors_diagonal_error(void) { 549 | int16_t left_error = sensors_distance[SENSOR_FRONT_LEFT_WALL_ID] - 320; 550 | int16_t right_error = sensors_distance[SENSOR_FRONT_RIGHT_WALL_ID] - 320; 551 | // printf("\t\t%4d - %4d\n", left_error, right_error); 552 | 553 | if (right_error < 0) { 554 | return right_error; 555 | } 556 | if (left_error < 0) { 557 | return -left_error; 558 | } 559 | 560 | return 0; 561 | } 562 | -------------------------------------------------------------------------------- /source_code/src/setup.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static enum RC5_TRIGGER rc5_trigger = RC5_TRIGGER_FALLING; 4 | 5 | /** 6 | * @brief Configura los relojes principales del robot 7 | * 8 | * SYSCLK a 168 MHz 9 | * GPIO A, B, C 10 | * USART3 11 | * SPI3 12 | * DMA 13 | * Timers 14 | * ADC 15 | * DWT 16 | * 17 | */ 18 | static void setup_clock(void) { 19 | 20 | rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_168MHZ]); 21 | 22 | rcc_periph_clock_enable(RCC_GPIOA); 23 | rcc_periph_clock_enable(RCC_GPIOB); 24 | rcc_periph_clock_enable(RCC_GPIOC); 25 | 26 | rcc_periph_clock_enable(RCC_SYSCFG); 27 | 28 | rcc_periph_clock_enable(RCC_USART3); 29 | 30 | rcc_periph_clock_enable(RCC_SPI3); 31 | 32 | rcc_periph_clock_enable(RCC_TIM1); 33 | rcc_periph_clock_enable(RCC_TIM2); 34 | rcc_periph_clock_enable(RCC_TIM3); 35 | rcc_periph_clock_enable(RCC_TIM4); 36 | rcc_periph_clock_enable(RCC_TIM5); 37 | rcc_periph_clock_enable(RCC_TIM8); 38 | 39 | rcc_periph_clock_enable(RCC_DMA1); 40 | rcc_periph_clock_enable(RCC_DMA2); 41 | 42 | rcc_periph_clock_enable(RCC_ADC1); 43 | rcc_periph_clock_enable(RCC_ADC2); 44 | 45 | dwt_enable_cycle_counter(); 46 | } 47 | 48 | /** 49 | * @brief Configura el SysTick para 1ms 50 | * 51 | */ 52 | static void setup_systick(void) { 53 | systick_set_frequency(SYSTICK_FREQUENCY_HZ, SYSCLK_FREQUENCY_HZ); 54 | systick_counter_enable(); 55 | systick_interrupt_enable(); 56 | } 57 | 58 | /** 59 | * @brief Configura las prioridades de Timers para evitar bloqueos 60 | * 61 | */ 62 | static void setup_timer_priorities(void) { 63 | nvic_set_priority(NVIC_TIM2_IRQ, 16 * 0); 64 | nvic_set_priority(NVIC_SYSTICK_IRQ, 16 * 1); 65 | nvic_set_priority(NVIC_DMA2_STREAM0_IRQ, 16 * 2); 66 | nvic_set_priority(NVIC_TIM5_IRQ, 16 * 3); 67 | nvic_set_priority(NVIC_USART3_IRQ, 16 * 4); 68 | 69 | nvic_enable_irq(NVIC_TIM5_IRQ); 70 | nvic_enable_irq(NVIC_TIM2_IRQ); 71 | nvic_enable_irq(NVIC_USART3_IRQ); 72 | nvic_enable_irq(NVIC_DMA2_STREAM0_IRQ); 73 | nvic_enable_irq(NVIC_EXTI15_10_IRQ); 74 | } 75 | 76 | /** 77 | * @brief Configura el USART3 para comunicacion Serial 78 | * 79 | */ 80 | static void setup_usart(void) { 81 | usart_set_baudrate(USART3, 115200); 82 | usart_set_databits(USART3, 8); 83 | usart_set_stopbits(USART3, USART_STOPBITS_1); 84 | usart_set_parity(USART3, USART_PARITY_NONE); 85 | usart_set_flow_control(USART3, USART_FLOWCONTROL_NONE); 86 | usart_set_mode(USART3, USART_MODE_TX_RX); 87 | // USART_CR1(USART3) |= USART_CR1_RXNEIE; 88 | // usart_enable_tx_interrupt(USART3); 89 | usart_enable(USART3); 90 | } 91 | 92 | /** 93 | * @brief Configura los puertos GPIO 94 | * 95 | */ 96 | static void setup_gpio(void) { 97 | // Entrada analógica menú 98 | gpio_mode_setup(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO7); 99 | 100 | // Entradas analógicas sensores 101 | gpio_mode_setup(GPIOC, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0 | GPIO1 | GPIO2 | GPIO3); 102 | 103 | // Salidas digitales sensores 104 | gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO0 | GPIO1 | GPIO2 | GPIO3); 105 | gpio_clear(GPIOA, GPIO0 | GPIO1 | GPIO2 | GPIO3); 106 | 107 | // Entrada analógica sensor de batería 108 | gpio_mode_setup(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO4); 109 | 110 | // Entrada analógica sensor de corriente de motores 111 | gpio_mode_setup(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO5 | GPIO6); 112 | 113 | // Entradas Encoders 114 | gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO4 | GPIO5 | GPIO6 | GPIO7); 115 | gpio_set_af(GPIOB, GPIO_AF2, GPIO4 | GPIO5 | GPIO6 | GPIO7); 116 | gpio_mode_setup(GPIOB, GPIO_MODE_INPUT, GPIO_PUPD_PULLDOWN, GPIO3); 117 | gpio_mode_setup(GPIOD, GPIO_MODE_INPUT, GPIO_PUPD_PULLDOWN, GPIO2); 118 | 119 | // Salida digital LED auxiliar 120 | gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO14); 121 | 122 | // Salida digital PIN auxiliar 123 | gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO13); 124 | 125 | // Salidas digitales LEDs ventilador 126 | gpio_mode_setup(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO4 | GPIO5); 127 | gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO0 | GPIO1 | GPIO2); 128 | gpio_mode_setup(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO15 | GPIO14 | GPIO13); 129 | gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO9 | GPIO8); 130 | 131 | // Salida PWM LEDS 132 | gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9 | GPIO10 | GPIO11); 133 | gpio_set_af(GPIOA, GPIO_AF1, GPIO9 | GPIO10 | GPIO11); 134 | 135 | // Salida PWM Motores 136 | gpio_mode_setup(GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO6 | GPIO7 | GPIO8 | GPIO9); 137 | gpio_set_af(GPIOC, GPIO_AF3, GPIO6 | GPIO7 | GPIO8 | GPIO9); 138 | gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO15); 139 | 140 | // Salida PWM Succión 141 | gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO8); 142 | gpio_set_af(GPIOA, GPIO_AF1, GPIO8); 143 | 144 | // USART3 145 | gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO10 | GPIO11); 146 | gpio_set_af(GPIOB, GPIO_AF7, GPIO10 | GPIO11); 147 | 148 | // MPU 149 | gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO15); 150 | gpio_set(GPIOA, GPIO15); 151 | gpio_mode_setup(GPIOC, GPIO_MODE_AF, GPIO_PUPD_PULLDOWN, GPIO10 | GPIO11 | GPIO12); 152 | gpio_set_af(GPIOC, GPIO_AF6, GPIO10 | GPIO11 | GPIO12); 153 | 154 | // IR 38kHz 155 | exti_select_source(EXTI12, GPIOA); 156 | rc5_trigger = RC5_TRIGGER_FALLING; 157 | exti_set_trigger(EXTI12, EXTI_TRIGGER_FALLING); 158 | exti_enable_request(EXTI12); 159 | } 160 | 161 | void exti15_10_isr(void) { 162 | exti_reset_request(EXTI12); 163 | rc5_register(rc5_trigger); 164 | switch (rc5_trigger) { 165 | case RC5_TRIGGER_FALLING: 166 | rc5_trigger = RC5_TRIGGER_RISING; 167 | exti_set_trigger(EXTI12, EXTI_TRIGGER_RISING); 168 | break; 169 | case RC5_TRIGGER_RISING: 170 | rc5_trigger = RC5_TRIGGER_FALLING; 171 | exti_set_trigger(EXTI12, EXTI_TRIGGER_FALLING); 172 | break; 173 | } 174 | } 175 | 176 | /** 177 | * @brief Configura el ADC1 para lectura de sensores individualmente a partir de un trigger por software 178 | * 179 | */ 180 | static void setup_adc2(void) { 181 | 182 | adc_power_off(ADC2); 183 | adc_enable_scan_mode(ADC2); 184 | adc_set_single_conversion_mode(ADC2); 185 | adc_enable_external_trigger_injected(ADC2, ADC_CR2_JSWSTART, ADC_CR2_JEXTEN_RISING_EDGE); 186 | adc_set_right_aligned(ADC2); 187 | adc_set_sample_time_on_all_channels(ADC2, ADC_SMPR_SMP_15CYC); 188 | adc_set_injected_sequence( 189 | ADC2, sizeof(get_sensors()) / sizeof(get_sensors()[0]), 190 | get_sensors()); 191 | adc_power_on(ADC2); 192 | } 193 | 194 | /** 195 | * @brief Configura el ADC2 para lectura del sensor de batería 196 | * 197 | */ 198 | static void setup_adc1(void) { 199 | adc_power_off(ADC1); 200 | adc_disable_external_trigger_regular(ADC1); 201 | adc_set_resolution(ADC1, ADC_CR1_RES_12BIT); 202 | adc_set_right_aligned(ADC1); 203 | // adc_set_clk_prescale(ADC_CCR_ADCPRE_BY2); 204 | adc_set_sample_time_on_all_channels(ADC1, ADC_SMPR_SMP_15CYC); 205 | adc_enable_scan_mode(ADC1); 206 | 207 | adc_set_regular_sequence(ADC1, get_aux_adc_channels_num(), get_aux_adc_channels()); 208 | adc_set_continuous_conversion_mode(ADC1); 209 | adc_enable_eoc_interrupt(ADC1); 210 | 211 | adc_power_on(ADC1); 212 | int i; 213 | for (i = 0; i < 800000; i++) { 214 | /* Wait a bit. */ 215 | __asm__("nop"); 216 | } 217 | 218 | adc_start_conversion_regular(ADC1); 219 | } 220 | 221 | static void setup_dma_adc1(void) { 222 | rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_ADC1EN); 223 | rcc_peripheral_enable_clock(&RCC_AHB1ENR, RCC_AHB1ENR_DMA2EN); 224 | dma_stream_reset(DMA2, DMA_STREAM4); 225 | 226 | dma_set_peripheral_address(DMA2, DMA_STREAM4, (uint32_t)&ADC_DR(ADC1)); 227 | dma_set_memory_address(DMA2, DMA_STREAM4, (uint32_t)get_aux_adc_raw()); 228 | dma_set_number_of_data(DMA2, DMA_STREAM4, get_aux_adc_channels_num()); 229 | dma_enable_memory_increment_mode(DMA2, DMA_STREAM4); 230 | dma_set_peripheral_size(DMA2, DMA_STREAM4, DMA_SxCR_PSIZE_16BIT); 231 | dma_set_memory_size(DMA2, DMA_STREAM4, DMA_SxCR_MSIZE_16BIT); 232 | dma_set_priority(DMA2, DMA_STREAM4, DMA_SxCR_PL_LOW); 233 | 234 | dma_enable_transfer_complete_interrupt(DMA2, DMA_STREAM4); 235 | // dma_enable_half_transfer_interrupt(DMA2, DMA_STREAM4); 236 | // dma_set_number_of_data(DMA2, DMA_STREAM4, get_aux_adc_channels_num()); 237 | dma_enable_circular_mode(DMA2, DMA_STREAM4); 238 | dma_set_transfer_mode(DMA2, DMA_STREAM4, DMA_SxCR_DIR_PERIPHERAL_TO_MEM); 239 | dma_channel_select(DMA2, DMA_STREAM4, DMA_SxCR_CHSEL_0); 240 | 241 | dma_enable_stream(DMA2, DMA_STREAM4); 242 | adc_enable_dma(ADC1); 243 | adc_set_dma_continue(ADC1); 244 | } 245 | 246 | /** 247 | * @brief Configura el TIM1 para manejar el el PWM del LED RGB y el Ventilador a 20kHz 248 | * 249 | */ 250 | static void setup_leds_pwm(void) { 251 | timer_set_mode(TIM1, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); 252 | 253 | timer_set_prescaler(TIM1, ((rcc_apb2_frequency * 2) / 20000000 - 1)); // 20kHz 254 | timer_set_repetition_counter(TIM1, 0); 255 | timer_enable_preload(TIM1); 256 | timer_continuous_mode(TIM1); 257 | timer_set_period(TIM1, LEDS_MAX_PWM); 258 | 259 | timer_set_oc_mode(TIM1, TIM_OC1, TIM_OCM_PWM1); 260 | timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_PWM1); 261 | timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_PWM1); 262 | timer_set_oc_mode(TIM1, TIM_OC4, TIM_OCM_PWM1); 263 | timer_set_oc_value(TIM1, TIM_OC1, 0); 264 | timer_set_oc_value(TIM1, TIM_OC2, 0); 265 | timer_set_oc_value(TIM1, TIM_OC3, 0); 266 | timer_set_oc_value(TIM1, TIM_OC4, 0); 267 | timer_enable_oc_output(TIM1, TIM_OC1); 268 | timer_enable_oc_output(TIM1, TIM_OC2); 269 | timer_enable_oc_output(TIM1, TIM_OC3); 270 | timer_enable_oc_output(TIM1, TIM_OC4); 271 | 272 | timer_enable_break_main_output(TIM1); 273 | 274 | timer_enable_counter(TIM1); 275 | } 276 | 277 | /** 278 | * @brief Configura el TIM8 para manejar el PWM de los Motores, a 20kHz 279 | * 280 | */ 281 | static void setup_motors_pwm(void) { 282 | timer_set_mode(TIM8, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); 283 | 284 | timer_set_prescaler(TIM8, rcc_apb2_frequency * 2 / 20000000 - 1); // 20kHz 285 | timer_set_repetition_counter(TIM8, 0); 286 | timer_enable_preload(TIM8); 287 | timer_continuous_mode(TIM8); 288 | timer_set_period(TIM8, MOTORES_MAX_PWM); 289 | 290 | timer_set_oc_mode(TIM8, TIM_OC1, TIM_OCM_PWM1); 291 | timer_set_oc_mode(TIM8, TIM_OC2, TIM_OCM_PWM1); 292 | timer_set_oc_mode(TIM8, TIM_OC3, TIM_OCM_PWM1); 293 | timer_set_oc_mode(TIM8, TIM_OC4, TIM_OCM_PWM1); 294 | timer_set_oc_value(TIM8, TIM_OC1, 0); 295 | timer_set_oc_value(TIM8, TIM_OC2, 0); 296 | timer_set_oc_value(TIM8, TIM_OC3, 0); 297 | timer_set_oc_value(TIM8, TIM_OC4, 0); 298 | timer_enable_oc_output(TIM8, TIM_OC1); 299 | timer_enable_oc_output(TIM8, TIM_OC2); 300 | timer_enable_oc_output(TIM8, TIM_OC3); 301 | timer_enable_oc_output(TIM8, TIM_OC4); 302 | 303 | timer_enable_break_main_output(TIM8); 304 | 305 | timer_enable_counter(TIM8); 306 | } 307 | 308 | /** 309 | * @brief Configura el TIM5 como ISR para ejecutrase cada 1ms. 310 | * Esta función ISR será la que contenga la gestión del control del robot 311 | * 312 | */ 313 | static void setup_main_loop_timer(void) { 314 | rcc_periph_reset_pulse(RST_TIM5); 315 | timer_set_mode(TIM5, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); 316 | timer_set_prescaler(TIM5, ((rcc_apb1_frequency * 2) / 1000000 - 1)); 317 | timer_disable_preload(TIM5); 318 | timer_continuous_mode(TIM5); 319 | timer_set_period(TIM5, 996); 320 | 321 | timer_enable_counter(TIM5); 322 | // El timer se iniciará en el arranque 323 | timer_enable_irq(TIM5, TIM_DIER_CC1IE); 324 | } 325 | 326 | /** 327 | * @brief Función de uso interno que lanza el TIM5 328 | * 329 | */ 330 | void tim5_isr(void) { 331 | if (timer_get_flag(TIM5, TIM_SR_CC1IF)) { 332 | timer_clear_flag(TIM5, TIM_SR_CC1IF); 333 | 334 | control_loop(); 335 | } 336 | } 337 | 338 | /** 339 | * @brief Configura el TIM2 como ISR para ejecutarse 16 veces cada 1ms. 340 | * Esta función ISR será la que maneje la lectura de sensores y el encendido/apagado de los emisores 341 | * 342 | */ 343 | static void setup_wall_sensor_manager(void) { 344 | rcc_periph_reset_pulse(RST_TIM2); 345 | timer_set_mode(TIM2, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); 346 | timer_set_prescaler(TIM2, ((rcc_apb1_frequency * 2) / 16000000 - 1)); // 16.66 ~16kHz 347 | timer_disable_preload(TIM2); 348 | timer_continuous_mode(TIM2); 349 | timer_set_period(TIM2, 520); // 16khz 350 | 351 | timer_enable_counter(TIM2); 352 | timer_enable_irq(TIM2, TIM_DIER_CC1IE); 353 | } 354 | 355 | /** 356 | * @brief Función de uso interno que lanza el TIM2 357 | * 358 | */ 359 | void tim2_isr(void) { 360 | if (timer_get_flag(TIM2, TIM_SR_CC1IF)) { 361 | timer_clear_flag(TIM2, TIM_SR_CC1IF); 362 | 363 | // gpio_toggle(GPIOB, GPIO13); 364 | sm_emitter_adc(); 365 | } 366 | } 367 | 368 | /** 369 | * @brief Configura los TIM3 y TIM4 para lectura en quadratura de encoders. 370 | * 371 | */ 372 | static void setup_quadrature_encoders(void) { 373 | timer_set_period(TIM4, 0xFFFF); 374 | timer_slave_set_mode(TIM4, TIM_SMCR_SMS_EM3); 375 | timer_ic_set_input(TIM4, TIM_IC1, TIM_IC_IN_TI1); 376 | timer_ic_set_input(TIM4, TIM_IC2, TIM_IC_IN_TI2); 377 | timer_enable_counter(TIM4); 378 | 379 | timer_set_period(TIM3, 0xFFFF); 380 | timer_slave_set_mode(TIM3, TIM_SMCR_SMS_EM3); 381 | timer_ic_set_input(TIM3, TIM_IC1, TIM_IC_IN_TI1); 382 | timer_ic_set_input(TIM3, TIM_IC2, TIM_IC_IN_TI2); 383 | timer_enable_counter(TIM3); 384 | } 385 | 386 | /** 387 | * @brief Setup SPI. 388 | * 389 | * SPI is configured as follows: 390 | * 391 | * - Master mode. 392 | * - Clock baud rate: PCLK1 / speed_div; PCLK1 = 36MHz. 393 | * - Clock polarity: 0 (idle low; leading edge is a rising edge). 394 | * - Clock phase: 0 (out changes on the trailing edge and input data 395 | * captured on rising edge). 396 | * - Data frame format: 8-bits. 397 | * - Frame format: MSB first. 398 | * 399 | * NSS is configured to be managed by software. 400 | * 401 | * Reference: https://github.com/Bulebots/meiga 402 | */ 403 | static void setup_spi(uint8_t speed_div) { 404 | spi_reset(SPI3); 405 | 406 | spi_init_master(SPI3, speed_div, SPI_CR1_CPOL_CLK_TO_0_WHEN_IDLE, 407 | SPI_CR1_CPHA_CLK_TRANSITION_1, SPI_CR1_DFF_8BIT, SPI_CR1_MSBFIRST); 408 | 409 | spi_enable_software_slave_management(SPI3); 410 | spi_set_nss_high(SPI3); 411 | 412 | spi_enable(SPI3); 413 | } 414 | 415 | static void setup_mpu(void) { 416 | setup_spi_high_speed(); 417 | lsm6dsr_init(); 418 | } 419 | 420 | /** 421 | * @brief Setup SPI for gyroscope read, less than 20 MHz. 422 | * 423 | * The clock baudrate is 84 MHz / 8 = 10.5 MHz. 424 | * 425 | * Reference: https://github.com/Bulebots/meiga 426 | */ 427 | void setup_spi_high_speed(void) { 428 | setup_spi(SPI_CR1_BAUDRATE_FPCLK_DIV_8); 429 | delay(100); 430 | } 431 | 432 | /** 433 | * @brief Setup SPI for gyroscope Write, less than 1 MHz. 434 | * 435 | * The clock baudrate is 84 MHz / 128 = 0.65625 MHz. 436 | * 437 | * Reference: https://github.com/Bulebots/meiga 438 | */ 439 | void setup_spi_low_speed(void) { 440 | setup_spi(SPI_CR1_BAUDRATE_FPCLK_DIV_128); 441 | delay(100); 442 | } 443 | 444 | /** 445 | * @brief Inicialización y configuración de todos los componentes del robot 446 | * 447 | */ 448 | void setup(void) { 449 | setup_clock(); 450 | setup_timer_priorities(); 451 | setup_gpio(); 452 | setup_usart(); 453 | setup_adc2(); 454 | setup_dma_adc1(); 455 | setup_adc1(); 456 | setup_leds_pwm(); 457 | setup_motors_pwm(); 458 | setup_main_loop_timer(); 459 | setup_wall_sensor_manager(); 460 | setup_quadrature_encoders(); 461 | setup_systick(); 462 | setup_mpu(); 463 | } -------------------------------------------------------------------------------- /source_code/src/timetrial.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void timetrial_start(void) { 4 | configure_kinematics(menu_run_get_speed()); 5 | clear_info_leds(); 6 | set_RGB_color(0, 0, 0); 7 | set_target_fan_speed(get_kinematics().fan_speed, 400); 8 | delay(500); 9 | } 10 | 11 | void timetrial_loop(void) { 12 | run_straight(CELL_DIMENSION * (maze_get_rows() - 1) - (ROBOT_BACK_LENGTH + WALL_WIDTH / 2.0f), 13 | ROBOT_BACK_LENGTH + WALL_WIDTH / 2.0f, 14 | get_kinematics().turns[MOVE_RIGHT_90].start, 15 | maze_get_rows() - 1, 16 | true, 17 | get_kinematics().linear_speed, 18 | get_kinematics().turns[MOVE_RIGHT_90].linear_speed); 19 | 20 | run_side(MOVE_RIGHT_90, get_kinematics().turns[MOVE_RIGHT_90]); 21 | 22 | run_straight(CELL_DIMENSION * (maze_get_columns() - 2), 23 | get_kinematics().turns[MOVE_RIGHT_90].end, 24 | get_kinematics().turns[MOVE_RIGHT_90].start, 25 | maze_get_columns() - 2, 26 | false, 27 | get_kinematics().linear_speed, 28 | get_kinematics().turns[MOVE_RIGHT_90].linear_speed); 29 | 30 | run_side(MOVE_RIGHT_90, get_kinematics().turns[MOVE_RIGHT_90]); 31 | 32 | run_straight(CELL_DIMENSION * (maze_get_rows() - 2), 33 | get_kinematics().turns[MOVE_RIGHT_90].end, 34 | get_kinematics().turns[MOVE_RIGHT_90].start, 35 | maze_get_rows() - 2, 36 | false, 37 | get_kinematics().linear_speed, 38 | get_kinematics().turns[MOVE_RIGHT_90].linear_speed); 39 | 40 | run_side(MOVE_RIGHT_90, get_kinematics().turns[MOVE_RIGHT_90]); 41 | 42 | run_straight(CELL_DIMENSION * (maze_get_columns() - 2), 43 | get_kinematics().turns[MOVE_RIGHT_90].end, 44 | get_kinematics().turns[MOVE_RIGHT_90].start, 45 | maze_get_columns() - 2, 46 | false, 47 | get_kinematics().linear_speed, 48 | get_kinematics().turns[MOVE_RIGHT_90].linear_speed); 49 | 50 | run_side(MOVE_RIGHT_90, get_kinematics().turns[MOVE_RIGHT_90]); 51 | 52 | move(MOVE_BACK_STOP); 53 | set_race_started(false); 54 | } -------------------------------------------------------------------------------- /source_code/src/usart.c: -------------------------------------------------------------------------------- 1 | #include "usart.h" 2 | 3 | int _write(int file, char *ptr, int len) 4 | { 5 | int i = 0; 6 | 7 | /* 8 | * Write "len" of char from "ptr" to file id "file" 9 | * Return number of char written. 10 | * 11 | * Only work for STDOUT, STDIN, and STDERR 12 | */ 13 | if (file > 2) { 14 | return -1; 15 | } 16 | while (*ptr && (i < len)) { 17 | usart_send_blocking(USART3, *ptr); 18 | // usart_send(USART3, *ptr); 19 | // if (*ptr == '\n') { 20 | // usart_send_blocking(USART3, '\r'); 21 | // } 22 | i++; 23 | ptr++; 24 | } 25 | return i; 26 | } -------------------------------------------------------------------------------- /source_code/src/utils.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | float map(float x, float in_min, float in_max, float out_min, float out_max) { 4 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 5 | } 6 | 7 | float constrain(float x, float min, float max) { 8 | if (x > max) { 9 | return max; 10 | } else if (x < min) { 11 | return min; 12 | } else { 13 | return x; 14 | } 15 | } 16 | --------------------------------------------------------------------------------