Darwin-OP Walking Simulation on Webots

Once these trajectories were generated. The generated angular points were saved to a csv file to be imported onto the controlling programs on the simulators.

Darwin-OP walking on Webots


This is the controller program on webots that does it :

#include "Symmetry.hpp"
#include <webots/Servo.hpp>
 
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <fstream>
 
using namespace webots;
using namespace std;
 
static const char *servoNames[NSERVOS] = {
  "ShoulderR" /*ID1 */, "ShoulderL" /*ID2 */, "ArmUpperR" /*ID3 */, "ArmUpperL" /*ID4 */,
  "ArmLowerR" /*ID5 */, "ArmLowerL" /*ID6 */, "PelvYR"    /*ID7 */, "PelvYL"    /*ID8 */,
  "PelvR"     /*ID9 */, "PelvL"     /*ID10*/, "LegUpperR" /*ID11*/, "LegUpperL" /*ID12*/,
  "LegLowerR" /*ID13*/, "LegLowerL" /*ID14*/, "AnkleR"    /*ID15*/, "AnkleL"    /*ID16*/,
  "FootR"     /*ID17*/, "FootL"     /*ID18*/, "Neck"      /*ID19*/, "Head"      /*ID20*/
};
 
//Constructor
Symmetry::Symmetry():
    Robot()
{
  //Get time step
//  mTimeStep = getBasicTimeStep();
    mTimeStep = 1;
 
  //Get the two RGB_LEDs
  mEyeLED = getLED("EyeLed");
  mHeadLED = getLED("HeadLed");
 
  //Get all the 20 Servos and enable them
  for(int i= 0; i < NSERVOS; i++)
  {
    mServos[i] = getServo(servoNames[i]);
    mServos[i]->enablePosition(mTimeStep);
  }
 
 
}
 
//Destructor
Symmetry::~Symmetry() {
}
 
//Step function
void Symmetry::myStep() {
  int ret = step(mTimeStep);
  if (ret == -1)
    exit(EXIT_SUCCESS);
}
 
//Wait function
void Symmetry::wait(int ms) {
  double startTime = getTime();
  double s = (double) ms / 1000.0;
  while (s + startTime >= getTime())
    myStep();
}
 
