image/svg+xml
Teensy code
SETUP:
Define all ports and create variables
-Start serial connection with OpenMV canmera and with a posibly connected computer
-Start I2C connection with the port multiplexers
-Start serial connection with OpenMV canmera and with a posibly connected computer
-Connect unconected pins to ground
-Configure IR sensors
LOOP
collect sensor data
-gather IR sensor data
-select color sensor N and gather its extract color from data
6x
-collect location of the goal from openMV
-Read compas data
Print all data to serial port for debuging.
Do the color sensors see a line?
YES
NO
Move back.
Is the robot in posesion of the ball?
YES
NO
Do we see the ball?
NO
YES
move towards the ball
Have we seen the ball in the last 10 loops?
NO
Start spinning whilst moving on the field
Is our front aimed towards the goal?
YES
NO
Turn towards the goal.
Move towards the goal.
YES
Continue last direction.