1. RCX-Code
Remark: This code
works with a three switch expander (see electronics section)
Actually we added
a third touch-sensor to the plotter-head to prevent dammage
when resetting to
zero.
Program-Name:c-plotter 20/3/2000
Robot: plotter1
Tasks:works together with c_plotter.exe
(c_plotter.pas).
After initialization, plotter moves head
to
(alpha=45,beta=45) then, by activating
task_5 waits for go_to_plot=1. This
flag should be set by the computer after
fixing new plotting-coordinates.
###define
h1 = 1
h2 = 2
h3 = 3
x1 = 4
x2 = 5
x3 = 6
alphaAim = 7
betaAim = 8
alphaPosition = 9
betaPosition = 10
switch1 = 11
switch2 = 12
switch3 = 13
RotationSensorValue = 14
addOrSub = 15
angleSensorZeroPoint = 16
go_for_plot = 17
selectprgm(slot_4)
beginoftask(main)
{-------------------------------initialization--------------------------}
setsensortype(sensor_3,no_type)
{home-brew angle sensor}
setsensormode(sensor_3,raw_mode,0)
setsensortype(sensor_2,no_type)
{switch-expander}
setsensormode(sensor_2,raw_mode,0)
setsensortype(sensor_1,light_type)
{home-brew rotation-sensor}
setsensormode(sensor_1,percent_mode,0)
setvar(addorsub,con,1) {determines whether the rotation-sensor
is supposed to add or subtract 1 to the actual sensor
value}
{---------------------------------zero-point-fixing---------------------}
{---------alpha-side-check-----------}
gosub(1) {read touch sensors}
while(var,switch1,eq,con,1) {make sure no
problem in
midth}
gosub(1)
{move both motors to a better position}
setrwd(motor_c)
setfwd(motor_a)
on(motor_a+motor_c)
endwhile()
off(motor_a+motor_c)
gosub(1)
while(var,switch3,EQ,con,0) {no touch}
gosub(1)
while(var,switch1,eq,con,1) {if there is a
problem in the midth}
gosub(1)
setrwd(motor_c) {move beta-beam}
on(motor_c)
endwhile()
off(motor_c)
setrwd(motor_a) {move alpha-beam}
on(motor_a)
endwhile()
off(motor_a)
setvar(angleSensorZeroPoint,con,0) {clear
value}
setvar(alphaposition,con,0) {clear}
gosub(2) {read angleSensor}
setvar(angleSensorZeroPoint,var,alphaposition)
{fix zero-point}
starttask(task_2) {read and store angle
value}
setvar(alphaposition,con,0) {clear again}
gosub(1) {------------beta-side-check----------}
while(var,switch2,EQ,con,0) {no
touch}
gosub(1)
while(var,switch1,eq,con,1)
{make sure no problem in
midth}
gosub(1)
setfwd(motor_a)
on(motor_a)
endwhile()
off(motor_a)
setfwd(motor_c)
on(motor_c)
endwhile()
off(motor_c)
setvar(rotationsensorValue,con,0)
{clearsensor}
starttask(1) {start reading rotation values}
{-----------------------------------------------------------------------------------------}
{-----------goto position (alphaAim=45,betaAim=45)}
setvar(alphaAim,con,45)
setvar(betaAim,con,45)
starttask(task_3)
starttask(task_4)
setvar(h3,var,h1)
sumvar(h3,var,h2) {flags for test}
while(var,h3,gt,con,0)
setvar(h3,var,h1)
sumvar(h3,var,h2) {flags for test}
endwhile()
playsystemsound(sweep_fast_sound)
starttask(task_5) {plot following alphaaim,betaaim
from
the computer}
endoftask(main)
{++++++++++++++++++++++++++++++++++++++++++++++++}
{-----------------------------------rotationsensor-reading---------}
beginoftask(task_1)
loop(con,forever)
while(senval,sensor_1,GT,con,0)
endwhile()
sumvar(RotationSensorValue,var,addOrsub)
while(senval,sensor_1,LT,con,5)
endwhile()
endloop()
endoftask()
{---------------------------------------anglesensor-reading-----------}
beginoftask(task_2)
loop(con,forever)
gosub(2)
endloop()
endoftask()
{-----------------------------------move alpha beam------------------}
beginOftask(task_3)
setvar(h1,con,1) {if motor
is working flag is set to 1}
if(var,alphaposition,LT,var,AlphaAim)
setfwd(motor_a)
on(motor_a)
while(var,alphaposition,LT,var,AlphaAim)
endwhile()
off(motor_a)
setvar(h1,con,0)
else()
setrwd(motor_a)
on(motor_a)
while(var,alphaposition,GT,var,AlphaAim)
endwhile()
off(motor_a)
setvar(h1,con,0)
endif()
endoftask()
{----------------------------------move beta beam ------------------}
beginoftask(task_4)
setvar(h2,con,1)
setvar(betaposition,var,RotationSensorValue)
divvar(betaposition,con,2)
if(var,betaPosition,LT,var,betaAim)
setvar(addOrSub,con,1)
setrwd(motor_c)
on(motor_c)
while(var,betaposition,LT,var,betaAim)
setvar(betaposition,var,RotationSensorValue)
divvar(betaposition,con,2)
endwhile()
off(motor_c)
setvar(h2,con,0)
else()
setvar(addOrSub,con,-1)
setfwd(motor_c)
on(motor_c)
while(var,betaposition,GT,var,betaAim)
setvar(betaposition,var,RotationSensorValue)
divvar(betaposition,con,2)
endwhile()
off(motor_c)
setvar(h2,con,0)
endif()
endoftask()
{=============================================}
beginoftask(task_5)
loop(con,forever)
while(var,go_for_plot,ne,con,1)
endwhile()
{-----------------------move alpha-beam-------------------------}
starttask(task_3)
{---------------------move beta-beam----------------------------}
starttask(task_4)
setvar(h3,var,h1)
sumvar(h3,var,h2) {flags for test: is plotter still plotting}
while(var,h3,gt,con,0)
setvar(h3,var,h1)
sumvar(h3,var,h2) {flags for test}
endwhile()
setvar(go_for_plot,con,0) {reset the plot-permission}
{--------------------------------------------------------------------------}
endloop()
endoftask()
{------------------------------------------------------------------------------------------}
beginofsub(1) {three switch dispatcher}
if(senval,sensor_2,LT,con,768) {111}
setvar(switch1,con,1)
setvar(switch2,con,1)
setvar(switch3,con,1)
else()
if(senval,sensor_2,LT,con,798) {110}
setvar(switch1,con,0)
setvar(switch2,con,1)
setvar(switch3,con,1)
else()
if(senval,sensor_2,LT,con,830) {101}
setvar(switch1,con,1)
setvar(switch2,con,0)
setvar(switch3,con,1)
else()
if(senval,sensor_2,LT,con,869) {100}
setvar(switch1,con,0)
setvar(switch2,con,0)
setvar(switch3,con,1)
else()
if(senval,sensor_2,LT,con,912) {011}
setvar(switch1,con,1)
setvar(switch2,con,1)
setvar(switch3,con,0)
else()
if(senval,sensor_2,LT,con,955) {010}
setvar(switch1,con,0)
setvar(switch2,con,1)
setvar(switch3,con,0)
else()
if(senval,sensor_2,LT,con,1001) {001}
setvar(switch1,con,1)
setvar(switch2,con,0)
setvar(switch3,con,0)
else()
{000}
setvar(switch1,con,0)
setvar(switch2,con,0)
setvar(switch3,con,0)
endif()
endif()
endif()
endif()
endif()
endif()
endif()
endofsub()
{----------------------------------------angle mesurement-----------------}
beginOfSub(2)
setvar(x1,senval,sensor_3) {x1:=input}
setvar(x2,con,27)
{x2:=27}
mulvar(x2,var,x1)
{x2:=x2*x1}
setvar(x3,con,1023)
{x3:=1023}
subvar(x3,var,x1)
{x3:=x3-x1}
divvar(x2,var,x3)
{x2:=x2/x3}
mulvar(x2,con,2)
{x2:=x2*2}
sumvar(x2,con,20)
{x2:=x2+20, error correction}
subvar(x2,var,angleSensorZeroPoint)
{0-point adjusting}
absvar(x2,var,x2) {only positive
values}
setvar(alphaposition,var,x2) {angle
in degres}
endofsub()
unit c_plotter;
interface
uses
Windows,
Messages, SysUtils, Classes, Graphics, Controls, Forms, Dialogs,spirit,
OleCtrls,
StdCtrls, ExtCtrls;
type
TForm_c_Plotter
= class(TForm)
Spirit1: TSpirit;
Shape1: TShape;
Edit1: TEdit;
Label1: TLabel;
Edit2: TEdit;
Label2: TLabel;
Edit3: TEdit;
Label3: TLabel;
Button1: TButton;
Edit4: TEdit;
Label4: TLabel;
Edit5: TEdit;
Label5: TLabel;
procedure FormMouseDown(Sender: TObject; Button: TMouseButton;
Shift: TShiftState; X, Y: Integer);
procedure FormCreate(Sender: TObject);
procedure FormClose(Sender: TObject; var Action: TCloseAction);
procedure Button1Click(Sender: TObject);
procedure FormMouseMove(Sender: TObject; Shift: TShiftState; X,
Y: Integer);
private
{ Private declarations }
procedure polar(posx,posy:integer;var alphaAim,betaAim:smallInt);
public
{ Public declarations }
end;
var
Form_c_Plotter:
TForm_c_Plotter;
implementation
{$R *.DFM}
const L=15;
{beam length-holes}
var first:boolean;
posx,posy,lastx,lasty:integer;
alphaAim,betaAim:smallInt;
procedure TForm_c_Plotter.FormMouseDown(Sender:
TObject; Button: TMouseButton;
Shift: TShiftState;
X, Y: Integer);
var h3,go_for_plot,idx,n:smallint;
temp:TDateTime;
a,b,myX,myY,dx,dist:real;
begin
posx:=x;posy:=y;
with spirit1
do
begin
if first
then
begin
form_c_plotter.cursor:=crhourglass;
form_c_plotter.canvas.moveto(posx,posy);
first:=false;
form_c_plotter.canvas.rectangle(x-2,y-2,x+2,y+2);
polar(posx,posy,alphaAim,betaAim);
temp:=now;
repeat
{wait one hundredth second}
application.processmessages;
until now-temp>1/24/60/60/100;
repeat {wait until plotter ready}
h3:=poll(0,2);
until (h3=0);
setvar(6,2,alphaAim);
setvar(7,2,betaAim);
setvar(16,2,1); {go_for_plot:=1}
lastx:=posx;lasty:=posy;
form_c_plotter.cursor:=crdefault;
end {if first}
else {if not first, draw line}
begin
form_c_plotter.canvas.lineto(posx,posy);
form_c_plotter.canvas.rectangle(x-2,y-2,x+2,y+2);
form_c_plotter.cursor:=crhourglass;
try
a:=(lasty-posy)/(lastx-posx);
b:=lasty-a*lastx;
except on EDivByZero do
exit;
end;
dist:=sqrt((lastx-posx)*(lastx-posx)+(lasty-posy)*(lasty-posy));
n:=trunc(dist*2*L/Form_c_Plotter.clientheight);
dx:=(posx-lastx)/n;
for idx:=0 to n do
begin
myx:=lastx+idx*dx;
myy:=a*myX+b;
polar(trunc(myx),trunc(myy),alphaAim,betaAim);
temp:=now;
repeat
{wait one hundredth second}
application.processmessages;
until now-temp>1/24/60/60/100;
repeat {wait until plotter ready}
h3:=poll(0,2);
until (h3=0);
setvar(6,2,alphaAim);
setvar(7,2,betaAim);
setvar(16,2,1); {go_for_plot:=1}
end; {for}
form_c_plotter.cursor:=crdefault;
lastx:=posx;lasty:=posy;
end;
end;
end;
procedure TForm_c_Plotter.FormCreate(Sender:
TObject);
begin
spirit1.initcomm;
first:=true;
spirit1.setvar(2,2,0);
spirit1.setvar(16,2,0);
end;
procedure TForm_c_Plotter.FormClose(Sender:
TObject; var Action: TCloseAction);
begin
spirit1.closecomm;
end;
procedure TForm_c_Plotter.Button1Click(Sender:
TObject);
var theta,alpha,phi,beta,xx,yy,cosPHi,sinPhi,tanPhi,z:real;
idx:integer;
begin
Form_c_Plotter.canvas.pen.color:=clteal;
Form_c_Plotter.canvas.brush.color:=clteal;
for idx:=1 to 20000
do
begin
posx:=random(clientheight);posy:=random(clientheight);
xx:=posx*2*L/form_c_plotter.clientheight;
yy:=posy*2*L/form_c_plotter.clientheight;
z:=sqrt(xx*xx+yy*yy);
if (z<2*L-2)and(z>4) then
begin
if xx=0 then theta:=0 else theta:=arctan(yy/xx)*180/pi;
cosPhi:=z/L/2;
sinPhi:=1-cosPhi*cosPhi;
tanPhi:=sinPhi/cosPhi;
Phi:=arctan(tanPhi)*180/Pi;
alpha:=Theta-Phi+45;
beta:=90+alpha-2*Theta;
if(alpha>=0) and(beta>=0)then
Form_c_Plotter.canvas.rectangle(posx-2,posy-2,posx+2,posy+2);
end;
end;
Form_c_Plotter.canvas.pen.color:=clblack;
Form_c_Plotter.canvas.brush.color:=clwhite;
end;
procedure TForm_c_Plotter.FormMouseMove(Sender:
TObject;
Shift: TShiftState;
X, Y: Integer);
begin
edit4.text:=inttostr(x);
edit5.text:=inttostr(y);
end;
procedure TForm_c_Plotter.polar(posx,posy:integer;var
alphaAim,betaAim:smallint);
var theta,alpha,phi,beta,xx,yy,cosPHi,sinPhi,tanPhi,z:real;
begin
xx:=posx*2*L/form_c_plotter.clientheight;
yy:=posy*2*L/form_c_plotter.clientheight;
z:=sqrt(xx*xx+yy*yy);
if z>2*L-2 then z:=2*L-2;
if z<4 then z:=4;
edit1.text:=inttostr(trunc(z));
try
theta:=arctan(yy/xx)*180/pi;
except on Exception do theta:=0;
end; {try}
cosPhi:=z/L/2;
sinPhi:=1-cosPhi*cosPhi;
tanPhi:=sinPhi/cosPhi;
Phi:=arctan(tanPhi)*180/Pi;
alpha:=Theta-Phi+45;
beta:=90+alpha-2*Theta;
alphaAim:=trunc(alpha);
edit2.text:=inttostr(alphaaim);
betaAim:=trunc(beta);
edit3.text:=inttostr(betaAim);
if alphaAim<0 then alphaAim:=0;
if betaAim<0 then betaAim:=0;
end;
end.