To drive the robot and achieve a self-balancing with torque, there needs to be involved some contact or friction between the wheels and the ground.
At first however I wanted to check what would happen if I just changed from giving a force to the robot to move uni-directionally, to applying actual torque.
Step 1 was to move from giving an input force to the prismatic joint (as seen below):
And give from the PID an input of torque directly into the revolute joint, to see if the wheels rotated or simply to check what would happen:
The resulting system would be simplified by trying to group blocks into subsystems (central subsytem containing the revolute joint, prismatic joint and their correspective components, while to troubleshoot and debug I have used a scope purely for Torque, an Output Graph for the Error, and some other sampling blocks with displays to quickly visually check for the peaks):
In the desired angle I have put a 10 Deg value and I have obtained the following result (1/8x speed):
The output graph for the torque is the following (meaning the robot balances accoringly within less than 0.1 seconds and then the actual torque goes to stay at 0 even if the robot keeps a certain inclination):
The robot doesn't move its wheels, it does self-balance inclination-wise but not position-wise, because it is supposed to continue moving in one direction. However since I assumed at the beginning that there should be some form of friction or contact between wheels and ground (in this case ground is inexistent) I had to implement something differently and make the wheels rotate. Maybe trying to do something different than just playing with prismatic joint and revolute joint would get me closer to a more realistic simulation.
Next: I could try to see how I can create some contact between wheels and ground.