After some time without Mindstorms I have updated my H25 project. It can be used to move two LEGO rims between three positions as seen in this video. You might want to use it as starting point for your own solution. Feel free to post improvements (and your code) in the comments!
I think the programm is too large to simply post it as a screenshot – You can download the project file here.
I have added comments for each step so it should not be too complicated to understand what each part is responsible for.
This part of the code moves the arm in the “start” position. I use the push button to detect when the arm has reached the outmost right position. The light sensor is used to detect when tha arm is raised. Here you might need to play with the values or fix a little sheet of paper on the brick in case not enough light is reflected. The gripper does not have a sensor. Therefore I close it with low force and simply assume it has reached the “closed” position before I open it.
In the second part I store the movement sequence in arrays.
All the movement is controlled in the third part – the outer and inner loop.
I use the inner loop´s counter as index to get the next value from the arrays that store the movement sequence.
The values stored in the array control how many times we loop through the inner loop (e.g. you store three values – three loops).
After the code has reached the maximum number of loops the program exits the inner loop. This “resets” the inner loop´s counter.
The outer loop directs back to the inner loop. Since the counter was reset it starts with “0” and we can iterate through the arrays (and the movement sequence) anew.