// function containing the main feedback loop
void Symmetry::run() {
 
 
  double position = 0;
  float xp[145] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138,139,140,141,142,143,144}; 
  float footl_loc [] = {-1.22,0,-0.52886,-0.53511,-0.53511,-0.53511,-0.53511,-0.53511,-0.53511,-0.53515,-0.5353,-0.53553,-0.53585,-0.53626,-0.53674,-0.5373,-0.53793,-0.53862,-0.53938,-0.5402,-0.54107,-0.54199,-0.54296,-0.54396,-0.54501,-0.54609,-0.5472,-0.54833,-0.54949,-0.55067,-0.55185,-0.55305,-0.55426,-0.55546,-0.55667,-0.55787,-0.55906,-0.56023,-0.5614,-0.56254,-0.56365,-0.56474,-0.5658,-0.56683,-0.56782,-0.56876,-0.56966,-0.57052,-0.57132,-0.57206,-0.57275,-0.57286,-0.57497,-0.57702,-0.57899,-0.58091,-0.58276,-0.61682,-0.64912,-0.67956,-0.708,-0.7343,-0.75832,-0.7799,-0.79891,-0.81523,-0.82876,-0.83943,-0.8472,-0.8521,-0.85415,-0.85346,-0.85016,-0.84442,-0.83643,-0.82642,-0.81462,-0.80127,-0.78658,-0.77075,-0.75396,-0.73635,-0.718,-0.69898,-0.6793,-0.65894,-0.63784,-0.61591,-0.59304,-0.56909,-0.54392,-0.51735,-0.48922,-0.48676,-0.48881,-0.4909,-0.49306,-0.49526,-0.49477,-0.49719,-0.49952,-0.50179,-0.50398,-0.5061,-0.50816,-0.51016,-0.5121,-0.51398,-0.51582,-0.51761,-0.51935,-0.52106,-0.52272,-0.52436,-0.52595,-0.52753,-0.52907,-0.5306,-0.5321,-0.53359,-0.53506,-0.53652,-0.53797,-0.53942,-0.54087,-0.54232,-0.54377,-0.54522,-0.54668,-0.54816,-0.54965,-0.55115,-0.55267,-0.55422,-0.55579,-0.55738,-0.559,-0.56066,-0.56234,-0.56407,-0.56583,-0.56762,-0.56947,-0.57135,-0.57329};
  float footr_loc [] = {1.22,0,0.52886,0.53511,0.53511,0.53511,0.53511,0.53511,0.53511,0.56636,0.59597,0.62388,0.65005,0.67438,0.69681,0.71726,0.73564,0.75188,0.76595,0.77778,0.78738,0.79473,0.79986,0.80282,0.80366,0.80248,0.79936,0.79441,0.78773,0.77944,0.76963,0.75839,0.74581,0.73193,0.7168,0.70044,0.68285,0.66403,0.64394,0.62254,0.59977,0.57557,0.54985,0.52253,0.4935,0.4901,0.49113,0.4921,0.49301,0.49386,0.49465,0.49477,0.49719,0.49952,0.50179,0.50398,0.5061,0.50816,0.51016,0.5121,0.51398,0.51582,0.51761,0.51935,0.52106,0.52272,0.52436,0.52595,0.52753,0.52907,0.5306,0.5321,0.53359,0.53506,0.53652,0.53797,0.53942,0.54087,0.54232,0.54377,0.54522,0.54668,0.54816,0.54965,0.55115,0.55267,0.55422,0.55579,0.55738,0.559,0.56066,0.56234,0.56407,0.56583,0.56762,0.56947,0.57135,0.57329,0.57286,0.57497,0.57702,0.57899,0.58091,0.58276,0.61682,0.64912,0.67956,0.708,0.7343,0.75832,0.7799,0.79891,0.81523,0.82876,0.83943,0.8472,0.8521,0.85415,0.85346,0.85016,0.84442,0.83643,0.82642,0.81462,0.80127,0.78658,0.77075,0.75396,0.73635,0.718,0.69898,0.6793,0.65894,0.63784,0.61591,0.59304,0.56909,0.54392,0.51735,0.48922,0.48676,0.48881,0.4909,0.49306,0.49526};
  float anklel_loc [] = {0,0,0,0,0.01538,0.030685,0.045839,0.06077,0.075406,0.089678,0.10352,0.11687,0.12966,0.14185,0.15338,0.16419,0.17426,0.18354,0.192,0.1996,0.20632,0.21214,0.21703,0.22098,0.22398,0.22601,0.22708,0.22718,0.2263,0.22446,0.22165,0.21788,0.21318,0.20754,0.201,0.19357,0.18528,0.17616,0.16625,0.15557,0.14418,0.13212,0.11944,0.1062,0.092453,0.078261,0.063691,0.048813,0.033697,0.018416,0.003045,-0.012341,-0.027666,-0.042857,-0.057838,-0.072538,-0.086887,-0.10082,-0.11427,-0.12718,-0.13949,-0.15115,-0.16211,-0.17233,-0.18177,-0.19039,-0.19817,-0.20506,-0.21106,-0.21613,-0.22027,-0.22346,-0.22569,-0.22695,-0.22724,-0.22655,-0.2249,-0.22228,-0.2187,-0.21418,-0.20873,-0.20237,-0.19511,-0.18699,-0.17803,-0.16827,-0.15774,-0.14649,-0.13456,-0.122,-0.10886,-0.09521,-0.0811,-0.0666,-0.051777,-0.036703,-0.021448,-0.0060893,0.0092996,0.024643,0.039866,0.054894,0.069655,0.084079,0.098099,0.11165,0.12467,0.1371,0.1489,0.16,0.17037,0.17997,0.18875,0.1967,0.20377,0.20994,0.2152,0.21953,0.2229,0.22532,0.22677,0.22726,0.22676,0.2253,0.22287,0.21949,0.21515,0.20988,0.2037,0.19662,0.18866,0.17987,0.17026,0.15988,0.14877,0.13697,0.12453,0.1115,0.097949,0.083925,0.069497,0.054732,0.039701,0.024477,0.0091326};
  float ankler_loc [] = {0,0,0,0,0.01538,0.030685,0.045839,0.06077,0.075406,0.089678,0.10352,0.11687,0.12966,0.14185,0.15338,0.16419,0.17426,0.18354,0.192,0.1996,0.20632,0.21214,0.21703,0.22098,0.22398,0.22601,0.22708,0.22718,0.2263,0.22446,0.22165,0.21788,0.21318,0.20754,0.201,0.19357,0.18528,0.17616,0.16625,0.15557,0.14418,0.13212,0.11944,0.1062,0.092453,0.078261,0.063691,0.048813,0.033697,0.018416,0.003045,-0.012341,-0.027666,-0.042857,-0.057838,-0.072538,-0.086887,-0.10082,-0.11427,-0.12718,-0.13949,-0.15115,-0.16211,-0.17233,-0.18177,-0.19039,-0.19817,-0.20506,-0.21106,-0.21613,-0.22027,-0.22346,-0.22569,-0.22695,-0.22724,-0.22655,-0.2249,-0.22228,-0.2187,-0.21418,-0.20873,-0.20237,-0.19511,-0.18699,-0.17803,-0.16827,-0.15774,-0.14649,-0.13456,-0.122,-0.10886,-0.09521,-0.0811,-0.0666,-0.051777,-0.036703,-0.021448,-0.0060893,0.0092996,0.024643,0.039866,0.054894,0.069655,0.084079,0.098099,0.11165,0.12467,0.1371,0.1489,0.16,0.17037,0.17997,0.18875,0.1967,0.20377,0.20994,0.2152,0.21953,0.2229,0.22532,0.22677,0.22726,0.22676,0.2253,0.22287,0.21949,0.21515,0.20988,0.2037,0.19662,0.18866,0.17987,0.17026,0.15988,0.14877,0.13697,0.12453,0.1115,0.097949,0.083925,0.069497,0.054732,0.039701,0.024477,0.0091326};
  float kneel_loc [] = {-2.25,0,-1.0577,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0701,-1.0701,-1.0701,-1.0701,-1.07,-1.07,-1.07,-1.0699,-1.0699,-1.0698,-1.0697,-1.0696,-1.0696,-1.0695,-1.0694,-1.0693,-1.0692,-1.0691,-1.069,-1.0689,-1.0688,-1.0686,-1.0685,-1.0684,-1.0683,-1.0682,-1.0681,-1.068,-1.0678,-1.0677,-1.0676,-1.0676,-1.0673,-1.067,-1.0667,-1.0664,-1.066,-1.1283,-1.1871,-1.2428,-1.2951,-1.3443,-1.3902,-1.4328,-1.4721,-1.5081,-1.5406,-1.5696,-1.595,-1.6167,-1.6347,-1.6488,-1.659,-1.6653,-1.6676,-1.666,-1.6603,-1.6507,-1.6372,-1.6198,-1.5986,-1.5738,-1.5453,-1.5133,-1.4779,-1.439,-1.3969,-1.3514,-1.3027,-1.2508,-1.1957,-1.1372,-1.0754,-1.0666,-1.0668,-1.0671,-1.0674,-1.0677,-1.0676,-1.0679,-1.0682,-1.0684,-1.0687,-1.0689,-1.069,-1.0692,-1.0693,-1.0695,-1.0696,-1.0697,-1.0698,-1.0699,-1.07,-1.07,-1.0701,-1.0701,-1.0701,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0702,-1.0701,-1.0701,-1.07,-1.07,-1.0699,-1.0698,-1.0698,-1.0697,-1.0696,-1.0695,-1.0693,-1.0692,-1.0691,-1.0689,-1.0687,-1.0685,-1.0683,-1.0681,-1.0678,-1.0676};
  float kneer_loc [] = {2.25,0,1.0577,1.0702,1.0702,1.0702,1.0702,1.0702,1.0702,1.1326,1.1917,1.2475,1.2999,1.3491,1.3949,1.4374,1.4765,1.512,1.544,1.5725,1.5973,1.6183,1.6356,1.6491,1.6588,1.6646,1.6666,1.6647,1.6589,1.6493,1.6359,1.6188,1.5979,1.5734,1.5452,1.5135,1.4783,1.4397,1.3977,1.3523,1.3037,1.2517,1.1965,1.138,1.0761,1.067,1.0672,1.0673,1.0674,1.0675,1.0676,1.0676,1.0679,1.0682,1.0684,1.0687,1.0689,1.069,1.0692,1.0693,1.0695,1.0696,1.0697,1.0698,1.0699,1.07,1.07,1.0701,1.0701,1.0701,1.0702,1.0702,1.0702,1.0702,1.0702,1.0702,1.0702,1.0702,1.0701,1.0701,1.07,1.07,1.0699,1.0698,1.0698,1.0697,1.0696,1.0695,1.0693,1.0692,1.0691,1.0689,1.0687,1.0685,1.0683,1.0681,1.0678,1.0676,1.0676,1.0673,1.067,1.0667,1.0664,1.066,1.1283,1.1871,1.2428,1.2951,1.3443,1.3902,1.4328,1.4721,1.5081,1.5406,1.5696,1.595,1.6167,1.6347,1.6488,1.659,1.6653,1.6676,1.666,1.6603,1.6507,1.6372,1.6198,1.5986,1.5738,1.5453,1.5133,1.4779,1.439,1.3969,1.3514,1.3027,1.2508,1.1957,1.1372,1.0754,1.0666,1.0668,1.0671,1.0674,1.0677};
  float hipPl_loc [] = {1.15,0,0.52886,0.53511,0.53511,0.53511,0.53511,0.53511,0.53511,0.53506,0.53491,0.53468,0.53436,0.53395,0.53346,0.5329,0.53227,0.53156,0.5308,0.52997,0.52908,0.52814,0.52715,0.52611,0.52503,0.52391,0.52276,0.52157,0.52036,0.51912,0.51787,0.5166,0.51531,0.51402,0.51273,0.51143,0.51014,0.50886,0.50759,0.50634,0.50511,0.5039,0.50273,0.50158,0.50048,0.49942,0.4984,0.49744,0.49653,0.49568,0.4949,0.49477,0.49235,0.48999,0.48769,0.48545,0.48326,0.51144,0.53803,0.56319,0.58712,0.60995,0.63184,0.65289,0.67319,0.69282,0.71181,0.73014,0.74776,0.7646,0.78051,0.79534,0.80888,0.82091,0.83122,0.83955,0.84569,0.84943,0.85059,0.84904,0.84468,0.83745,0.82733,0.81436,0.79858,0.78011,0.75905,0.73553,0.70971,0.68173,0.65174,0.61987,0.58623,0.5798,0.57804,0.57623,0.57436,0.57243,0.57286,0.57074,0.56867,0.56665,0.56468,0.56276,0.56088,0.55905,0.55725,0.5555,0.55378,0.5521,0.55045,0.54883,0.54723,0.54566,0.54412,0.54259,0.54108,0.53958,0.5381,0.53662,0.53515,0.53369,0.53222,0.53076,0.52928,0.52781,0.52632,0.52481,0.5233,0.52176,0.5202,0.51861,0.517,0.51536,0.51368,0.51196,0.5102,0.5084,0.50655,0.50466,0.5027,0.50069,0.49862,0.49649,0.49429}; // these are my calculated hip position to keep the body vertical
  float hipPr_loc [] = {-1.15,0,-0.52886,-0.53511,-0.53511,-0.53511,-0.53511,-0.53511,-0.53511,-0.56628,-0.59575,-0.62359,-0.6499,-0.67472,-0.69813,-0.72015,-0.74082,-0.76014,-0.7781,-0.7947,-0.80988,-0.82359,-0.83577,-0.84633,-0.85516,-0.86217,-0.86725,-0.87029,-0.87121,-0.8699,-0.86631,-0.86038,-0.85209,-0.84143,-0.82841,-0.81306,-0.79546,-0.77565,-0.75373,-0.72978,-0.70389,-0.67615,-0.64664,-0.61543,-0.58257,-0.57692,-0.57604,-0.57519,-0.5744,-0.57366,-0.57297,-0.57286,-0.57074,-0.56867,-0.56665,-0.56468,-0.56276,-0.56088,-0.55905,-0.55725,-0.5555,-0.55378,-0.5521,-0.55045,-0.54883,-0.54723,-0.54566,-0.54412,-0.54259,-0.54108,-0.53958,-0.5381,-0.53662,-0.53515,-0.53369,-0.53222,-0.53076,-0.52928,-0.52781,-0.52632,-0.52481,-0.5233,-0.52176,-0.5202,-0.51861,-0.517,-0.51536,-0.51368,-0.51196,-0.5102,-0.5084,-0.50655,-0.50466,-0.5027,-0.50069,-0.49862,-0.49649,-0.49429,-0.49477,-0.49235,-0.48999,-0.48769,-0.48545,-0.48326,-0.51144,-0.53803,-0.56319,-0.58712,-0.60995,-0.63184,-0.65289,-0.67319,-0.69282,-0.71181,-0.73014,-0.74776,-0.7646,-0.78051,-0.79534,-0.80888,-0.82091,-0.83122,-0.83955,-0.84569,-0.84943,-0.85059,-0.84904,-0.84468,-0.83745,-0.82733,-0.81436,-0.79858,-0.78011,-0.75905,-0.73553,-0.70971,-0.68173,-0.65174,-0.61987,-0.58623,-0.5798,-0.57804,-0.57623,-0.57436,-0.57243};
  float hipRl_loc [] = {0,0,0,0,0.01538,0.030685,0.045839,0.06077,0.075406,0.089678,0.10352,0.11687,0.12966,0.14185,0.15338,0.16419,0.17426,0.18354,0.192,0.1996,0.20632,0.21214,0.21703,0.22098,0.22398,0.22601,0.22708,0.22718,0.2263,0.22446,0.22165,0.21788,0.21318,0.20754,0.201,0.19357,0.18528,0.17616,0.16625,0.15557,0.14418,0.13212,0.11944,0.1062,0.092453,0.078261,0.063691,0.048813,0.033697,0.018416,0.003045,-0.012341,-0.027666,-0.042857,-0.057838,-0.072538,-0.086887,-0.10082,-0.11427,-0.12718,-0.13949,-0.15115,-0.16211,-0.17233,-0.18177,-0.19039,-0.19817,-0.20506,-0.21106,-0.21613,-0.22027,-0.22346,-0.22569,-0.22695,-0.22724,-0.22655,-0.2249,-0.22228,-0.2187,-0.21418,-0.20873,-0.20237,-0.19511,-0.18699,-0.17803,-0.16827,-0.15774,-0.14649,-0.13456,-0.122,-0.10886,-0.09521,-0.0811,-0.0666,-0.051777,-0.036703,-0.021448,-0.0060893,0.0092996,0.024643,0.039866,0.054894,0.069655,0.084079,0.098099,0.11165,0.12467,0.1371,0.1489,0.16,0.17037,0.17997,0.18875,0.1967,0.20377,0.20994,0.2152,0.21953,0.2229,0.22532,0.22677,0.22726,0.22676,0.2253,0.22287,0.21949,0.21515,0.20988,0.2037,0.19662,0.18866,0.17987,0.17026,0.15988,0.14877,0.13697,0.12453,0.1115,0.097949,0.083925,0.069497,0.054732,0.039701,0.024477,0.0091326};
  float hipRr_loc [] = {0,0,0,0,0.01538,0.030685,0.045839,0.06077,0.075406,0.089678,0.10352,0.11687,0.12966,0.14185,0.15338,0.16419,0.17426,0.18354,0.192,0.1996,0.20632,0.21214,0.21703,0.22098,0.22398,0.22601,0.22708,0.22718,0.2263,0.22446,0.22165,0.21788,0.21318,0.20754,0.201,0.19357,0.18528,0.17616,0.16625,0.15557,0.14418,0.13212,0.11944,0.1062,0.092453,0.078261,0.063691,0.048813,0.033697,0.018416,0.003045,-0.012341,-0.027666,-0.042857,-0.057838,-0.072538,-0.086887,-0.10082,-0.11427,-0.12718,-0.13949,-0.15115,-0.16211,-0.17233,-0.18177,-0.19039,-0.19817,-0.20506,-0.21106,-0.21613,-0.22027,-0.22346,-0.22569,-0.22695,-0.22724,-0.22655,-0.2249,-0.22228,-0.2187,-0.21418,-0.20873,-0.20237,-0.19511,-0.18699,-0.17803,-0.16827,-0.15774,-0.14649,-0.13456,-0.122,-0.10886,-0.09521,-0.0811,-0.0666,-0.051777,-0.036703,-0.021448,-0.0060893,0.0092996,0.024643,0.039866,0.054894,0.069655,0.084079,0.098099,0.11165,0.12467,0.1371,0.1489,0.16,0.17037,0.17997,0.18875,0.1967,0.20377,0.20994,0.2152,0.21953,0.2229,0.22532,0.22677,0.22726,0.22676,0.2253,0.22287,0.21949,0.21515,0.20988,0.2037,0.19662,0.18866,0.17987,0.17026,0.15988,0.14877,0.13697,0.12453,0.1115,0.097949,0.083925,0.069497,0.054732,0.039701,0.024477,0.0091326};
  float x = 0;
  float anklel; 
  float ankler; 
  float footl; 
  float footr;
  float kneel; 
  float kneer;
  float hipPl; 
  float hipPr;
  float hipRl; 
  float hipRr;
