Add code and docs received from HPD

Upload code and documents received from HPD.

README.md

@@ -1 +1,7 @@
-# wSMA-Cryostat-Selector
+# wSMA-Cryostat-Selector
+
+This repository contains the motion controller code for the Galil DMC30000 motion controller in the wSMA Cryostat selector wheel controller box. and Python code for interfacing to the motion controller over TCP Modbus.
+
+The motion controller is a [Galil DMC3x01x series](http://www.galilmc.com/motion-controllers/single-axis/dmc-3x01x) controller. The program is written in a Basic type language, and the motion controller is programmed via the [Galil Design Kit](http://www.galilmc.com/downloads/software/gdk) software package.
+
+The Python code communicates with the controller using the Python PyModbus package.

motion-controller/src/wheel controller -4-23-20 -final.dmc

@@ -0,0 +1,281 @@
+#AUTO; 'automatic starting command
+'***************************************************************************
+'Motion Control for SAO Selector Wheel
+'***************************************************************************
+'Description:
+'
+'***************************************************************************
+#SETUP
+SIA=1,29,14,-1<10>1; 'SSI absolute encoder setting string
+YCA=16384;	' 16384 counts per revolution
+
+MT -2.5;		' active high pulses, use -2.5 to reverse direction
+						' we are going say counter-clockwise (ccw) motion is positive
+						' clock-wise is negative for both stepper and "net resolver" motion
+
+LCA=15;			'	15 samples after moving, go to 25% current for holding torque
+
+DM A[6];		'dimension integer register array, length 10 16-bit uints
+						'  A[0], current position - valid values are: 1-4 obvious moves, 5 is home, 6 is shutdown
+						' A[1], commanded position - valid values are: 1-4 - set internally
+						' A[2], speed of commanded motion - passed in, 1 for slow, 2 for medium
+						' A[3], error code or completed motion
+						' A[4], time elapsed in movement, in milliseconds
+						' A[5], postion of the resolver, minus the home, after the move - ERROR ON IMPLEMENTATION
+
+ME1;				' allow modbus to write to arrays
+
+
+
+#init
+' present initialization based upon manual homing of wheel during bench test
+' so... manually home wheel before starting program
+A[0]=5; ' commanded position, initiallized to cause a HOME procedure
+A[1]=0; ' Current Position, initiallized to cause a HOME procedure
+A[2]=1; ' initiallize speed to slow
+
+raw_pos=17000;	'once used should be large integer value, not 17000
+angle=17000;		'once used should be 0-360
+dangle=17000;	  'once used should -360 to 360
+calc_mov=0;     'once used should be non zero
+raw_pos1=17000;	'once used should be large integer value, not 17000
+angle1=17000;		'once used should be 0-360
+dangle1=17000;	  'once used should -360 to 360
+raw_pos2=17000;  'once used should be 0-16384
+angle2=17000;		'once used should be 0-360
+dangle2=17000;	  'once used should -360 to 360
+calc_mv2=0;     'once used should be non zeroraw_pos=17000;	'once used should be large integer value, not 17000
+raw_pos3=17000;  'once used should be 0-16384
+angle3=17000;		'once used should be 0-360
+dangle3=17000;	  'once used should -360 to 360
+calc_mv3=0;     'once used should be non zeroraw_pos=17000;	'once used should be large integer value, not 17000
+raw_pos4=17000;  'once used should be 0-16384
+angle4=17000;		'once used should be 0-360
+dangle4=17000;	  'once used should -360 to 360
+calc_mv4=0;     'once used should be non zero
+raw_pos5=17000;  'once used should be 0-16384
+angle5=17000;		'once used should be 0-360
+dangle5=17000;	  'once used should -360 to 360
+calc_mv5=0;     'once used should be non zero
+
+
+dspr=256000; 		'drive steps per filter wheel revolution: 200*256*5
+rres=16384;			'resolver 'steps' per rev
+rstep=45.1111;  '45.1111 is rres/360 is resolver steps per degree
+drstep=15.625;	'15.625= 256000/16384 = dspr/rres = drive step/resolver steps
+
+	'configure SPEED SETTINGS
+medsp=75000;
+slowsp=30000;
+suslow=10000;
+
+
+medAC= 150000;
+medDC= 150000;
+slowAC= 50000;
+slowDC= 50000;
+suslowAC= 10000;
+suslowDC= 10000;
+fastDC= 300000;
+
+v1=0;
+
+
+'----------------------------------------------------------------------------
+
+#EVENTLP
+'read resolver input and position setpoint from integer register array
+'check setpoint limits and limit if needed
+' then calculate current angular position from home based on resolver input
+
+'WT 1000; 'slows it down for manual control and testing
+com_pos=A[0];
+cur_pos=A[1];
+
+
+'MG "main routine com_pos: ", com_pos;
+'MG "main routine cur_pos: ", cur_pos;
+
+IF (com_pos<>cur_pos); 'event handler loop
+	IF (com_pos=5); JP #HOME; ENDIF;
+	IF (com_pos<=1); setpoint=pos1; JP #MOVE; ENDIF; 'control for out of range low values of position input
+	IF (com_pos=2); setpoint=pos2; JP #MOVE; ENDIF; 'control for out of range low values of position input
+	IF (com_pos=3); setpoint=pos3; JP #MOVE; ENDIF; 'control for out of range low values of position input
+	IF (com_pos=4); setpoint=pos4; JP #MOVE; ENDIF; 'control for out of range low values of position input
+	IF (com_pos>6); setpoint=pos4; JP #MOVE; ENDIF; 'control for out of range high values of position input
+'	IF (com_pos=6);'EN;'commented out as an unhandled exception.  "6" is the escape from the loop
+ELSE
+	JP #EVENTLP;
+ENDIF;
+
+EN	;		'should never reach this
+'----------------------------------------------------------------
+
+#MOVE
+'MG "MOVEIT!!";
+A[3]=1;				'set error handler to movement in progress
+speed=A[2];
+IF (speed<=1); SP slowsp; AC slowAC; DC slowDC; ENDIF; 'set speed from input and control for out of bound low
+IF (speed>=2); SP medsp; AC medAC; DC medDC; ENDIF; 'set speed from input, medium speed
+
+raw_pos=_TPA;
+'MG "MOVE to position: ", com_pos;
+'MG "Speed setting: ", speed;
+'MG "Movement parameters: Accel, Speed, decel  ", _AC, _SP, _DC;
+
+'MG "MOVE routine cur_pos: ", cur_pos;
+'MG "raw position", raw_pos;
+'MG "setpoint is: ", setpoint;
+'MG "rres: ", rres;
+
+
+' the wheel moves counter-clockwise from home to filter positions,
+' the resolver input change is negative in counter-clockwise direction,
+' so *-1 to and check limit to ensure net_pos never goes negative (array can't store neg value)
+'net_pos= -1 * (raw_pos - home);
+IF (raw_pos < 0);
+	raw_pos=0;
+ENDIF;
+angle = (raw_pos/rstep);									' 45.1111=(rres/360) angle from home, ccw is positive displacement
+dangle = ((setpoint - raw_pos)/rstep);		'angle delta from resolver position setpoint, ccw is positive
+calc_mov = @INT[((raw_pos-setpoint)*drstep)]; 'drive steps to correct wheel position to sp, ccw motion is positive
+
+'MG "about to start motion"
+
+bgtime=TIME;
+'SH
+
+IF (@ABS[dangle] > 0.8);
+	PR calc_mov;
+	BGA;
+	AMA;
+	raw_pos1=_TPA;
+	dangle1 = ((setpoint - raw_pos1)/rstep)
+	A[5]=@ABS[100*dangle1];
+ENDIF;
+
+'MG "move1 completed in ", deltaT1, " milliseconds";
+
+'round 2 on MOVE, if needed
+raw_pos2=_TPA;
+angle2 = (raw_pos2/rstep);									' angle from home, ccw is positive displacement
+dangle2 = ((setpoint - raw_pos2)/rstep);		'angle delta from resolver position setpoint, ccw is positive
+calc_mv2 = @INT[((raw_pos2-setpoint)*drstep)]; 'drive steps to correct wheel position to sp, ccw motion is positive
+
+
+
+IF (@ABS[dangle2] > 0.8);
+	PR calc_mv2;
+	BGA;
+	AMA;
+	raw_pos2=_TPA;
+	dangle2a = ((setpoint - raw_pos2)/rstep)
+	A[5]=@ABS[100*dangle2a];
+ENDIF;
+
+'round 3 on MOVE, if needed
+raw_pos3=_TPA;
+angle3 = (raw_pos3/rstep);									' angle from home, ccw is positive displacement
+dangle3 = ((setpoint - raw_pos3)/rstep);		'angle delta from resolver position setpoint, ccw is positive
+calc_mv3 = @INT[((raw_pos3-setpoint)*drstep)]; 'drive steps to correct wheel position to sp, ccw motion is positive
+
+
+
+IF (@ABS[dangle3] > 0.8);
+	PR calc_mv3;
+	BGA;
+	AMA;
+	raw_pos3=_TPA;
+	dangle3a = ((setpoint - raw_pos3)/rstep)
+	A[5]=@ABS[100*dangle3a];
+ENDIF;
+
+'round 4 on MOVE, if needed
+raw_pos4=_TPA;
+angle4 = (raw_pos4/rstep);									' angle from home, ccw is positive displacement
+dangle4 = ((setpoint - raw_pos4)/rstep);		'angle delta from resolver position setpoint, ccw is positive
+calc_mv4 = @INT[((raw_pos4-setpoint)*drstep)]; 'drive steps to correct wheel position to sp, ccw motion is positive
+
+
+
+IF (@ABS[dangle4] > 0.8);
+	SP suslow; AC suslowAC; DC fastDC;
+	PR calc_mv4;
+	BGA;
+	AMA;
+	raw_pos4=_TPA;
+	dangle4a = ((setpoint - raw_pos4)/rstep)
+	A[5]=@ABS[100*dangle4a];
+ENDIF;
+
+'round 5 on MOVE, if needed
+raw_pos5=_TPA;
+angle5 = (raw_pos5/rstep);									' angle from home, ccw is positive displacement
+dangle5 = ((setpoint - raw_pos5)/rstep);		'angle delta from resolver position setpoint, ccw is positive
+calc_mv5 = @INT[((raw_pos5-setpoint)*drstep)]; 'drive steps to correct wheel position to sp, ccw motion is positive
+
+
+
+IF (@ABS[dangle5] > 0.8);
+	SP suslow; AC suslowAC; DC fastDC; 
+	PR calc_mv5;
+	BGA;
+	AMA;
+	raw_pos5=_TPA;
+	dangle5a = ((setpoint - raw_pos5)/rstep)
+	A[5]=@ABS[100*dangle5a];;
+ENDIF;
+
+'MO;
+endtime=TIME;
+deltaT=endtime-bgtime;
+A[4]=deltaT;
+
+A[1]=A[0]; 		'set control data structure current position equal to commanded postion
+A[3]=0;				'set error handler to movement complete
+
+
+
+'MG "current position is: ", _TPA;
+'MG "setpoint is: ", setpoint;
+'endtime=TIME;
+'deltaT=endtime-bgtime;
+'MG "move completed in ", deltaT, " milliseconds";
+
+
+
+JP #EVENTLP;'
+
+'----------------------------------------------------------------
+#HOME
+
+SP slowsp; AC slowAC; DC fastDC;  
+
+
+'MG "Homing Selector Wheel";
+SH;
+HMA;
+'TM 1000;' actually sets an update rate of 976 microseconds. Thus the value returned by the TIME operand will be off by 2.4% of the actual time
+bgtime=TIME;
+BGA;
+AMA;
+'MO;
+home=_TPA;
+endtime=TIME;
+deltaT=endtime-bgtime;
+A[4]=deltaT;
+'DP 0; 'this will keep track of commanded position step counts in DE - is not encoder info
+pos1=home+153;
+pos2=pos1+4096;
+pos3=pos2+4096;
+pos4=pos3+4096;
+A[1]=5; 'set current position indicator to HOME
+curr_pos=A[1];
+A[0]=1; 'force a move to pos1 after HOME
+'MO;
+
+
+JP #EVENTLP
+'----------------------------------------------------------------
+
+

