Программное обеспечение для предварительных испытаний манипулятора грунтозаборного комплекса космического аппарата "Фобос-грунт"

end;

procedure TForm1.Btn_prg_startClick(Sender: TObject);

var

i : word;

pc : word;

s : string;

begin

For i:=0 to LB1.Items.Count-1 do

begin

s:=LB1.Items[i];

Prg[i]:=TProgram_item.create;

//Prg[i].cmd_type:=0;

Prg[i].cmd_str:=s;

//Prg[i].cmd_act:=Get_Element(s,2);

//pc:=Get_char_pos(s,',',2);

//Prg[i].cmd_params:=copy(s,pc+1,Length(s)-pc-1);

end;

//Обнуляем счетчик текущей команды

cur_command:=1;

//Запрещаем изменение программы

Btn_prg_stop.Enabled:=True;

Btn_prg_start.Enabled:=False;

Btn_prg_del.Enabled:=False;

Btn_prg_edit.Enabled:=False;

Btn_prg_ins.Enabled:=False;

Btn_prg_add.Enabled:=False;

Btn_prg_load.Enabled:=False;

Btn_prg_save.Enabled:=False;

Btn_prg_clear.Enabled:=False;

//Переходим в режим передачи программы

Timer_prg.Enabled:=True;

end;

procedure TForm1.Btn_prg_stopClick(Sender: TObject);

begin

//WPMode:=0;

Btn_prg_stop.Enabled:=False;

LB1.ItemIndex:=-1;

Timer_prg.Enabled := False;

Btn_prg_start.Enabled:=True;

Btn_prg_del.Enabled:=True;

Btn_prg_edit.Enabled:=True;

Btn_prg_ins.Enabled:=True;

Btn_prg_add.Enabled:=True;

end;

procedure TForm1.Btn_prg_addClick(Sender: TObject);

begin

Form2.AddCommand;

end;

procedure TForm1.Btn_prg_insClick(Sender: TObject);

begin

Form2.InsertCommand;

end;

procedure TForm1.Btn_prg_editClick(Sender: TObject);

begin

Form2.EditCommand;

end;

procedure TForm1.Btn_prg_delClick(Sender: TObject);

begin

Form2.DeleteCommand;

end;

procedure Prg_run_next_step (s: string);

begin

end;

procedure TForm1.Timer_prgTimer(Sender: TObject);

var

i : word;

begin

if Prg[cur_command].check_finalized()=True

then

begin

Inc(Cur_command);

if cur_command<=Total_commands

then

Prg[cur_command].lunch_cmd

else

begin

Timer_prg.Enabled:=False;

cur_command:=0;

end;

end;

end;

Function TProgram_item.lunch_cmd(): boolean;

var

cmd_array : TArInt20;

j : word;

begin

cmd_array:=Split_str(cmd_str,',');

Case cmd_str[1] of

//Команда на включение вращения манипулятора

'r': begin

for j:=3 to round(Length(cmd_array)/2) do

begin

Motors[cmd_array[j]].set_uMode(Velocity_mode);

Motors[cmd_array[j]].set_requested_velocity(cmd_array[j+1]);

end;

Result:=True;

end;

//Команда на выставку колен манипулятора в некое угловое положение

'a': begin

for j:=3 to round(Length(cmd_array)/2) do

begin

Motors[cmd_array[j]].set_uMode(Velocity_mode);

Motors[cmd_array[j]].set_requested_angle(cmd_array[j+1]);

end;

Result:=True;

end;

//Команда выбора режима слежения программы

'm': begin

end;

't': begin;

cmd_t_count:=cmd_array[2];

Result:=True;

end;

end;

end;

Function TProgram_item.check_finalized(): boolean;

var

cmd_array : TArInt20;

j, mot_cnt, res : word;

begin

showMessage(cmd_str);

cmd_array:=Split_str(cmd_str,',');

Case cmd_str[1] of

//Команда на включение вращения манипулятора

'r': begin

mot_cnt:=round(Length(cmd_array)/2);

res:=0;

for j:=3 to mot_cnt do

begin

if Motors[cmd_array[j]].get_real_velocity()=cmd_array[j+1] then Inc(res);

end;

if mot_cnt=res then check_finalized:=True else check_finalized:=False;

end;

//Команда на выставку колен манипулятора в некое угловое положение

'a': begin

mot_cnt:=round(Length(cmd_array)/2);

res:=0;

for j:=3 to round(Length(cmd_array)/2) do

begin

if Motors[cmd_array[j]].get_real_angle=cmd_array[j+1] then Inc(res);

end;

if mot_cnt=res then check_finalized:=True else check_finalized:=False;

end;

//Команда выбора режима слежения программы

'm': begin

end;

//Команда паузы

't': begin

Dec(cmd_t_count);

if cmd_t_count=0 then check_finalized:=True else check_finalized:=False;

end;

end;

end;

procedure TForm1.Button4Click(Sender: TObject);

var

s: string;

Ar: TArInt20;

i: integer;

w : word;

begin

s:='r,2,3,5,1000,5000,2000';

ar:=split_str(s,',');

