Project lab 9 - Coding Distributor and Feeder construction

Posted on 9:49 AM by Unknown
Date: 12/06/2013
Duration of activity: 09:00-23:30 --> 14 hours and 30 minutes
Group members participating: Ole Rasmussen and Kasper Føns

1. Goal

Figure 1: The finished distributor construction. It has two axes of motion; the first axis (x) is the movement of the bridge left or right (from the point of view of the car). The second axis (y) is the car itself that drives forward and backward.

Having completed the physical construction of the distributor (see Figure 1) the only thing left is the software. One goal of this project lab is to program the distributor.
Since our Sorter and Loader systems have been completed, we also have the time to try to get the Feeder subsystem up and working. Since we are under time pressure, we have dropped the requirement for it to handle both axles and beams, so that it only needs to handle beams now.

2. Plan

Our plan is to code the Distributor and construct the Feeder.

2.1 Distributor

  1. Identify optimal acceleration and speed values for the distributor
  2. Find out how to reset the distributor to its initial position
  3. Find a way to determine where the different Lego axles should be dropped in the axle and beam trays and code it

2.2 Feeder

First we will redesign the elevator design since it was far from stable enough.

Then we will need to make sure only one beam is transported through the system and that the beam is positioned so that the BeamCounter can count the number of holes. At the end of the day we will test the complete system to make sure it works for the demonstration tomorrow.

3. Experiment: acceleration and speed

The goal of this experiment is to determine at which acceleration and speed the distributor moves the fastest while still being precise.

3.1 RegulatedMotor: recap

Before doing the actual experiment let's remind ourselves how the distributor is meant to work. Just like with the car tray we decided that the distributor should be controlled by using the RegulatedMotor class of leJOS [1]. RegulatedMotor provides methods for controlling an NXT motor using closed-loop [2] PID-like regulation [3]. For example, the rotate(degrees) method will rotate a motor the specified amount of degrees and uses the built-in tachometer as sensor to double check the rotation. The setAcceleration and setSpeed methods can be used to adjust the speed and acceleration of the rotation. The tachometer is used to ensure that the motor rotates the required amount at the specified speed and acceleration. leJOS does this by regulating the power to the motor based on the error, where the error is the number of tacho degrees that the motor is currently off it's expected position. An example of this in practice is a car driving uphill. With the leJOS regulation the car will maintain it's speed regardless of the motor load (within a certain amount of course) so the car will drive just as fast uphill as it will downhill. We decided to use the RegulatedMotor for controlling the distributor, because the closed-loop tachometer control ensures the Lego pieces will be dropped of at (almost) exactly the positions we specify.

3.2 Setup

The experiment will be conduced similarly to the previous gearing experiment with the distributor car in project lab 5. The difference in this experiment is that we will measure variables for two different constructions; first we will experiment with the car only, and then we will experiment with the bridge (see Figure 1). The reason for this is that even though both are driven by one equally geared NXT motor the load on them are different. The car only has to drive itself, but the bridge has to drive itself plus the car which is more weight. Therefore, it is reasonable to assume that the car and bridge will need different values for speed and acceleration to drive smooth and fast.

Figure 2: The setup for the car experiment. The attached beam (red circle) moves with the car. By marking where it starts on the paper we can see how precise the car is by measuring how close it returns to the starting position after moving back and forth a few times.
Figure 3: The setup for the bridge experiment. The attached beam (red circle) moves with the bridge. By marking where it starts on the paper we can see how precise the bridge is by measuring how close it returns to the starting position after moving back and forth a few times.
The concrete setup for the experiments are shown in Figure 2 and 3. Figure 2 shows the setup for the car experiment and Figure 3 shows the setup for the bridge experiment. Common to both is the fact that we have attached a beam that moves along a paper on the ground. The experiment will, like the gearing experiment mentioned earlier, be conducted by marking the start position of the beam on the paper. Then we will tell the car/bridge to move a certain amount back and forth a number of times, and measure how far away from the mark on the paper the beam is when it returns. We repeat this with different values of acceleration and speed and note which of them is the most precise and fast. We will start with the highest values for acceleration and speed since they will provide the fastest traversal (see the initial values section). If they are not precise enough we will try with lower values until we reach a satisfactory point. Figure 4 shows the code used to perform the experiment.

 import lejos.robotics.RegulatedMotor;  
 import lejos.util.Delay;  
   
 public class SpeedVsPrecisionTest  
 {  
   private static final int MEASURED_MAX_SPEED_AT_MAX_VOLTAGE = 885;  
   private static final int MOTOR_DEFAULT_ACCEL = 6000;  
   private static final int TEST_TRACK_ANGLE_LEN = 360 * 3;  
   private static final int TEST_REPEATS = 3;  
   
   public static void run(RegulatedMotor motor)  
   {  
     test(motor, MEASURED_MAX_SPEED_AT_MAX_VOLTAGE, MOTOR_DEFAULT_ACCEL);  
     Delay.msDelay(5000);  
     test(motor, MEASURED_MAX_SPEED_AT_MAX_VOLTAGE, MOTOR_DEFAULT_ACCEL / 2);  
   }  
   
   private static void test(RegulatedMotor motor, int speed, int acceleration)  
   {  
     motor.setSpeed(speed);  
     motor.setAcceleration(acceleration);  
   
     for (int i = 0; i < TEST_REPEATS; i++)  
     {  
       motor.rotate(-TEST_TRACK_ANGLE_LEN);  
       motor.rotate(TEST_TRACK_ANGLE_LEN);  
     }  
   }  
 }
