Control code for robot of team Galaxia 5987 in memory of David Zohar, Genesis, playing in the 2019 FIRST Robotics Competition season, Destination: Deep Space. Genesis will compete at FIRST Israel District Events #1 and #4 and in FIRST Championship (Detroit).

This repository holds the control code of Genesis' subsystems written in Java. The vision recognition code is stored here.

• Drivetrain: Six-whell WCD with a Pure Pursuit alogrithm to follow paths.
• Elevator: Two-staged Elevator controlled by a motion profile using the Talon SRX's "Motion Magic" control mode. The Elevator has preset heights: Rocket Levels 1/2/3 (for Hatch Panels and for Cargo), Cargo Ship, Loading Station and ground.
• Hatch Panels: Pneumatic system holding the Hatch Panels.
• Cargo Gripper: Wristed Gripper holding the Cargo. The Gripper has preset angles controlled by a motion profile to collect Cargo from the ground and score it in the Cargo Ship or the Rocket.
• Climbing: Four-legged mechanism raising Genesis to Habitat Level 2 or 3. Uses rotation matrices to stay leveled and stable.

WIP: Auto adjustment to the Cargo Ship, Rocket and Loading Station. Genesis uses two Intel RealSense D435 depth cameras to calculate distances and angles from the vision targets. Genesis uses edge detection to find Cargo and a neural network to find Hatch Panels. If the Operator chooses to score in the Cargo Ship, Genesis chooses by itself which Bay in the Cargo Ship to go to.