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;
}
}
}