Figure 4: The code for this experiment. The run method runs a number of tests (2 here) and the test method  is responsible for one test - that is, setting the speed and acceleration and then rotating the motor forward and backward a number of times.

3.3 Initial values

We have used a speed value of 885 as the maximum speed possible. According to leJOS the maximum sustainable speed for a motor is dependent on the battery voltage (see Figure 5). As a result the maximum value that can ever be returned from getMaxSpeed must be on a battery with the largest possible voltage. We measured the value 885 on a fully charged new (white) NXT battery. It is thus reasonable to assume that this is the maximum speed value any NXT will return..

 public float getMaxSpeed() {  
     // It is generally assumed, that the maximum accurate speed of Motor is  
     // 100 degree/second * Voltage  
     return Battery.getVoltage() * 100.0f;  
   }
Figure 5: the leJOS implementation of NXTRegulatedMotor.getMaxSpeed mentions that the max speed is dependent on the battery voltage. One can only wonder why this information isn't in the method documentation.
One possible problem with the logic above is if the battery voltage doesn't always affect the motor power. Assume that we have a new battery that provides 10v when fully charged and an old battery that only provides 9v when fully charged. Also assume that the motor runs at full power (100) on the 10v battery. The problem arises if the motor also runs on full power with only 9v provided, in which case getMaxSpeed on the 10v battery will return one value x, but on the 9v battery it will return another different value y that's lower than on the 10v battery (per the calculation in Figure 5). Thus what we have now is two different max speeds: x, and y<x. In other words when we set the motor to speed x on the new battery it runs as fast as when we set it to speed y on the old battery.

It may not be easy to see why this may be a problem. To illustrate why let's say we have conducted an experiment on the 10v battery (with max speed of 885) and found out that 885-100=785 was a good value for the speed. What happens when we run the program on an NXT with a different (9v) battery? If the 9v battery has a max speed of 785 then the program will actually run the motor at full speed on the 9v battery, whereas it would run the motor with 100 speed less than max on the 10v battery. This is a obviously a problem since 785 speed on the 10v battery is NOT the same as 785 speed on the 9v battery with regarding to the actual speed of the motor.
In reality we're not sure this is an actual problem, and if it is we have no easy soltuions. For our project it's not directly relevant as the NXTs will be running on the same batteries.

Regarding the acceleration value of 6000 we have simply used this as it is the default value in the NXTRegulatedMotor class.

3.4 Results

3.4.1 Car

We ended up conducting two experiments for the car with the following values for speed and acceleration:
  1. Speed = 885, acceleration = 6000
  2. Speed = 885, acceleration = 3000
The first set of values are the default ones. Figure 5 and 6 show the experiments with these two value combinations. As can be seen in Figure 6, the car is more stable and smooth with an acceleration of 3000 without being significantly slower. Actually it was so smooth and fast that we decided we didn't need to conduct further experiments.

Result: with speed = 885 and acceleration = 3000 the car is both fast and stable while only being a tiny bit slower than with the maximum values of speed = 885 and acceleration = 6000. Thus these are the values we will be using for the car.
Figure 5: The car running with speed = 885, acceleration = 6000. It's not very smooth when stopping and starting.
Figure 6: The car running with speed = 885, acceleration = 3000. It's much more smooth than with acceleration  = 6000.

3.4.2 Bridge

Just like with the car we ended up conducting two experiments for the bridge with the following values for speed and acceleration:
  1. Speed = 885, acceleration = 6000
  2. Speed = 885, acceleration = 3000
The first set of values are the default ones. Figure 7 and 8 show the experiments with these two value combinations. As can be seen in Figure 8, the bridge is more stable and smooth with an acceleration of 3000 without being significantly slower. Actually it was so smooth and fast that we decided we didn't need to conduct further experiments.

Result: with speed = 885 and acceleration = 3000 the bridge is both fast and stable while only being a tiny bit slower than with the maximum values of speed = 885 and acceleration = 6000. Thus these are the values we will be using for the bridge. Interestingly enough these are the same optimal values we identified for the car. This presumably means there isn't as much difference between the car and bridge load as we originally assumed.
Figure 7: The bridge running with speed = 885, acceleration = 6000. It's not very smooth when stopping and starting.
Figure 8: The bridge running with speed = 885, acceleration = 3000. It's more smooth than with acceleration = 6000.

4. Resetting the distributor to its initial position

4.1 Choosing the initial position

