Приложение Г. Листинг библиотеки адаптивного опускания ног 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"); }
Популярное: Как вы ведете себя при стрессе?: Вы можете самостоятельно управлять стрессом! Каждый из нас имеет право и возможность уменьшить его воздействие на нас... Почему люди поддаются рекламе?: Только не надо искать ответы в качестве или количестве рекламы... Организация как механизм и форма жизни коллектива: Организация не сможет достичь поставленных целей без соответствующей внутренней... ©2015-2024 megaobuchalka.ru Все материалы представленные на сайте исключительно с целью ознакомления читателями и не преследуют коммерческих целей или нарушение авторских прав. (217)
|
Почему 1285321 студент выбрали МегаОбучалку... Система поиска информации Мобильная версия сайта Удобная навигация Нет шокирующей рекламы |