WIP: Gyro Climb

[roshanr10]

THIS HAS NOT BEEN TESTED ON THE ROBOT AND SHOULD NOT BE MERGED UNTIL COMPLETED

Overall, a basic P controller for gyro climb – still need to figure out gyro input for climbing level... NavX is @ ~30º from normal, possibly need some vector math.

Just pushing up so people are aware this iteration exists and we can build on this if desired.

[derrell]

"NavX is @ ~30º from normal" should be in a comment in the code.

[roshanr10]

@derrell not even close, the logic seems fine but we need to figure out how we obtain the proper gyro value and the corresponding P value. hence the WIP, sorry if that wasn't more clear, I'll update the message to reflect this better

I'm about to create subtasks, @djwalker200 said he'll take a look at the vector math if we need to.

we're considering many potential options at this point, I'll put them all up here in the next hour or so.

[roshanr10]

@TechplexEngineer and I tried getting angles yesterday but we're seeing movement on two gyro axes when we move in one axis (from our perception) so we need to figure out how to use that output if we're going with NavX. I have another idea as well, hang tight on the issues

[derrell]

Ok. Keep in mind that the documentation for navx says that we can't do what we're trying to do. It is very clear that the navx must be mounted horizontally or vertically, and not at an angle. I know that you'd said that you tested and were getting good values from the axes, but this ssue of movement on two axes may in fact be as a result of the angled mounting of the navx.

[roshanr10]

@derrell I'm aware that the documentation says that we can't but remounting that is a major design concern, so we'll try briefly and drop it otherwise. I have an alternate idea in mind as well that might mitigate other concerns with the gyro PID as well– I'm in the process of typing up issues