Having determined the speed and acceleration of both the car and bridge, we turn to the question of what to do when the distributor is started (when it's NXT is turned on). The challenge in this scenario is that when the NXT is started, we have no idea where the distributor car or bridge is located on the tracks. What we want to do is to reset the car and the bridge to their initial position each time the distributor is started. The initial position will be where the distributor will pick up the next Lego piece, and therefore it needs to go to this position. We need to decide where the initial position is. Figure 9 shows a visualization of the initial position. Due to the fact that the motors are physically upside down in both the car and the bridge, when the motors are told to drive forward the car and bridge will actually drive backwards. We have defined the initial position as the corner where the car and track drives when they are bold told to drive forwards (which in physically is backwards). This position is marked with a blue circle in the figure. There is no special reason for choosing this corner other than that it made the most sense to have the initial position be the position that you have to drive back (backwards) to.

Figure 9: The initial position of the distibutor (blue circle) is defined as the corner of the tray where the car and bridge drives when both are set to drive forwards. Note that when the car and bridge motor drives forward the car and bridge actually moves backwards. This is illustrated by the green arrows. The reason is that the motors are reversed in the physical construction.

4.2 First try: returning to the initial position with leJOS stall detection

Assume the car and bridge are not in the initial position when the distributor is started. One may think it's easy to return them to the initial position by just driving the motors forward (remember, forward == physically backwards) a given amount of time and then stopping. This would work but it has two serious problems. Firstly, it would be slow. Because there is no way to know exactly how far away from the initial position the car and bridge are, we need to assume that they are always as far away as possible, and thus we need to ensure the amount of time they drive forward is enough to make them return from the most far away position (the opposite corner of the initial position). Assume that we measure this to be 10 seconds. If the car and bridge are only a few cm's away from the initial position the distributor will still use 10 seconds to return them - clearly a waste of time.
The second problem is that if the distributor keeps driving the motors when they are actually stuck (as it will in all cases but when it starts in the opposite corner of the initial position), the power that should be used to move the motors will be transferred to the surrounding Lego construction itself and put it under heavy load (we call this the force transfer problem). Obviously when we keep pushing power to the motors, and they cannot move, the resulting force has to go somewhere else, and this is much more critical than the first problem because it can actually break the Lego construction if it isn't sturdy enough. These problems call for a better solution.

If we think about it, the challenge we are facing here is remarkably similar to the one we faced with resetting the distributor gate tray to it's initial position in project lab 4a; we need to reset something driven by a motor to a specific position without external sensors. We solved the gate tray problem by using the leJOS stall detection mechanisms built into RegulatedMotor, so obviously we tried using this with the distributor as well. The video in Figure 10 shows the bridge doing a reset with the leJOS stall detection. Surprisingly this does not work nearly as well as with the tray gate. The first problem is that the bridge doesn't end up at the exact initial position - it's being "shot" off the position by a cm or two. The second problem is the force transfer problem. In the video it is obvious that the motor keeps driving even when the bridge is "stuck" at the correct initial position. It takes a second or two for the motor to catch up on the fact that the bridge has stopped, and in those seconds it transfers the force out through the axles and further on. This is a serious problem because when the motor then goes into float mode all the stored energy in the Lego will push back on the motor and cause it to rotate the opposite way for a short while. This is actually the reason why the bridge ends up a bit offset from the correct position.
But, this must mean the the gate tray mechanism also has the force transfer problem. Actually it has, but because the tray has a very compact and sturdy construction, the motor can't transfer nearly as much force to the Lego as it can with the distributor bridge. The bridge is driven by a long custom drive shaft construction consisting of several Lego axles and connectors. The problem with this is that a lot of energy can be stored in the driveshaft itself. There is no such driveshaft with the tray. 
The next thing we tried was to solve the problems by using code to neutralize the stored energy in the Lego.

Figure 10: The bridge resetting its position with the leJOS stall detection. This results in both overshooting the target (i.e. the bridge doesn't end up directly next to the stop pieces) and transfer of force from the motor to the surrounding Lego.

4.3 Second try: neutralizing the backforce energy

If the problem is that the motor transfers energy into the Lego that is then pushed back when the motors goes into float mode, then we should be able to neutralize some of this energy by releasing it slowly instead of all at once. This is exactly the idea we have employed in the code in Figure 11.

 private void stopMotorByNeutralizingBackForce()  
 {  
     flt();  
     Delay.msDelay(MSTIME_TO_NEUTRALIZE_BACKFORCE);  
     stop();  
     Delay.msDelay(MSTIME_NEEDED_TO_SETTLE_AFTER_STOP);  
     flt();  
 }
Figure 11: This code tries to neutralize the backforce stored in the Lego around the motor by slowly releasing it.
Essentially what the code does is that it only floats the motor a short amount of time whereafter it calls stop. The impotant thing about the stop method is that it will "lock" the motor to it's current tacho position. Assume that the MSTIME_TO_NEAUTRALIZE_BACKFORCE constant is so small that when the code reaches the stop method the motor is still receiving the push-back force from the Lego. What happens then is that the motor will measure it's current tacho position and hold itself there. Effectively what we have done is to neutralize MSTIME_TO_NEAUTRALIZE_BACKFORCE amount of the backforce, and if we then again put the motor in float mode there will be less backforce left in the system to push the motor. If the constants are adjusted correctly it is possible to neutralize just the right amount of backforce so that whatever backforce is left in the system isn't enough to move the bridge.
The video in Figure 12 illustrates the bridge running with the backforce neutralization code. It can be seen that the motor does a kind of "two-step" motion where it first drives a bit backwards, then stops and then again goes a bit backwards. However, the important part is that the last backforce push only moves the motor, not the bridge. The result is that the bridge is at the exact position is should be at; directly adjacent to the buffer stop pieces at the end of the tracks.

Figure 12: The bridge resetting its position with the leJOS stall detection and our custom solution to neutralize back-force. This solves the overshooting problem, but we still have force transfer from the motor to the surrounding Lego.

However, all is not perfect. What the video also shows is that we still have the problem of transferring force to the Lego. While this is a minor problem, in the long run we imagine it could mean the bridge itself could break. What we want is a solution that produces as little backforce as possible while returning the bridge to the exact position.

4.4: Third try: custom stall detection

It is obvious that we can't use the leJOS stall detection to avoid the build up of back-force. This is simply due to the way it works. RegulatedMotor will always try to maintain it's specified speed. The way leJOS does this is by regulating the power to the motor; if the motor doesn't move fast enough it gets more power and vice verse. The result is that per design the leJOS stall detection will always build up back-force before being stalled. It's easy to see why. As an example let's look at the bridge again. What happens is that when the bridge reaches the position where it can't move anymore, leJOS detects that the motor has a difficult time moving so it will increase its power. Unfortunately this doesn't help, because the bridge can't move any further no matter what power is supplied. leJOS, however, doesn't care. It just keeps increasing the power until it reaches 100% or it detects a stall, whichever comes first. The catch is that to be able to detect a stall leJOS needs to see that the motor is stuck for a specific amount of time - this amount of time is large enough for the motor to reach 100% power. Essentially what this this tells us is that we need to implement or own stall detection.

What we have done is to implement a new type of motor in leJOS: StallableMotor. This class inherits from NXTRegulatedMotor and provides some convenience methods like forwardUntilStalledAsync and waitForStall. The interesting part of this class is the run method (StallableMotor implements Runnable because it needs its own thread) shown in Figure 13.

 @Override  
 public void run()  
 {  
   suspendRegulation();  
   tachoPort.controlMotor(30, BasicMotorPort.FORWARD);  
   int oldTaco = getTachoCount();  
   while (true)  
   {  
     Delay.msDelay(50);  
     if (oldTaco == getTachoCount())  
       break;  
     oldTaco = getTachoCount();  
   }  
   resetTachoCount();  
   flt();  
   hasDriveStalled = true;  
 }
Figure 13: The code for our custom stall detection in the StallableMotor class. This class inherits from NXTRegulatedMotor which is why it has direct access to methods like suspendRegulation, getTachoCount etc.
The first thing the code does is do suspend the regulation of the motor in question. All RegulatedMotors are controlled in a single loop system in leJOS, and if we want to control the motor manually by low-level commands like controlMotor, we need to take the motor out of this control loop (if we don't bad things happen). Next, we start the motor by giving it 30% power forward (remember, forward is physically backwards). Then we have a small loop that each 50ms checks if the tacho count has changed since the last measurement. If it hasn't we're stalled, and then we reset the tacho count and put the motor into float mode. Resetting the tacho count may not be necessary, but we did it just to be on the safe side - after all we have been controlling the motor completely outside of the leJOS regulation system, so by resetting the tacho count we can be sure leJOS doesn't think the motor is at rotation x while it actually is at rotation y.

The difference between our custom stall detection and the leJOS version is that we don't provide any more power to the motor when it can't move. This means that even when the motor is stuck it will only be getting 30% power and thus the amount of force it can transfer to the surrounding Lego is limited. Figure 14 shows a video of the bridge resetting with our custom stall detection. It can be seen that it works perfectly; the bridge returns to the exact position and there is almost no transfer of force to the Lego.

Figure 14: The bridge resetting its position with our custom stall detection. It can be seen that this solves both  original problems of overshooting and transferring force to the surrounding Lego.

5. Dropping off Lego pieces in the trays

We have identified optimal values for speed and acceleration for the car and bridge. We have also found a way to reset the distributor to its initial position, where it will pick up the next Lego piece to be distributed. All that's missing now is for the distributor to actually drive out to the correct compartment in the underlying tray and deliver the picked-up Lego piece.

We have derived a strategy that will allow us to complete this last piece of the puzzle:
  1. First, we will determine where the distributor should drop the Lego axles on the axle tray. This involves creating a coordinate system for the tray (in cm), where each drop-point will be a point in this coordinate system. 
  2. Then, we will find a way to translate the tray coordinate system (in cm) to tacho counts. In other words, if the distributor should drop-off a Lego piece at the point (1,1)cm we need to find out how many tacho counts this corresponds to for both the car and the bridge.
  3. Finally, we may need to adjust the cm to tacho conversion based on the fact that the car will be offset by a specific amount compared to the tray itself. For example if we calculate that the car and bridge should rotate 360 degrees to drive 1cm each, this may in fact not be enough because the car doesn't start diretely above the first compartment in the tray - it actually starts a bit offset from it.
After having done this we of course also need to repeat all the steps for the beam tray. If this strategy seems complex worry not, we will explain it in detail in the next sections.

5.1 Drop-points for the axle tray

The first thing we do is to take a picture of the axle tray, measure the dimensions of it and then draw a coordinate system that reflects these measurements. Figure 15 shows the result.

Figure 15: The coordinate system for the axle tray shows the exact dimensions of the tray and all its compartments.

Now that we know the exact dimensions of the tray we can calculate the points where the distributor should drop off each of the axles. The first idea might be to just drop off the axles in the center of each compartment. This will not work however - at least not for the larger axles. To see why this is we turn to physics. 

In Figure 16 we have illustrated what a typical drop-off scenario may look like. The drop-off point is the point on the tray directly beneath the point on the distributor car where the car releases the item to be dropped. The (not so) surprising thing is that the drop-off point is not the point where the item will end up in the tray. Because the distributor car tray is angled and placed some distance above the Lego tray, the Lego piece will actually be dropped in a parabola shape away from the distributor car. Furthermore, the length of the parabola line is directly related to the length of the Lego piece being dropped. This is also illustrated in the figure. A smaller axle has a shorter parabola drop line than a longer one. Therefore, if we want to drop an axle in a specific compartment, depending on the length of the axle, we want to move the drop-off point more or less back from the center point of the target compartment to account for the parabola drop line.

Figure 16: A typical drop-off scenario. The actual  point where the Lego pieces will be dropped in the tray (dropped-at point) is determined by a parabola line originating from the drop-off point on the distributor tray. Finally we found a practical usage for high-school physics!
If we had to be completely rational we ought to calculate the formula for the parabolas and use that to determine the exact drop-off points on the axle tray. In practice we didn't want to go that deep. We believe that simply being aware of the fact that Lego pieces are dropped in a parabola will be sufficient. 

With the the parabola drop-lines in mind we proceeded to calculate the actual drop-off points for the axle tray. Our first idea was to calculate all drop-off points based on the position of the first. In practice we decided the drop-off point for axles of length two should be right in the center of the first compartment. This works because the axles are so small that the parabola line will be very steep, and thus hit the compartment itself. From there on we calculate the center points of the remaining compartments, and in each step we subtract 0.5cm from the center. This means that we are offsetting the drop-off point with 0.5cm each time the axle length increases by 1, thus accounting for the fact that the parabola lines become longer. The result is shown in Figure 17. 
Figure 17: The calculated drop-off points for each axle length. Each point after the 2-axles is successively offset by 0.5cm on the y-axis to account for the parabola drop-lines length increasing with the length of the axles. 

5.2 Cm to tacho (degrees) translation

The calculated drop-off points in the cm coordinate system in Figure 17 is nice, but we have a unit mismatch between the distributor and the tray. The distributor doesn't work in the metric system, it works in tachos (i.e. angle/degrees). We can't tell the distributor car or bridge to drive 10cm. What we can do, however, is tell it to rotate the motor a specific amount of degrees. Essentially what we need to be able to use the calculated drop-off points, is a way to translate between cm and degrees of motor rotations (or tacho counts, it's the same thing in leJOS).

Figure 18: The result for the cm to tacho test for the car. The car drives 39cm when the motor rotates 375*3 degrees.
To calculate the translation factor between cm and tachos, we performed a simple experiment for both the car and the bridge. Basically we mark the units starting position on a piece of paper on the ground, tell the motor to rotate a specified amount of degrees, and then measure how many cm's the unit has moved. Figure 18 shows the result for the car (the bridge result is identical, so we won't show it here). The program used to perform the test, CmToTachoTest.java is shown in Figure 19. 

 public class CmToTachoTest  
 {  
   private static final int TEST_TRACK_ANGLE_LEN = 375 * 3;  
   public static void run(RegulatedMotor motor)  
   {  
     motor.setSpeed((int)motor.getMaxSpeed());  
     motor.setAcceleration(3000);  
     motor.rotate(-TEST_TRACK_ANGLE_LEN);  
   }  
 }
Figure 19: The code we used to perform the cm to tacho test. Note that the motor actually rotates a negative amount of degrees. This is just an arbitrary choice that fit with our physical setup, where the car would drive from the left to the right as indicated by Figure 18.
We now have information on how many degrees the motor rotated ((TEST_TRACK_ANGLE_LEN=375)*3) from Figure 19, and how many cm the unit moved (~39, from Figure 18). With those numbers we can calculate the translation factor between cm and tacho counts. Specifically, we calculate that to drive the car 1 cm the motor should rotate (375*3/39)=28.84 degrees. We round this up to 29cm since that's easier to remember and work with. Now we can translate between the drop-off points in cm to tacho counts for the motor. Sweet! We create a translation function that does the magic as shown in Figure 20.
 private static int cmToRotationAngle(float cm)  
 {  
   return Math.round(MOTOR_CM_TO_ROTATION_ANGLE_FACTOR*cm);  
 }
Figure 20: The Distributor.cmToRotationAngle function is the result of our experiment. It translates cm's to motor rotations so we can use the easy-to-work-with-and-understand metric coordinate system of the tray when we specify drop-off points for the distributor. Much nicer than spreading seemingly arbitrary angle values all around our code. MOTOR_CM_TO_ROTATION_ANGLE_FACTOR is the number 29 we calculated in the experiment.

5.3 Translating between the tray and car coordinate systems

Let's sum up what we have done so far:
  • We have built a metric coordinate system for the axle tray and determined the points where each axle should be dropped off.  In other words when the car drives to a drop-off point is should place itself such that the drop-off point is directly beneath the point on it's own tray where the Lego piece is released. This was illustrated earlier in Figure 16.
  • We have also found a way to translate between the metric coordinates to motor rotations so we can tell the distributor to drive to a specific point in a metric coordinate system.
Figure 21: The distributor's coordinate system (blue) is offset from the trays coordinate system (black). To actually move the distributor to the desired position on the tray, we must add this offset to all points in the tray coordinate system before or after converting to tachos (depending on how you see it). Note that even though it is drawn as positive, the offsets may be negative as well, i.e. if the car starts inside the tray.
It sounds like we're done. For any given drop-off point we can tell the distributor to drive there. However, there's a subtle issue we have not addressed it. Figure 21 illustrates this. Assume we have a drop-off point on the tray as in the figure; (28,7;25). We translate these to tacho coordinates according to our formula and get a new set of coordinates (a,b) in the distributor's coordinate system. If we tell the distributor to drive to these coordinates it doesn't end up at (28,7;25), but at the position marked by the blue car in the figure. The problem is that the distributors coordinate system and the tray coordinate system are not aligned. Due to the physical design of the distributor, the car will actually be positioned slightly offset from the tray itself when at its initial position. This offset is marked as "offset x" and "offset y" on the figure. What we must do to get the car to drive to the correct place on the tray, is to add this offset to all points in the distributors coordinate system. In practice this is called a coordinate system transformation 

What we did in section 5.2 when translating between cm and tachos is in fact also a coordinate transformation, we just didn't mention it explicitly because it was easier to explain without. However, as we have just described, to complete the transformation, we also need to account for the offset between the tray and the distributor coordinate systems. We must identify how much the coordinate systems are offset from each other, and the way we do that is of course by performing an experiment. "Experiment" may be exaggerated a bit. All we do is to reset the distributor to it's initial position and measure the distances in cm from the car's drop-off point to the corner of the tray (0,0). We measure the offset on the x-axis to -4.1cm and on the y-axis -2cm. Note that the offsets are negative because the car's initial position actually is inside the tray. The initial position is thus the minimum position the car can drop things at.

With the offset information we have all we need to perform a complete coordinate transformation. The next section will describe how we do this in practice.

5.4 The Distributor class

The Distributor class is the result of all the work done so far. It has a very simple API: 

  • public Distributor(StallableMotor carMotor, StallableMotor bridgeMotor, TransporterTray tray, Map<Integer, Point> trayCoordinateSystem_DropOffPoints, Point trayToDistributorOffset)
  • public void dropOffPieceAt(int position)
  • public void resetPosition()
  • public void setTrayCoordinateSystem_DropOffPoints(Map<Integer, Point> trayCoordinateSystem_DropOffPoints)
  • public void setTrayToDistributorOffset(Point trayToDistributorOffset)
The constructor takes two StallableMotors; one to control the car and another to control the bridge. It also needs a tray to pick up and release Lego pieces at the correct times. Finally it takes a map of drop-off points given in the underlying tray coordinate system, and a point that represents the offset information. Note that there is no need to pass in the cm to tacho conversion information, as that is the same for all trays (axles, beams, etc..). However the offset information may be different depending on the tray layout.

The resetPosition resets the distributor to it's initial position. This is the result of the design and experiments done in section 4.

The setter methods are provided to enable on-the-fly switching of the underlying trays. This makes it possible to quikcly switch out i.e. the axle tray with the beam tray and then sort beams without restarting the whole system.

The dropOffPieceAt method makes the distributor drive to the point signified by the position argument (i.e. in the map given in the constructor), drop off its item, and drive back to the initial position. This is the method that represents all we have described in the previous section. The code for this method can be seen in Figure 22. In line 3-4 we get the point in the tray coordinate system we should drive to. Then we do the first part of the coordinate transform by offsetting it by the given offset point. Next, on line 7-10 we tell the motors to rotate the specified amount of degrees that correspond to the amount of cm (note we multiply by -1 because we actually need to rotate backwards to drive the car and bridge forwards). This is the second and last part of the coordinate transform. On line 11-12 we wait for the motors to complete their rotations, and then on line 13 we release the item from the tray. This is a synchronous preparation which is why we don't need to wait for it. 

What happens afterwards is purely an optimization. After having dropped the Lego piece we could simply call resetPosition, however that would be terribly slow. Remember that the distributor resets itself by using StallableMotor and waiting for a stall - this is done, by design, with only a motor power of 30%. If we use this method to reset, it would take ages for the distributor to go back to its initial position. Instead what we do on line 14-17 is to tell the distributor to drive both the car and bridge back to their almost initial positions, but with full speed. By driving back to positions that are 2cm away from the initial ones, we avoid banging into any stop-bricks or destroying the Lego construction itself. Of course 2cm away from the initial position is not the initial position, so after doing the "fast-backward" we use resetPosition to reset properly to the initial position. The difference is that we only need to drive a maximum of 2cm with both the car and the bridge while being slow.

1:  public void dropOffPieceAt(int position)  
2:  {  
3:    Point targetInTrayCoordSys = trayCoordinateSystem_DropOffPoints.get(position);  
4:    Point targetInDistributorCoordSys =  
5:        new Point(targetInTrayCoordSys.x + trayToDistributorOffset.x,  
6:             targetInTrayCoordSys.y + trayToDistributorOffset.y);  
7:    int bridgeMotorRotationAngle = -1*cmToRotationAngle(targetInDistributorCoordSys.x);  
8:    bridgeMotor.rotate(bridgeMotorRotationAngle, true);  
9:    int carMotorRotationAngle = -1*cmToRotationAngle(targetInDistributorCoordSys.y);  
10:    carMotor.rotate(carMotorRotationAngle, true);  
11:    bridgeMotor.waitComplete();  
12:    carMotor.waitComplete();  
13:    tray.releaseItem();  
14:    carMotor.rotate(Math.max(-carMotorRotationAngle-cmToRotationAngle(2f),0), true);  
15:    bridgeMotor.rotate(Math.max(-bridgeMotorRotationAngle-cmToRotationAngle(2f),0), true);  
16:    bridgeMotor.waitComplete();  
17:    carMotor.waitComplete();  
18:    resetPosition();  
19:  }
Figure 22: The Distributor.dropOffPieceAt method. This is the result of all our efforts described in this section.

6. Revisiting the axle tray drop-off points

While the idea with the drop-point placements in Figure 17 seems good we quickly found out that it doesn't work in practice. The distributor was able to drop the smaller axles in the correct compartments, but with anything above 5 or 6 length axles it would sometimes drop them in the compartments above the correct ones (i.e. too far on the y-axis). We thought about this for a bit, and concluded that the issue must be that we haven't accounted enough for the parabola drop-line length with the longer pieces. In other words, the distributor should drop off the longer axles farther back on the y-axis than what is illustrated on Figure 17.

What we did next was to calculate a new set of drop-points such that almost all drop-points are in the compartment behind the compartment where the piece should actually be dropped. The exception is the three compartments in the bottom where the minimum drop-position is 2 on the y-axis. The result can be seen in Figure 23. From the fourth drop point (4,1;10,4) and on, we successively subtract 0.4 from the y-coordinate to account for the increased parabola drop-line length when the axle lengths increase. This is double the amount of before. Note that we have also rearranged the compartments such that we drop the smallest axles in the bottom three columns of the tray. This was done on purpose to exploit the minimum drop position of 2 on the y-axis. For example we wouldn't be able to drop 12-axles (like we thought we could earlier) in any of the lower compartments.
Figure 23: The new drop-off points are much further down on the y-axis than earlier. We also exploit the fact that the minimum drop position on the y-axis is 2, by dropping the smallest axles in the lower three compartments.

7. Feeder

We need to change the current Feeder system as it is very unreliable. We believe we might be able to cope with a one-stop elevator instead of a two-step one.

7.1 Elevator system

We wanted to build an elevator system where the elevator part was supported on all sides such that it would be unable to move out of the construction (as our current version does). A test of the new elevator system can be seen in the video below.


Compared to the first elevator system, this is far more robust and was also much easier to build. The first elevator system is shown below for reference.

7.2 Transportation

Over the course of this project, we have collected all the conveyor belt links that we could find. At this point we have enough for building a small conveyor belt, and we will use this conveyor belt in the Feeder system to transport beams from the elevator and forward.

7.3 Only one beam at a time

Now that we have the transportation system in place, we need to handle one of the requirements of the Feeder: It may only send forward one beam at a time. Our first conveyor belt allowed for up to three beams to be laying side by side on the belt. As in [4] we wanted to push extraneous beams back into the Feeder holding bay. We were unable to push only two of three side-by-side beams down, so we modified the conveyor belt to only have room enough for one beam. Other beams could then be laying on top of it, but we could push them out using the system on Figure 24.

Figure 24: Construction making sure that beams placed on top of other beams are thrown off the track. We have not yet constructed the Feeder such that thrown off beams are returned to the holding tank.
A video showing a sample run with the construction in Figure 24 can be seen below.

Sometimes when a beam is half on top of another beam, they can get stuck in the construction, see video below.

So the solution still needs more work. However, due to time pressure we wanted to move to the next problem as we did not have any better ideas at that moment.

7.4 Beams need to be positioned correctly

At this point we have one beam at a time, and now we need to make sure that the beam is placed such that the holes is in the side and not pointing upwards, since the BeamCounter will then be unable to count them.

We had several ideas on how to implement this solution. We could either rotate the beams if needed or we could throw beams that were positioned incorrectly down to the holding tank again.

We tried to implement the last solution. We added a lightsensor above the track which should be able to sense if there were holes upwards. An arm would the push the brick off the belt if the holes where upward. A prototype can be seen in Figure 25. We where unable to use the lightsensor to detect the holes and were totally unable to detect if a beam was even passing through. A white beam was no problem, but a black beam would not change our light readings compared to just having the black conveyor belt. We ditched the idea and gave up on the Feeder as the day was running out. We did not have enough time to fix all these problems to get it working, and we still needed to connect it to the BeamCounter, and not to mention only giving one beam at a time. If we administered two beams just after each other, the BeamCounter would not be able to pick them apart and would think a single beam with many holes had passed by.
Figure 25: The lightsensor sees that the holes is pointing upwards and the arm has moved out to reject the beam and get it back to the holding tank. This is only a prototype version.
Another idea we had was to implement a rotation device using a z40 gear rotating above the beam. If the flat surface part of the beam was pointing upwards, the gear would not be able to get traction to it and the beam would continue on. However, if the beam had the holes pointing upwards, the gear would get traction and forcefully turn the beam or throw it off the conveyor belt.

A third idea could be to somehow alter the BeamCounter such that it could sense the holes no matter what position the beam was in. However, we did not have the time or any ideas how to implement this.

8. System test

We needed to make sure that the system would work for the demonstration. We needed to get all the working components (Sorter, Loader and Distributor) to function together. When we have coded them to function like they should we discovered two issues when testing our system.

8.1 AxleSorter getting wrong axle lengths

It seemed that the AxleSorter subsystem consistently found that an axle of length n > 8 were of length n-1. Way back in "Project lab 2 - Preliminary investigations" we had calculated the key parameter TIME_PER_AXLE_LENGTH. It now seemed that the value was too large and that we needed to decrease it. We decreased it from 301 to 293, which was a major change. A value of 293 seemed to give good results, however, we had found the value 301 by performing many experiments, and the result of those experiments would not fit a value of 293. We believe that the battery power might have increased since those experiments meaning that the motor would have greater power and the wheel could rotate faster. That would then make the axles run faster through the sensor system and we would therefore need to change our TIME_PER_AXLE_LENGTH.

8.2 Distributor colliding with the Sorter-Loader platform

Another problem we found was that the Distributor sometimes collided with the Sorter-Loader platform when moving right on the track. This would move the Sorter-Loader platform such that the next brick would not correctly be ejected into the Distributor. We had to retract the legs of the Sorter-Loader platform away from the Distributor, which solved the problem.

9. Conclusion

9.1 Distributor

The first thing we did in this lab was identifying the optimal speed and acceleration values for the distributor car and bridge. We did this by performing an experiment. We found that with speed=885 and acceleration=3000 both the car and bridge were precise and fast.
To reset the distributor to its initial position, we went through several iterations of code and design, including the leJOS built-in stall detection, before coming up the the final custom stalling solution.

Finally we followed a carefully thought out process to design and code the logic that's responsible for the main part of the distributor; picking up and dropping Lego pieces off at the correct tray compartments. However, we only had time to do this for the axle tray.

9.2 Feeder

We now have a system where the Sorter, Loader and Distributor can cooperate in sorting a Lego axle as long as a human feeds the system. We still need to create the Distributor map for Lego beams before those can be sorted. We have given up on the Feeder.

10. Next lab

In the next project lab we will repeat the strategy mentioned in section 5 to determine the drop-off points and  the offset point for the beam tray. This should complete the distributor.

11. References

[1] “RegulatedMotor (leJOS NXJ PC-API documentation).” [Online]. Available: http://lejos.sourceforge.net/nxt/pc/api/lejos/robotics/RegulatedMotor.html. [Accessed: 24-Jun-2013].
[2] “Embedded Systems, Embodied Agents - Lecture slides week 2 - Slide 3.” [Online]. Available: http://legolab.cs.au.dk/DigitalControl.dir/NXT/Lectures/Lecture2/Lecture.pdf. [Accessed: 05-Jun-2013].
[3] “Embedded Systems, Embodied Agents - Lecture slides week 2 - Slide 7-17.” [Online]. Available: http://legolab.cs.au.dk/DigitalControl.dir/NXT/Lectures/Lecture2/Lecture.pdf. [Accessed: 05-Jun-2013].
[4] “LEGO Axle Sorter AS-L40A - YouTube.” [Online]. Available: http://www.youtube.com/watch?v=fM9qGZCc4DY. [Accessed: 17-Jun-2013].

This entry was posted in