Made control system serial:
At first, the control system was parallel, in the sense that it would try and move all actuators in the same iteration.
Now, each actuator is taken to its final position one by one.
Error Fixing and stabilization issues:
Corrected the following issues:
1. The Autobed was getting a [0,0,0] input at start. Was therefore moving to zero.
Solved by taking current sensor output and assigning it to the input to the autobed during initialization. Also the reached_destination boolean array is made True by default.
2. The Autobed did not take inputs after the first time.
Fixed by changing reached_destination.all() to reached_destination.any()
Made stabilization changes:
Code written earlier was causing errors due to the .any added. Reverted to .all and added an actuator_number variable.
Cleaned up an initialization of actuator_number that wasn't needed