void Control::Control_all(uint16_t speed) { ctrlTimer.reset(); ctrlTimer.start(); int handler = 10 ; while(isWalking&&(handler>0)) { int timerValue = ctrlTimer.read_us(); if(timerValue >= ctrlInterval){ //.....................Read Inputs.................. pb.mode(PullUp); double step_speed = (unsigned)speed; double tcAngle = (unsigned)jointTC.GetAngle(); double tcVel = (unsigned)jointTC.GetVelocity(); double ctAngle = (unsigned)jointCT.GetAngle(); double ftAngle = (unsigned)(jointFT.GetAngle()-0x0400); double f_Contact = !pb?1.0:0.0; // Adjusting the angles to be the same as in Von Twickel's implementation. tcAngle = tcAngle - 2048; tcAngle = -tcAngle *0.088; // Reverse tcAngle for contra-lateral legs. tcAngle = -tcAngle; ctAngle = 270-ctAngle*0.088; ftAngle = ftAngle*0.088; //.....................ThC Joint Control............// // 7->17 output17 = sigmoid17.Push_s_sensor(f_Contact,20,0.5); // 1,2,17->14 output14 = servo14.ThCServo(-tcAngle,tcVel,output17); // 14->8 Protraction.tcJointCtrl(output14,32,step_speed); // 14->9 Retraction.tcJointCtrl(output14,-32,step_speed); //.....................CTr Joint Control............// double dep_to_lev_ft = ftAngle-120; double dep_to_lev_tc = tcAngle -(-25); double lev_to_dep_ft = ftAngle-70; // 1->18 output18 = sigmoid18.Push_s_sensor(dep_to_lev_tc,-32,0.361); // 5->19 output19 = sigmoid19.Push_s_sensor(dep_to_lev_ft,32,0.750); // 5->21 output21 = sigmoid21.Push_s_sensor(lev_to_dep_ft,-32,0.472); // 18,19 -> 20 output20 = orNeuron20.Push_or(output18,output19,32,32); // 20,21->15 output15 = sigmoid15.Push_s(output20,output21,32,-30); // HEIGHT CONTROLLER output24 = servo24.CTrHeightServo(ftAngle,ctAngle,tcAngle,-4.2); if (f_Contact>0.5){ if (output24>0){ Levation.ctJointCtrl(output24,32,step_speed); } else { Depression.ctJointCtrl(output24,32,step_speed); } } // 15->10 Levation.ctJointCtrl(output15,32,step_speed); // 15->11 Depression.ctJointCtrl(output15,-32,step_speed); //.....................FTi Joint Control............ double flx_to_ext_ft = ftAngle - 105; double ext_to_flx_ft = ftAngle - 105; //5->25 output25 = sigmoid25.Push_s_sensor(flx_to_ext_ft,32,0.667); //7->26 output26 = sigmoid26.Push_s(0,f_Contact,0,-20); //25,26->27 output27 = orNeuron27.Push_or(output25,output26,32,32); //5->28 output28 = sigmoid28.Push_s_sensor(ext_to_flx_ft,-32,0.667); // 7->29 output29 = sigmoid29.Push_s(0,f_Contact,0,20); //28,29->30 output30 = andNeuron30.Push_and(output28,output29,32,32); // 27,30->16 output16 = sigmoid16.Push_s(output27,output30,-30,32); // 16->12 Flexion.ftJointCtrl(output16,32,step_speed); // 16->13 Extension.ftJointCtrl(output16,-32,step_speed); ctrlTimer.reset(); handler = handler - 1; } } }