for i:=1 to 20 do Memo1.Lines.Add(IntToStr(ar[i]));

end;

end.

Unit Elements;

Interface

type

BufferType = array[0..1024] of char;

TAReal = ARRAY[1..6] of byte;

TArInt20 = ARRAY[1..20] of integer;

var

ONOFF: array[0..1] of string = ('Off', 'On');

{Выбирает из строки с запятыми элемент с номером num}

Function Get_Element(ste:string; num : byte):String;

{Выбираем название предложения из строки, начинающейся с $}

Function Get_String_Name(ste:string): string;

{Выдает количество элементов, разделенных запятыми}

Function Get_Number_of_Elements ( ste: string): byte;

{Процедура поиска символа в строке}

Function ExistIn(ste : string; ch : char):boolean;

{Процедура поиска и определения порядкового номера символа в строке}

Function ExistInSTR(ste : string; ch : char):word;

{Процедура поиска и определения порядкового номера символа в строке}

Function ExistStrInStr(sSource : string; sSearch:string):boolean;

{}

Function StrToReal( s: string): Real;

Function Get_char_pos( s: string; ch : char; Entry : word): word;

Function Split_str(cmd_str : string; delim_char : char) : TArInt20;

Implementation

Function Split_str;

var

cmd_ar: array of integer;

s1, s : string;

j, intv, Code : integer;

cnt : word;

begin

s:='';

s1:=cmd_str+delim_char;

cnt:=1;

for j:=1 to length(s1) do

begin

if s1[j]=delim_char then

begin

Val(s,intv,Code);

if Code=0 then Split_str[cnt]:=intv else Split_str[cnt]:=-1;

Inc(cnt);

s:='';

end

else s:=s+s1[j];

end;

for cnt:=cnt to 20 do Split_str[cnt]:=-1;

end;

Function Get_char_pos;

var

i, j, k : word;

begin

j := Length(s);

k := Entry;

If j>0

then

begin

For i:=1 to j do

begin

If s[i]=ch then Inc(k,-1);

If k=0 then

begin

Get_char_pos:=i;

break;

end;

end;

if k>0 then Get_char_pos:=0

end

else

Get_char_pos:=0

end;

Function StrToReal( s: string): Real;

var

AR6: TAReal;

i : byte;

begin

For i:=1 to 6 do

AR6[i]:=Ord(s[i]);

Result:=Real48(AR6);

end;

{Процедура поиска и определения порядкового номера символа в строке}

Function ExistInSTR(ste : string; ch : char):word;

var

i, j : word;

begin

j:=Length(ste);

If j>0 then

For i:=1 to j do If ste[i]=ch then begin

ExistInStr:=i;

exit;

end

else ExistInStr:=0

else ExistInStr:=0

end;

{Процедура поиска и определения порядкового номера символа в строке}

Function ExistStrInStr(sSource : string; sSearch:string):boolean;

var

i, j : word;

begin

If (SSource='') or (sSearch='')

then

begin

Result:=False;

exit;

end;

If Length(sSource)<Length(sSearch)

then

begin

Result:=False;

exit;

end;

j:=Length(sSearch);

For i:=1 to Length(sSource)-j+1 do

If Copy(SSource,i,j)=sSearch

then

begin

Result:=True;

Exit;

end

else

Result:=False;

end;

{Процедура поиска символа в строке}

Function ExistIn(ste : string; ch : char):boolean;

var

i : word;

v: boolean;

begin

v:=False;

If Length(ste)=1 then begin

If ste[1]=ch then ExistIn:=True

else ExistIn:=False;

Exit;

end;

For i:=1 to Length(ste) do If ste[i]=ch then v:=True;

ExistIn:=v;

end;

{Выбираем название предложения из строки, начинающейся с $}

Function Get_String_Name(ste:string): string;

var

i : word;

begin

If (ste='') or not ExistIn(ste,',')

then begin

Get_String_Name:='';

Exit;

end

else begin

i:=1;

Repeat

i:=i+1;

Until (ste[i]=',') or (I>Length(ste)+1);

If ste[1]='$' then Get_String_Name:=Copy(ste,2,i-2)

else Get_String_Name:='';

end;

end;

{Выдает количество элементов, разделенных запятыми}

Function Get_Number_of_Elements(ste: string): byte;

var

i, k : byte;

begin

IF ste='' then begin Get_Number_of_Elements:=0;Exit;end;

i:=1;

k:=1;

Repeat

i:=i+1;

If ste[i]=',' then k:=k+1;

Until i=Length(ste);

Get_Number_of_Elements:=k;

end;

Function Get_Element(ste:string; num : byte):String;

var k, k1, i : byte;

stv:string;

begin

stv:=ste+',';

IF num=1 then begin

k:=0;

Repeat k:=k+1; Until stv[k]=',';

If k=1 then Get_Element:=''

else Get_Element:=Copy(stv,1,k-1);

Exit;

end

else begin

k:=0;

i:=0;

{Просматриваем строку до num-той запятой}

Repeat

i:=i+1;

If stv[i]=',' then k:=k+1; {Увеличиваем счетчик запятых}

