We coded a 6 motor drive internal sensor in block programming and it is spinning in circles
I would recommend leveraging the print function to help you troubleshoot and understand what the brain is trying to do. Print all relevant info on the screen including your desired heading, inertial reading, and error correction from your P-loop stuff. I imagine the error is going out of site and unable to settle.
You need to know what happens when your heading crosses zero. Does it go to -1 or does it go to 364, how does this affect your algorithm?
Also I would recommend lifting the wheels off the ground so you can see which direction they are moving when receiving a code and see if you can (in the air) twist the robot to a heading where the wheels seem to be working together instead of against each other.
I am taking for granted that you have ‘forward’ and ‘reverse’ properly defined in your setup and that all motors are working as intended and not backwards due to setup or gearing.
1 Like