python/python_com_basic.py

@@ -0,0 +1,50 @@
+import sys
+#import time
+#import readline
+#import traceback
+
+from pymodbus.client.sync import ModbusTcpClient
+
+IP1="192.168.42.100"
+client = ModbusTcpClient(IP1)
+
+
+setpoint_addr = 1000
+speed_addr = 1002
+retcode_addr = 1003
+time_addr = 1004
+delta_addr = 1005
+
+
+speed = 1
+setpoint = 2
+
+
+w = client.write_registers(speed_addr, speed)
+w = client.write_registers(setpoint_addr, setpoint)
+if w.isError():
+    print(f"Error writing: {w}")
+else:
+    print(f"Successfully wrote values.")
+
+#time.sleep(5) #delay might be based on speed.  right now, it isn't.
+
+r = client.read_input_registers(delta_addr, 1)
+if r.isError():
+    print(f"Error reading: {r}")
+else:
+    print(f"Read resolver position delta (100x degrees): {r.registers}")
+
+r = client.read_input_registers(retcode_addr, 1)
+if r.isError():
+    print(f"Error reading return code: {r}")
+else:
+    print(f"Read return code: {r.registers}")
+
+r = client.read_input_registers(time_addr, 1)
+if r.isError():
+    print(f"Error reading elapsed time: {r}")
+else:
+    print(f"Read movement time (ms): {r.registers}")
+
+