If k=num-2 then k1:=i+1; {Сохраняем номер запятой до нужного элемента}

Until (k=num) or (i>=Length(stv)); {Выходим достигнув запятой после нужного элемента}

If I< Length(stv)+1 then

If k1+1=i then Get_Element:=''

else Get_Element:=Copy(stv,k1+1,i-k1-1);

end;

end;

end.

unit Commands;

interface

uses

Windows, Messages, SysUtils, Classes, Graphics, Controls, Forms, Dialogs,

StdCtrls,

Elements;

type

TForm2 = class(TForm)

Label1: TLabel;

Label2: TLabel;

Label3: TLabel;

ComboCommand: TComboBox;

ComboTypeParam: TComboBox;

EditParam: TEdit;

Button1: TButton;

Button2: TButton;

procedure Button1Click(Sender: TObject);

procedure Button2Click(Sender: TObject);

procedure FormCreate(Sender: TObject);

private

{ Private declarations }

public

{ Public declarations }

{Параметры передачи программы}

PCommand:String;

Pparam : String;

PTypeParam:string;

PEntered :boolean;

EChar : char;

procedure AddCommand;

procedure InsertCommand;

procedure EditCommand;

procedure DeleteCommand;

end;

var

Form2: TForm2;

implementation

uses unit1;

{$R *.DFM}

procedure TForm2.AddCommand;

begin

//Задаем признак необходимого действия

EChar:='a';

//Очищаем все элементы программы

ComboCommand.Text:='';

EditParam.Text:='';

ComboTypeParam.Text:='';

//Выводим окно ввода команды

Form2.Show;

end;

procedure TForm2.InsertCommand;

begin

//Проверяем наличие выбранного элемента списка комманд

If Form1.LB1.ItemIndex<0 then Exit;

//Задаем признак необходимого действия

EChar:='i';

//Очищаем все элементы программы

ComboCommand.Text:='';

EditParam.Text:='';

ComboTypeParam.Text:='';

//Выводим окно ввода команды

Form2.Show;

end;

procedure TForm2.EditCommand;

var

s : string;

begin

//Проверяем наличие выбранного элемента списка комманд

If Form1.LB1.ItemIndex<0 then Exit;

//Задаем признак необходимого действия

EChar:='e';

//Читаем элемент программы

s:=Form1.LB1.Items.Strings[Form1.LB1.ItemIndex];

//Задаем текущие значения элементов программы

ComboCommand.Text :=Get_Element(s,1);

EditParam.Text :=Get_Element(s,2);

ComboTypeParam.Text :=Get_Element(s,3);

//Выводим окно ввода команды

Form2.Show;

end;

procedure TForm2.DeleteCommand;

begin

//Проверяем наличие выбранного элемента списка комманд

If Form1.LB1.ItemIndex<0 then Exit;

//Удаляем команду из программы

Form1.LB1.Items.Delete(Form1.LB1.ItemIndex);

//Запрещаем очистку и сохранение текста программы при отсутствии текста

If Form1.LB1.Items.Count<1 then begin

Form1.Btn_prg_start.Enabled:=False;

Form1.Btn_prg_del.Enabled:=False;

Form1.Btn_prg_edit.Enabled:=False;

Form1.Btn_prg_ins.Enabled:=False;

end;

end;

procedure TForm2.Button1Click(Sender: TObject);

var

i, i1, i2, Code, Code1, Code2 : integer;

s, ss, ss2, s1, s2, s3: string;

Fault : boolean;

begin

Fault:=False;

//Формируем первичный текст программы

s:=ComboCommand.Text+','+EditParam.Text+','+ComboTypeParam.Text;

//Проверяем правильность команды

//Выделяем команду

s1:=Get_Element(s,1);

//Выделяем тип параметра

s2:=Get_Element(s,3);

//Выделяем значение параметра

s3:=Get_Element(s,2);

//Выделяем описание команды

ss:=Form1.GetCodeDescription(s1);

//Проверяем соответствие типа параметра

If not ((s2=Get_Element(ss,3))

or (s2=Get_Element(ss,4))

or (s2=Get_Element(ss,5)))

then

begin

ShowMessage('Тип параметра не соответствует команде!');

Fault:=True;

end;

//Проверяем допустимое значение параметра

If s2=Get_Element(ss,3) then

begin

i1:=StrToInt(Get_Element(ss,6));

Val(s3,i2,Code);

If (Code<>0) then

begin

ShowMessage('Значение параметра - не число!');

Fault:=True;

end

else

If i2>i1 then

begin

ShowMessage('Превышено значение параметра!');

Fault:=True;

end

end;

If s2=Get_Element(ss,4) then

begin

ss2:=Get_Element(ss,7);

Val(ss2,i1,Code1);

Val(s3,i2,Code2);

If (Code1<>0) or (Code2<>0)

then

begin

ShowMessage('Значение параметра - не число!');

Fault:=True;

end

else

If i2>i1 then

begin

ShowMessage('Превышено значение параметра!');

Fault:=True;

end

end;

If s2=Get_Element(ss,5) then

