carousel.comp: Add direct position control for stepgen and encoder modes.

Signed-off-by: andypugh <andy@bodgesoc.org>

Author: andypugh

src/hal/components/carousel.comp

@@ -52,8 +52,8 @@ When the enable pin is set to true the component will immediately set the
 "active" pin to true and then (for a bidirectional instance) calculate the
 shortest path to the requested pocket number. The appropriate motor direction
 output pins will then be set. Bit outputs for forward and reverse are provided
-as well as a three-state velocity output for driving a DC motor PWM or a 
-velocity-mode stepgen. 
+as well as a three-state velocity output for driving a DC motor PWM or a
+velocity-mode stepgen.
 
 The component will monitor the carousel position and, when the correct position
 is reached, set the motor-control pins to 0, set "active" to 0 and set "ready"
@@ -74,7 +74,7 @@ Once the decel-time has expired the carousel will, if it was moving forwards, re
 to the position marker, off of the arker and then bak on to the FWD edge. If moving in reverse
 it will continue off the marker and then reverse slowly on to the FWD edge.
 This algnment is only possible with a motor-vel controlled bidirectional carousel,
-Other combinations will be acepted but probably won't have the desired behaviour. 
+Other combinations will be acepted but probably won't have the desired behaviour.
 Some tuning will be needed of align-dc and decel-time to achieve reliable operation.
 
 In the unusual case that the index and pulse signals do not align it is
@@ -89,6 +89,12 @@ To operate the component with an encoder or stepgen use mode "C". The \\fBscale\
 pin should be the number of steps or encoder counts between pocket centres. The
 \\fBwidth\\fR pin can be used to stop the motor some distance before the centre of
 the pocket to allow the motor time to decelerate.
+In mode "C" it is possible to use either the speed/direction control used in other
+modes or to use direct position mode using the counts-target pin. The
+internal scaling of the encoder or stepgen should be set to 1.0. A PID
+hal component will be needed for encoder applications whereas stepgen
+configurations can use a stepgen in position control mode or in velocity
+control mode with a PID.
 
 For tool changers which lock the carousel against a stop the \\fBrev-pulse\\fR pin can
 be set to a non-zero value. The motor-rev pin will then be set for this many seconds
@@ -119,7 +125,7 @@ certainly needs decel-time setting too""";
 pin in float decel-time "Time to wait for carousel to stop before final alignment and position check";
 pin in signed counts "Connect to the rawcounts of an encoder or a stepgen in 'counts' mode";
 pin in signed scale = 100 "The number of stepgen or encoder counts between successive pockets";
-pin in signed width = 0   "How far each side of the exact scale to signal a new pocket";
+pin in signed width = 10   "How far each side of the exact scale to signal a new pocket";
 pin in signed home-offset = 0 "The offset (in counts) between the index and pocket 1";
 pin io bit index-enable "Used to home to an encoder index";
 pin in  bit    jog-fwd "Jog the carousel forwards one tool position";
@@ -133,11 +139,13 @@ pin out signed current-position "This pin indicates the current position feedbac
 pin out float  motor-vel "The duty-cycle or velocity to drive a DC motor or stepgen";
 pin out bit    homed = 0 "Shows that homing is complete. Only used in index and edge modes";
 pin in  bit    unhome = 0 "Should only really be necessary for testing";
+pin out float counts-target "Target position for a stepgen or external PID controller";
 
 param r  signed state = 0 "Current component state";
 param r  bit    homing  = 0 "Shows that homing is in progress. Only used for index mode";
 param r  float  timer "Shows the value of the internal timer";
 param r signed motor-dir "Internal tag for search direction";
+param r signed counts-offset "Internal offset of index pin";
 param rw unsigned debounce "How many thread cycles to wait for the position to stabilise";
 
 option count_function;
@@ -152,7 +160,7 @@ variable int inst_pockets;
 variable int inst_code;
 variable int inst_parity;
 variable int old_index = 0;
-variable int base_counts = 0;
+param r signed base_counts = 0;
 variable int target;
 variable int old_unhome = 0;
 function _ ;
@@ -278,7 +286,9 @@ FUNCTION(_){
             c = (counts - base_counts);
             t = floor(((float)c + w) / scale);
             if (c >= (t * scale - w)  && c <= (t * scale + w)) {
-                new_pos = 1 + t - inst_pockets * floor((float)t / inst_pockets);
+                new_pos = 1 + (t % inst_pockets);
+                // C modulus of negative numbers is inconvenient
+                if (new_pos < 1) new_pos += inst_pockets;
             }
             align_pin = (c % scale <= 2 && c % scale >= -2); // don't try to align to an exact count.
         }
@@ -344,26 +354,34 @@ FUNCTION(_){
                     motor_rev = 1;
                     motor_vel = rev_dc;
                     motor_dir = -1;
+                    counts_target += (mod_pocket - current_position - inst_pockets) * scale;
                 } else {
                     motor_fwd = 1;
                     motor_rev = 0;
                     motor_vel = fwd_dc;
                     motor_dir = 1;
+                    counts_target += (mod_pocket - current_position ) * scale;
                 }
             } else {
                 if (current_position - mod_pocket > (inst_pockets / 2)) {
                     motor_fwd = 1;
                     motor_rev = 0;
                     motor_vel = fwd_dc;
                     motor_dir = 1;
+                    counts_target += (mod_pocket - current_position + inst_pockets) * scale;
                 } else {
                     motor_fwd = 0;
                     motor_rev = 1;
                     motor_vel = rev_dc;
                     motor_dir = -1;
+                    counts_target += (mod_pocket - current_position) * scale;
                 }
             }
         } else {
+            counts_target += (mod_pocket - current_position) * scale;
+            if (counts_target < counts ) {
+                counts_target += scale * inst_pockets;
+            }
             motor_fwd = 1;
             motor_rev = 0;
             motor_vel = fwd_dc;
@@ -457,6 +475,8 @@ FUNCTION(_){
         motor_vel = fwd_dc;
         motor_dir = 1;
         index_enable = 1;
+        counts_target += scale * inst_pockets;
+        old_index = 1; // ensure 0->1 cycle of home sensor
         state = 11;
     case 11: ;// waiting for index & pulse
         int ind = 0;
@@ -474,22 +494,23 @@ FUNCTION(_){
             motor_rev = 0;
             motor_vel = 0;
             base_counts = counts - home_offset;
+            counts_target = base_counts;
             if ( align_dc == 0) {
                 state = 0;
             } else { 
                 timer = decel_time;
                 state = 5;
             }
         }
-        old_index = (sense(0) && sense(1));
+        old_index = ind;
         break; // So that we don't see the tool1 pulse twice
     case 20: //jogging fwd/rev
         if (current_position != target){
-			deb = debounce;
-			return;
-        } else if (deb-- >0) {
-			return;
-		}
+            deb = debounce;
+            return;
+        } else if (deb-- > 0) {
+            return;
+        }
         if (align_dc != 0 && ! align_pin) return;
         motor_fwd = 0;
         motor_rev = 0;