//  for(int i= 0; i < 50; i++)
//  {
//    anklel_loc[i] = -anklel_loc[i];
//    ankler_loc[i] = -ankler_loc[i];
//    hipRl_loc[i] = -hipRl_loc[i];
//    hipRr_loc[i] = -hipRr_loc[i];
//    }
  myStep();
  for(int i= 0; i < NSERVOS; i++)
  {
    position = mServos[i]->getPosition();
    cout << i ;
    cout << "[" << position << "]\n";
  }
 
  while (1) {
 
    if (x<2)  
    {
      footl = lip(x,xp,footl_loc);
      footr = lip(x,xp,footr_loc);
      kneel = lip(x,xp,kneel_loc);
      kneer = lip(x,xp,kneer_loc);
      hipPl = lip(x,xp,hipPl_loc);
      hipPr = lip(x,xp,hipPr_loc);
      mServos[15]->setPosition(footl);
      mServos[14]->setPosition(footr);
      mServos[13]->setPosition(kneel);
      mServos[12]->setPosition(kneer);
      mServos[11]->setPosition(hipPl);
      mServos[10]->setPosition(hipPr);
 
//    cout << y ;
//    position = mServos[0]->getPosition();
//    cout << "[" << position << "]\n";
//    if (position >= 2.1)
//          {sign = (-1);}
//        if (position <= 0)
//          {sign = (1);}
//    position = position + (sign)*0.1;
//    mServos[0]->setPosition(y);
//    x = x + 0.02;
    }
    else if (x<144)
    {
    footl = lip(x,xp,footl_loc);
    footr = lip(x,xp,footr_loc);
    kneel = lip(x,xp,kneel_loc);
    kneer = lip(x,xp,kneer_loc);
    hipPl = lip(x,xp,hipPl_loc);
    hipPr = lip(x,xp,hipPr_loc);
 
          // hip swing movement
    anklel = lip(x,xp,anklel_loc);
    ankler = lip(x,xp,ankler_loc);
    hipRl = lip(x,xp,hipRl_loc);
    hipRr = lip(x,xp,hipRr_loc);
 
    cout << hipRr ;
    mServos[17]->setPosition(anklel);
    mServos[16]->setPosition(ankler);
    mServos[15]->setPosition(footl);
    mServos[14]->setPosition(footr);
    mServos[13]->setPosition(kneel);
    mServos[12]->setPosition(kneer);
    mServos[11]->setPosition(hipPl);
    mServos[10]->setPosition(hipPr);
    mServos[9]->setPosition(hipRl);
    mServos[8]->setPosition(hipRr);
    }
    if (x<2)
      x = x+ 0.01;
    else if(x<144)
      x = x + 3;
    else
      x = 51;
    // step
    myStep();
  }
}
 
float Symmetry::lip(float x,float xp[],float yp[])
{
  float y=0;
  float xp0=0;
  float yp0=0;
  float xp1=0;
  float yp1=0;
  for (int i =0; i<=145; i++)
  {
    if (xp[i]<=x)
    {
      xp0 = xp[i];
      yp0 = yp[i];
    }
    if (xp[i]>x)
    {
      xp1 = xp[i];
      yp1 = yp[i];
      break;
    }
  }
  float m = (yp1-yp0)/(xp1-xp0);
  y = (yp0 +(m*(x-xp0)));
  return (y);
}

Other Pages