begin

i1:=StrToInt(Get_Element(ss,8));

Val(s3,i2,Code);

If Code<>0 then

begin

ShowMessage('Значение параметра - не число!');

Fault:=True;

end

else

If i2>i1 then

begin

ShowMessage('Превышено значение параметра!');

Fault:=True;

end

end;

If Fault then Exit;

//В зависимости от запроса вносим изменения в программу

Case EChar of

//Изменяем элемент программы

'e': begin

Form1.LB1.Items.Strings[Form1.LB1.ItemIndex]:=s;

end;

//Добавляем новый элемекнт в конец программы

'a': begin

Form1.LB1.Items.Add(s);

end;

//Вставляем новый элемент в указанное курсором место

'i': begin

Form1.LB1.Items.Insert(Form1.LB1.ItemIndex,s);

end;

end;

//

Form1.Btn_prg_start.Enabled:=True;

Form1.Btn_prg_del.Enabled:=True;

Form1.Btn_prg_edit.Enabled:=True;

Form1.Btn_prg_ins.Enabled:=True;

//Скрываем форму ввода команды

Form2.Hide;

end;

procedure TForm2.Button2Click(Sender: TObject);

begin

//Скрываем форму ввода команды

Form2.Hide;

end;

procedure TForm2.FormCreate(Sender: TObject);

var

i : word;

begin

For i:=0 to Form1.SGC.Lines.count-1 do

ComboCommand.Items.Add(Get_Element(Form1.SGC.Lines.Strings[i],1));

end;

end.

Unit Definitions;

{$D+,S+,L+}

interface

Uses Math, Messages;

type

short_string = string[30];

Tmotor = class

public

m_enabled : boolean;

ADC_enabled : boolean;

m_dErrorCode : longword;

private

s_dev_n : string;

s_pr_n : string;

s_in_n : string;

s_p_n1 : string;

m_connected : boolean;

m_uMode : shortint;

m_KeyHandle : longint;

m_wNodeId : word;

real_velocity : longint;

requested_velocity : longint;

real_current : longint;

requested_current : longint;

zero_pulse_value : longint;

requested_angle : real;

ADC_angle : real;

ADC_chan_id : integer;

public

constructor create;

function connect : boolean;

function set_connection_parameters ( s_dev: string; s_pr : string; s_in: string; s_p : string) : boolean;

function get_conn_params_str():string;

function get_uMode : shortint;

function set_uMode ( arequested_uMode : shortint) : boolean;

function set_velocity ( arequested_velocity : longint) : boolean;

function set_requested_velocity ( arequested_velocity : longint) : boolean;

function set_current ( arequested_current : longint) : boolean;

function set_pulse_position ( arequested_position : longint) : boolean;

function set_zero_pulse() : boolean;

function disable_connection : boolean;

function get_req_velocity : longint ;

function get_real_velocity : longint ;

function get_real_current : longint;

function get_real_pulse_position : longint;

function get_zero_pulse() : longint;

function reset_counter : longword;

function set_requested_angle(angle : real):boolean;

function get_requested_angle : real;

function get_real_angle : real;

function calc_speed( diff: real) : longint;

function calc_motors_speed : longint;

function calc_adc_speed : longint;

function calc_velocity_speed : longint;

function set_ADC_angle(angle : real):boolean;

function get_ADC_angle : real;

function Set_ADC_chan_id(chan_id : integer) : boolean;

function Get_ADC_chan_id() : integer;

end;

const

//Motor operation modes definition

Step_direction_mode =-6; // 'Step/Direction Mode';

Master_encod_mode = -5; // 'Master Encoder Mode';

Current_mode = -3; // 'Current Mode';

Velocity_mode = -2; // 'Velocity Mode';

Position_mode = -1; // 'Position Mode';

Profile_position_mode = 1 ; // 'Profile Position Mode';

Profile_velocity_mode = 3; // 'Profile Velocity Mode';

Homing_mode = 6; // 'Homing Mode';

Pulse_angle_rate : real = 2.365e-5;

Limit_angle=8; //При разнице углового положения больше этой величины моторам дается макс. скорость

zero_angle_limits = 0.002;

Velocity_rate: word =200;

Velocity_max = 8000;

Velocity_angle_rate = 800;

Acceleration_max = 800;

{Initialisation}

function VCS_OpenDevice(DeviceName:PChar;ProtocolStackName:PChar;InterfaceName:PChar;PortName:PChar;var pErrorCode:longword):longint;stdcall;external 'EposCmd.dll' name '_VCS_OpenDevice@20'

