Приложение Г. Листинг библиотеки адаптивного опускания ног Movemets.cpp
#include "StdAfx.h"
#include "Movements.h"
char* down(char mask, bool write)
{
int sens_lp=0;
int sens_ls=0;
int sens_lz=0;
int sens_pp=0;
int sens_ps=0;
int sens_pz=0;
int position_lp=Current_position.lp_vertical;
int position_ls=Current_position.ls_vertical;
int position_lz=Current_position.lz_vertical;
int position_pp=Current_position.pp_vertical;
int position_ps=Current_position.ps_vertical;
int position_pz=Current_position.pz_vertical;
bool DownLpComplete=true;
bool DownLsComplete=true;
bool DownLzComplete=true;
bool DownPpComplete=true;
bool DownPsComplete=true;
bool DownPzComplete=true;
if (mask & PUT_LP)
DownLpComplete=false;
if (mask & PUT_LS)
DownLsComplete=false;
if (mask & PUT_LZ)
DownLzComplete=false;
if (mask & PUT_PP)
DownPpComplete=false;
if (mask & PUT_PS)
DownPsComplete=false;
if (mask & PUT_PZ)
DownPzComplete=false;
bool Implementation_complete=false;
int ExCodeLp=0;
int ExCodeLs=0;
int ExCodeLz=0;
int ExCodePp=0;
int ExCodePs=0;
int ExCodePz=0;
while (Implementation_complete==false)
{
if(DownLpComplete==false)
{
ReadSensorsLegs();
sens_lp=RDPACKAGE[lp_leg];
if ((sens_lp!=255)&&(position_lp<50))
{
position_lp+=corr_step;
AddAgrumentToCommand(Lp_Leg,position_lp);
}
if ((sens_lp==255)&&(position_lp<=52))
{
ExCodeLp=1;
DownLpComplete=true;
}
if ((sens_lp!=255)&&(position_lp>=50))
{
ExCodeLp=2;
DownLpComplete=true;
}
}
if(DownLsComplete==false)
{
ReadSensorsLegs();
sens_ls=RDPACKAGE[ls_leg];
if ((sens_ls!=255)&&(position_ls<50))
{
position_ls+=corr_step;
AddAgrumentToCommand(Ls_Leg,position_ls);
}
if ((sens_ls==255)&&(position_ls<=52))
{
ExCodeLs=1;
DownLsComplete=true;
}
if ((sens_ls!=255)&&(position_ls>=50))
{
ExCodeLs=2;
DownLsComplete=true;
}
}
if(DownLzComplete==false)
{
ReadSensorsLegs();
sens_lz=RDPACKAGE[lz_leg];
if ((sens_lz!=255)&&(position_lz<50))
{
position_lz+=corr_step;
AddAgrumentToCommand(Lz_Leg,position_lz);
}
if ((sens_lz==255)&&(position_lz<=52))
{
ExCodeLz=1;
DownLzComplete=true;
}
if ((sens_lz!=255)&&(position_lz>=50))
{
ExCodeLz=2;
DownLzComplete=true;
}
}
if(DownPpComplete==false)
{
ReadSensorsLegs();
sens_pp=RDPACKAGE[pp_leg];
if ((sens_pp!=255)&&(position_pp>-50))
{
position_pp-=corr_step;
AddAgrumentToCommand(Pp_Leg,position_pp);
}
if ((sens_pp==255)&&(position_pp>=-52))
{
ExCodePp=1;
DownPpComplete=true;
}
if ((sens_pp!=255)&&(position_pp<=-50))
{
ExCodePp=2;
DownPpComplete=true;
}
}
if(DownPsComplete==false)
{
ReadSensorsLegs();
sens_ps=RDPACKAGE[ps_leg];
if ((sens_ps!=255)&&(position_ps>-50))
{
position_ps-=corr_step;
AddAgrumentToCommand(Ps_Leg,position_ps);
}
if ((sens_ps==255)&&(position_ps>=-52))
{
ExCodePs=1;
DownPsComplete=true;
}
if ((sens_ps!=255)&&(position_ps<=-50))
{
ExCodePs=2;
DownPsComplete=true;
}
}
if(DownPzComplete==false)
{
ReadSensorsLegs();
sens_pz=RDPACKAGE[pz_leg];
if ((sens_pz!=255)&&(position_pz>-50))
{
position_pz-=corr_step;
AddAgrumentToCommand(Pz_Leg,position_pz);
}
if ((sens_pz==255)&&(position_pz>=-52))
{
ExCodePz=1;
DownPzComplete=true;
}
if ((sens_pz!=255)&&(position_pz<=-50))
{
ExCodePz=2;
DownPzComplete=true;
}
}
Implementation_complete=DownLpComplete && DownLsComplete && DownLzComplete && DownPpComplete && DownPsComplete && DownPzComplete;
}
String ExCode ="";
sprintf(ExCode,"%i%i%i%i%i%i",ExCodeLp,ExCodeLs,ExCodeLz,ExCodePp,ExCodePs,ExCodePz);
if (write)
{ReadSensorsLegs();
sens_lp=RDPACKAGE[lp_leg];
sens_ls=RDPACKAGE[ls_leg];
sens_lz=RDPACKAGE[lz_leg];
sens_pp=RDPACKAGE[pp_leg];
sens_ps=RDPACKAGE[ps_leg];
sens_pz=RDPACKAGE[pz_leg];
ReadSensors();
int accelx=(int)RDPACKAGE[AccelX];
int accely=sensmedium();
Current_sensors.touch_lp=sens_lp;
Current_sensors.touch_ls=sens_ls;
Current_sensors.touch_lz=sens_lz;
Current_sensors.touch_pp=sens_pp;
Current_sensors.touch_ps=sens_ps;
Current_sensors.touch_pz=sens_pz;
Current_sensors.accel_x=accelx;
Current_sensors.accel_y=accely;
if ((Current_position.lp_vertical!=-60)&&(Current_position.ls_vertical!=-60)&&(Current_position.lz_vertical!=-60)&&(Current_position.pp_vertical!=60)&&(Current_position.ps_vertical!=60)&&(Current_position.pz_vertical!=60))
{
AnalisesFileWrite();
}
}
return ExCode;
}
void HorizontalCorrection(int LegNumber,int direction)
{
switch (LegNumber)
{
case 1:
{
int horizontal_position_lp=Current_position.lp_horizontal;
if (direction==0)
horizontal_position_lp+=15;
else
horizontal_position_lp-=15;
AddAgrumentToCommand(Lp_Leg_h,horizontal_position_lp);
}
case 2:
{
int horizontal_position_ls=Current_position.ls_horizontal;
if (direction==0)
horizontal_position_ls+=15;
else
horizontal_position_ls-=15;
AddAgrumentToCommand(Ls_Leg_h,horizontal_position_ls);
}
case 3:
{
int horizontal_position_lz=Current_position.lz_horizontal;
if (direction==0)
horizontal_position_lz-=15;
else
horizontal_position_lz+=15;
AddAgrumentToCommand(Lz_Leg_h,horizontal_position_lz);
}
case 4:
{
int horizontal_position_pp=Current_position.pp_horizontal;
if (direction==0)
horizontal_position_pp-=15;
else
horizontal_position_pp+=15;
AddAgrumentToCommand(Lp_Leg_h,horizontal_position_pp);
}
case 5:
{
int horizontal_position_ps=Current_position.ps_horizontal;
if (direction==0)
horizontal_position_ps+=15;
else
horizontal_position_ps-=15;
AddAgrumentToCommand(Lp_Leg_h,horizontal_position_ps);
}
case 6:
{
int horizontal_position_pz=Current_position.pz_horizontal;
if (direction==0)
horizontal_position_pz+=15;
else
horizontal_position_pz-=15;
AddAgrumentToCommand(Lp_Leg_h,horizontal_position_pz);
}
}
}
void Up(int LegNumber)
{
switch (LegNumber)
{
case 1:
{
AddAgrumentToCommand(Lp_Leg,-60);
break;
}
case 2:
{
AddAgrumentToCommand(Ls_Leg,-60);
break;
}
case 3:
{
AddAgrumentToCommand(Lz_Leg,-60);
break;
}
case 4:
{
AddAgrumentToCommand(Pp_Leg,60);
break;
}
case 5:
{
AddAgrumentToCommand(Ps_Leg,-60);
break;
}
case 6:
{
AddAgrumentToCommand(Pz_Leg,-60);
break;
}
}
}
void DownWithSearch(bool search,char mask)
{
if (search==false)
{
down(mask);
return;
}
else
{
char * ReturnCode=down(mask);
bool LpHorVal=(ReturnCode[0]==2);
bool LsHorVal=(ReturnCode[1]==2);
bool LzHorVal=(ReturnCode[2]==2);
bool PpHorVal=(ReturnCode[3]==2);
bool PsHorVal=(ReturnCode[4]==2);
bool PzHorVal=(ReturnCode[5]==2);
if (LpHorVal==true)
{
Up(1);
HorizontalCorrection(1,0);
down(PUT_LP);
}
if (LsHorVal==true)
{
Up(2);
HorizontalCorrection(2,0);
down(PUT_LS);
}
if (LzHorVal==true)
{
Up(3);
HorizontalCorrection(3,0);
down(PUT_LZ);
}
if (PpHorVal==true)
{
Up(4);
HorizontalCorrection(4,0);
down(PUT_PP);
}
if (PsHorVal==true)
{
Up(5);
HorizontalCorrection(5,0);
down(PUT_PS);
}
if (PzHorVal==true)
{
Up(6);
HorizontalCorrection(6,0);
down(PUT_PZ);
}
}
}
void PUT1()
{
down(PUT_PZ);
}
void FirstPosition()
{
ExecCommand("1 0 t 2 3 -27");
ExecCommand("2 0 t 2 3 -15");
ExecCommand("2 0 t 2 5 -20");
ExecCommand("1 0 t 2 5 26");
ExecCommand("2 0 t 2 1 26");
ExecCommand("1 0 t 2 1 21");
}
|
