-Feel like the robot is going the same way (around 30 degrees on my phone) because running it consecutively showed that it went straight that way instead of turning
right like usual
~Might be the heading code newly put into
the drive loop
-Inputed debugging strings to output heading and distance. The heading values in raw
heading (IMUModule) and heading in testFindCone are different.
~The heading Magellan should have is around 278 according to IMU on robot. The heading
went from 23X to 4X, so it seems that this heading value and PID toward it
does not accomplish anything. I believe the turning/oscilation exhibited was due to
each new heading extracted from being in mid-turn. In the beginning however, the
robot is turning in the direction the cone is at. Once overshot, the robot
just oscilates into a straight line.
-Debug print of coneDirection showed that coneDirection heading was 0, which is why
Magellan goes in the same direction away from the cone.
~After consulting robert, we decided to have the robot find which direction the cone was
at, turn toward it's current heading +- 20 depending on the direction until it sees
the cone again via peaking at the camera. If the robot sees the cone, it will change
the difference in heading to 5 until the cone is centered on the screen. We will
reuse much of Ashuai's code from RobotControlBuilder2, which facilitates this.
-First and second run showed odd behavior, with much studdering with which direction
~Need debugging print values to see
what is wrong
-Jason noted that instead of "coneDistance" we should be using "coneDirection" (typo).
Behavior was twitchy, adjustHeading was not being updated, but when the robot saw
the cone, it moved forward and the rest of the code was able to bump it.
~Need to debug twitchyness
-Observed that the robot went the opposite way and turned right when the cone was
~The coneDirection was being modified oppositely by
adjustHeading by adding positive values and subtracting negative
values (hence, going right instead of left).