function VCS_CloseAllDevices(var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name'_VCS_CloseAllDevices@4'

function VCS_CloseDevice(KeyHandle:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_CloseDevice@8'

function VCS_GetProtocolStackSettings(KeyHandle:longint;var pBaudrate:longint;var pTimeout:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetProtocolStackSettings@16'

function VCS_SetProtocolStackSettings(KeyHandle:longint;Baudrate:longint;Timeout:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetProtocolStackSettings@16'

{Help functions}

function VCS_GetBaudrateSelection(DeviceName:PChar;ProtocolStackName:PChar;InterfaceName:PChar;PortName:PChar;StartOfSelection:boolean;var pBaudrateSel:longint;VAR pEndOfSelection:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetBaudrateSelection@32'

function VCS_GetDeviceName(KeyHandle:longint;pDeviceName:PChar;MaxStrSize:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetDeviceName@16'

function VCS_GetDeviceNameSelection(StartOfSelection:boolean;pDeviceNameSel:PChar;MaxStrSize:word;var pEndOfSelection:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetDeviceNameSelection@20'

function VCS_GetDriverInfo(pLibraryName:PChar;MaxStrNameSize:word;pLibraryVersion:PChar;MaxStrVersionSize:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetDriverInfo@20'

function VCS_GetInterfaceName(KeyHandle:longint;pInterfaceName:PChar;MaxStrSize:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetInterfaceName@16'

function VCS_GetInterfaceNameSelection(DeviceName:PChar;ProtocolStackName:PChar;StartOfSelection:boolean;pInterfaceNameSel:PChar;MaxStrSize:word;var pEndOfSelection:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetInterfaceNameSelection@28'

function VCS_GetKeyHandle(DeviceName:PChar;ProtocolStackName:PChar;InterfaceName:PChar;PortName:PChar;var pKeyHandle:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetKeyHandle@24'

function VCS_GetPortName(KeyHandle:longint;pPortName:PChar;MaxStrSize:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetPortName@16'

function VCS_GetPortNameSelection(DeviceName:PChar;ProtocolStackName:PChar;InterfaceName:PChar;StartOfSelection:boolean;pPortSel:PChar; MaxStrSize:word;var pEndOfSelection:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetPortNameSelection@32'

function VCS_GetProtocolStackName(KeyHandle:longint;pProtocolStackName:PChar;MaxStrSize:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetProtocolStackName@16'

function VCS_GetProtocolStackNameSelection(DeviceName:PChar;StartOfSelection:boolean;pProtocolStackNameSel:PChar;MaxStrSize:word;var pEndOfSelection:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetProtocolStackNameSelection@24'

{Configuration}

function VCS_GetCurrentRegulatorGain(KeyHandle:longint;NodeId:word;var pP:longint;var pI:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetCurrentRegulatorGain@20'

function VCS_GetEncoderParameter(KeyHandle:longint;NodeId:word;var pCounts:word;var pPositionSensorType:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetEncoderParameter@20'

function VCS_GetMotorParameter(KeyHandle:longint;NodeId:word;var pMotorType:word;var pContinuousCurrent:word;var pPeakCurrent:word; var pPolePair:byte;var pThermalConstant:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetMotorParameter@32'

function VCS_GetPositionRegulatorGain(KeyHandle:longint;NodeId:word;var pP:word;var pI:word;var pD:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetPositionRegulatorGain@24'

function VCS_GetVelocityRegulatorGain(KeyHandle:longint;NodeId:word;var pP:word;var pI:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetVelocityRegulatorGain@20'

function VCS_SetCurrentRegulatorGain(KeyHandle:longint;NodeId:word;P:word;I:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetCurrentRegulatorGain@20'

function VCS_SetEncoderParameter(KeyHandle:longint;NodeId:word;Counts:word;PositionSensorType:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetEncoderParameter@20'

function VCS_SetMotorParameter(KeyHandle:longint;NodeId:word;MotorType:word;ContinuousCurrent:word;PeakCurrent:word;PolePair:byte;ThermalConstant:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetMotorParameter@32'

function VCS_SetPositionRegulatorGain(KeyHandle:longint;NodeId:word;P:word;I:word;D:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetPositionRegulatorGain@24'

function VCS_SetVelocityRegulatorGain(KeyHandle:longint;NodeId:word;P:word;I:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetVelocityRegulatorGain@20'

{Current Mode}

function VCS_GetCurrentMust(KeyHandle:longint;NodeId:word;var pCurrentMust:smallint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetCurrentMust@16'

function VCS_SetCurrentMust(KeyHandle:longint;NodeId:word;CurrentMust:smallint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetCurrentMust@16'

{Homing Mode}

function VCS_FindHome(KeyHandle:longint;NodeId:word;HomingMode:shortint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_FindHome@16'

function VCS_GetHomingParameter(KeyHandle:longint;NodeId:word;var pHomingAcceleration:longword;var pSpeedSwitch:longword;var pSpeedIndex:longword;var pHomeOffset:longint;var pCurrentThreshold:longword;var pHomePosition:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetHomingParameter@36'

function VCS_SetHomingParameter(KeyHandle:longint;NodeId:word;HomingAcceleration:longword;SpeedSwitch:longword;SpeedIndex:longword;HomeOffset:longint;CurrentThreshold:longword;HomePosition:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetHomingParameter@36'

function VCS_StopHoming(KeyHandle:longint;NodeId:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_StopHoming@12'

{Inputs Outputs}

function VCS_DigitalInputConfiguration(KeyHandle:longint;NodeId:word;DigInputNb:word;Configuration:word;Mask:boolean;Polarity:boolean;ExecutionMask:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_DigitalInputConfiguration@32'

function VCS_DigitalOutputConfiguration(KeyHandle:longint;NodeId:word;DigOutputNb:word;Configuration:word;State:boolean;Mask:boolean;Polarity:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_DigitalOutputConfiguration@32'

function VCS_GetAllDigitalInputs(KeyHandle:longint;NodeId:word;var pInputs:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetAllDigitalInputs@16'

function VCS_GetAllDigitalOutputs(KeyHandle:longint;NodeId:word;var pOutputs:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetAllDigitalOutputs@16'

function VCS_SetAllDigitalOutputs(KeyHandle:longint;NodeId:word;Outputs:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetAllDigitalOutputs@16'

function VCS_GetAnalogInput(KeyHandle:longint;NodeId:word;Number:word;var pAnalog:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetAnalogInput@20'

{Motion Info}

function VCS_GetCurrentIs(KeyHandle:longint;NodeId:word;var pCurrentIs:smallint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetCurrentIs@16'

function VCS_GetMovementState(KeyHandle:longint;NodeId:word;var pTargetReached:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetMovementState@16'

function VCS_GetPositionIs(KeyHandle:longint;NodeId:word;var pPositionIs:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetPositionIs@16'

function VCS_GetVelocityIs(KeyHandle:longint;NodeId:word;var pVelocityis:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetVelocityIs@16'

{Position Mode}

function VCS_GetPositionMust(KeyHandle:longint;NodeId:word;var pPositionMust:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetPositionMust@16'

function VCS_SetPositionMust(KeyHandle:longint;NodeId:word;PositionMust:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetPositionMust@16'

{Profile Position Mode }

function VCS_HaltPositionMovement(KeyHandle:longint;NodeId:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_HaltPositionMovement@12'

function VCS_MoveToPosition(KeyHandle:longint;NodeId:word;TargetPosition:longint;AbsolutePos:boolean;Immediately:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_MoveToPosition@24'

function VCS_GetTargetPosition(KeyHandle:longint;NodeId:word;var pTargetPosition:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetTargetPosition@16'

function VCS_GetPositionProfile(KeyHandle:longint;NodeId:word;var pProfileVelocity:longword;var pProfileAcceleration:longword;var pProfileDeceleration:longword;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetPositionProfile@24'

function VCS_SetPositionProfile(KeyHandle:longint;NodeId:word;ProfileVelocity:longword;ProfileAcceleration:longword;ProfileDeceleration:longword;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetPositionProfile@24'

{Profile Velocity Mode}

function VCS_HaltVelocityMovement(KeyHandle:longint;NodeId:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_HaltVelocityMovement@12'

function VCS_MoveWithVelocity(KeyHandle:longint;NodeId:word;TargetVelocity:longword;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_MoveWithVelocity@16'

function VCS_GetTargetVelocity(KeyHandle:longint;NodeId:word;var pTargetVelocity:longword;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetTargetVelocity@16'

function VCS_GetVelocityProfile(KeyHandle:longint;NodeId:word;var pProfileAcceleration:longword;var pProfileDeceleration:longword;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetVelocityProfile@20'

function VCS_SetVelocityProfile(KeyHandle:longint;NodeId:word;ProfileAcceleration:longword;ProfileDeceleration:longword;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetVelocityProfile@20'

{State Machine}

function VCS_ClearFault(KeyHandle:longint;NodeId:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_ClearFault@12'

function VCS_GetDisableState(KeyHandle:longint;NodeId:word;var pIsDisabled:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetDisableState@16'

function VCS_GetEnableState(KeyHandle:longint;NodeId:word;var pIsEnabled:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetEnableState@16'

function VCS_GetFaultState(KeyHandle:longint;NodeId:word;var pIsInFault:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetFaultState@16'

function VCS_GetOperationMode(KeyHandle:longint;NodeId:word;var pMode:shortint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetOperationMode@16'

function VCS_GetQuickStopState(KeyHandle:longint;NodeId:word;var pIsQuickStopped:boolean;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetQuickStopState@16'

function VCS_SendNMTService(KeyHandle:longint;NodeId:word;CommandSpecifier:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SendNMTService@16'

function VCS_SetDisableState(KeyHandle:longint;NodeId:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetDisableState@12'

function VCS_SetEnableState(KeyHandle:longint;NodeId:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetEnableState@12'

function VCS_SetOperationMode(KeyHandle:longint;NodeId:word;Mode:shortint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetOperationMode@16'

function VCS_SetQuickStopState(KeyHandle:longint;NodeId:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetQuickStopState@12'

{Utilities}

function VCS_GetObject(KeyHandle:longint;NodeId:word;ObjectIndex:word;ObjectSubIndex:byte;var pData:Pointer;NbOfBytesToRead:longword;var pNbOfBytesRead:longword;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetObject@32'

function VCS_GetVersion(KeyHandle:longint;NodeId:word;var pHardwareVersion:word;var pSoftwareVersion:word;var pApplicationNumber:word;var pApplicationVersion:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetVersion@28'

function VCS_Restore(KeyHandle:longint;NodeId:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_Restore@12'

function VCS_SetObject(KeyHandle:longint;NodeId:word;ObjectIndex:word;ObjectSubIndex:byte;Data:Pointer;NbOfBytesToWrite:longword;var pNbOfBytesWritten:longword;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetObject@32'

function VCS_Store(KeyHandle:longint;NodeId:word;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_Store@12'

{Velocity Mode}

function VCS_GetVelocityMust(KeyHandle:longint;NodeId:word;var pVelocityMust:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_GetVelocityMust@16'

function VCS_SetVelocityMust(KeyHandle:longint;NodeId:word;VelocityMust:longint;var pErrorCode:longword):boolean;stdcall;external 'EposCmd.dll' name '_VCS_SetVelocityMust@16'

{General Gateway}

function VCS_SendCANFrame(KeyHandle:longint;CobID:word;Length:word;var pData:Pointer;var pErrorCode:longword):longint;stdcall;external 'EposCmd.dll' name '_VCS_SendCANFrame@20'

function VCS_RequestCANFrame(KeyHandle:longint;CobID:word;Length:word;var pData:Pointer;var pErrorCode:longword):longint;stdcall;external 'EposCmd.dll' name '_VCS_RequestCANFrame@20'

{Dialog Layer}

function VCS_OpenDeviceDlg(var pErrorCode:longword):longint;stdcall;external 'EposCmd.dll' name '_VCS_OpenDeviceDlg@4'

// Custom functions

function Get_motor_mode_name(motor_mode_number : shortint) : short_string;

implementation

function Get_motor_mode_name;

begin

case motor_mode_number of

-6: Result := 'Step/Direction Mode';

-5: Result := 'Master Encoder Mode';

-3: Result := 'Current Mode';

-2: Result := 'Velocity Mode';

-1: Result := 'Position Mode';

1: Result := 'Profile Position Mode';

3: Result := 'Profile Velocity Mode';

6: Result := 'Homing Mode';

else

Result := 'Unknown Mode';

end;

end;

constructor Tmotor.create;

begin

end;

function Tmotor.connect;

begin

if m_connected then

begin

Result := True;

exit;

end;

m_KeyHandle:=VCS_OpenDevice(Pchar(s_dev_n), Pchar(s_pr_n), Pchar(s_in_n), Pchar(s_p_n1), m_dErrorCode);

m_wNodeId:=0;

if (m_dErrorCode=0) then

begin

VCS_ClearFault(m_KeyHandle, m_wNodeId, m_dErrorCode);

VCS_SetEnableState(m_KeyHandle,m_wNodeId,m_dErrorCode);

if m_dErrorCode=0 then

begin

Result:=True;

m_connected:=True;

end

else

begin

Result:=False;

m_connected:=False;

end

end

else

begin

Result:=False;

m_connected:=False;

end;

end;

function TMotor.get_conn_params_str;

begin

Result:='Dev No.:'+s_dev_n+'Pr No.:'+s_pr_n+'In No.:'+s_in_n+'P No.:'+s_p_n1;

end;

function Tmotor.set_connection_parameters;

begin

s_dev_n := s_dev;

s_pr_n := s_pr;

s_in_n := s_in;

s_p_n1 := s_p;

m_wNodeId:=0;

Result:=True;

end;

function Tmotor.set_zero_pulse;

begin

zero_pulse_value:=get_real_pulse_position;

Result:=True;

end;

function Tmotor.set_requested_angle;

begin

requested_angle:=angle;

Result:=True;

end;

function Tmotor.get_requested_angle;

begin

Result:=requested_angle;

end;

function Tmotor.set_adc_angle;

begin

adc_angle:=angle;

Result:=True;

end;

function Tmotor.get_adc_angle;

begin

Result:=adc_angle;

end;

function Tmotor.calc_speed;

var

new_velocity : longint;

begin

//Calc needed speed;

if Abs(diff)>Limit_angle

then

begin

//Calc new speed

new_velocity:=sign(diff)*Velocity_max;

//check with acceleration limits

if ( Abs(new_velocity-get_real_velocity)>Acceleration_max)

then new_velocity:=get_real_velocity+sign(diff)*Acceleration_max

else new_velocity:=new_velocity;

end

else new_velocity:=Round(diff*velocity_angle_rate);

//Check with zero limits

if (Abs(diff)<zero_angle_limits) then new_velocity:=0;

//Output the result

Result:=new_velocity;

end;

function TMotor.calc_motors_speed;

begin

Result:=Calc_speed(requested_angle-get_real_angle);

end;

function TMotor.calc_adc_speed;

begin

Result:=Calc_speed(requested_angle-get_adc_angle);

end;

function Tmotor.get_real_angle;

begin

Result:=Pulse_angle_rate*(get_real_pulse_position-get_zero_pulse);

end;

function Tmotor.get_uMode;

var

Res: shortint;

begin

VCS_GetOperationMode(m_KeyHandle, m_wNodeId, Res, m_dErrorCode);

Result:=Res;

end;

function Tmotor.set_uMode;

var

Res: boolean;

begin

if (Get_motor_mode_name(arequested_uMode)<>'Unknown Mode') then

Res:= VCS_SetOperationMode(m_KeyHandle, m_wNodeId, arequested_uMode, m_dErrorCode);

if res then m_uMode:=arequested_uMode;

Result:=Res;

end;

function Tmotor.set_velocity;

var

Res : boolean;

begin

Res:=VCS_SetVelocityMust(m_KeyHandle, m_wNodeId, arequested_velocity, m_dErrorCode);

Result:=Res;

end;

function Tmotor.set_requested_velocity;

begin

requested_velocity:= arequested_velocity;

Result:=True;

end;

function Tmotor.calc_velocity_speed;

var

diff : longint;

new_velocity : longint;

reqd_spd, real_spd : longint;

begin

reqd_spd := requested_velocity;

real_spd := get_real_velocity;

//Check for rotating dirrection change in this case we use at requested speed zero speed

if ( (sign(real_spd)<>sign(0)) AND (sign(reqd_spd)<>sign(real_spd)) ) then reqd_spd:=0;

diff:= reqd_spd - real_spd;

//Calc needed speed;

if Abs(diff)>Acceleration_max

then

new_velocity:=real_spd+sign(diff)*Acceleration_max

else

new_velocity:=reqd_spd;

//Output the result

Result:=new_velocity;

end;

function Tmotor.set_current;

var

Res : boolean;

begin

Res:=VCS_SetCurrentMust(m_KeyHandle, m_wNodeId, arequested_current, m_dErrorCode);

if Res then requested_current:= arequested_current;

Result:=Res;

end;

function Tmotor.disable_connection : boolean;

begin

VCS_SetDisableState(m_KeyHandle, m_wNodeId, m_dErrorCode);

VCS_CloseDevice(m_KeyHandle, m_dErrorCode);

end;

function Tmotor.get_req_velocity : longint ;

begin

Result := requested_velocity;

end;

function Tmotor.get_real_velocity : longint ;

var

real_velocity:integer;

begin

VCS_GetVelocityIs(m_KeyHandle,m_wNodeId, real_velocity, m_dErrorCode);

Result := real_velocity;

end;

function Tmotor.get_zero_pulse : longint ;

var

real_velocity:integer;

begin

Result := zero_pulse_value;

end;

function Tmotor.get_real_current : longint;

var

real_current: smallint;

begin

VCS_GetCurrentIs(m_KeyHandle,m_wNodeId, real_current, m_dErrorCode);

Result := real_current;

end;

function TMotor.get_real_pulse_position : longint;

var

position : longint;

begin

VCS_GetPositionIs(m_KeyHandle, m_wNodeId, position, m_dErrorCode);

Result:=position;

end;

function TMotor.set_pulse_position;

begin

Result:=VCS_SetPositionMust(m_KeyHandle, m_wNodeId, arequested_position, m_dErrorCode);

end;

function Tmotor.reset_counter;

var

r:boolean;

res_wr: longword;

pdata: pointer;

dd: longint;

obj_index : word;

obj_subindex: byte;

begin

Obj_Index:=24676;

Obj_Index:=6064;

Obj_SubIndex:=0;

dd:=34;

pdata:=@dd;

r:=VCS_SetObject(m_KeyHandle, m_wNodeId,Obj_Index,Obj_SubIndex,pdata,4, res_wr, m_dErrorCode);

Result:=res_wr;

end;

function TMotor.Set_ADC_chan_id;

begin

ADC_chan_id:=chan_id;

Result:=True;

end;

function TMotor.Get_ADC_chan_id;

begin

Result:=ADC_chan_id;

end;

end.



Приложение Б

Оборудование необходимое для подключения МП ГЗК ФМ 4110-200 при НКУ

Для проведения испытаний необходимо следующее оборудование:

- Компьютер работающий под управлением ОС Windows XP, оснащенный 6-ю портами RS-232.

- Контролеры EPOS 24/5 - 6 шт.

- Блок питания с выходным напряжением 24В и мощностью не менее 24Вт.

- Тепловая камера

- Испытуемый манипулятор

- Соединительные кабели.

СО ФМ-4110-200

Размещено на http://www.allbest.ru/

Размещено на http://www.allbest.ru/

Блок схема подключения функционирования МП ГЗК ФМ 4110-200 при НКУ

Оборудование необходимое для подключения МП ГЗК ФМ 4110-200 при климатических испытаниях

Для проведения испытаний необходимо следующее оборудование:

- Компьютер работающий под управлением ОС Windows XP, оснащенный 6-ю портами RS-232.

- Контролеры EPOS 24/5 - 6 шт.

- Блок питания с выходным напряжением 24В и мощностью не менее 24Вт.

- Тепловая камера

- Испытуемый манипулятор

- Соединительные кабели.

Размещено на http://www.allbest.ru/

Размещено на http://www.allbest.ru/

Блок схема подключения функционирования МП ГЗК ФМ 4110-200 при климатических испытаниях



Приложение В

Установка МП-ГЗК на КА с узлом зачековки

Размещено на Allbest.ru