void Stable(void)
{
static double gval[3];
static double pre_pitch, pitch, pre_roll, roll;
int i;
if(timer_nowtime() - STABLETIMER > FRESHTIME)
{
ReadGSensor();
gval[0] = (double)(G_AXIS_VALUE[0]);
gval[1] = (double)(G_AXIS_VALUE[1]);
gval[2] = (double)(G_AXIS_VALUE[2]);
pitch = (GetPitch(gval)- 5.0*DEG_TO_RAD)*0.5 + pre_pitch*0.5;
roll = (GetRoll(gval) - 4.5*DEG_TO_RAD)*0.5 + pre_roll*0.5;
BODY_PITCH = BODY_PITCH + STABLE_P*pitch + (pitch - pre_pitch)*STABLE_D/FRESHTIME;
BODY_ROLL = BODY_ROLL + STABLE_P*roll + (roll - pre_roll)*STABLE_D/FRESHTIME;
if(BODY_PITCH > 60.0*DEG_TO_RAD)
BODY_PITCH = 60.0*DEG_TO_RAD;
else if(BODY_PITCH < -60.0*DEG_TO_RAD)
BODY_PITCH = -60.0*DEG_TO_RAD;
if(BODY_ROLL > 60.0*DEG_TO_RAD)
BODY_ROLL = 60.0*DEG_TO_RAD;
else if(BODY_ROLL < -60.0*DEG_TO_RAD)
BODY_ROLL = -60.0*DEG_TO_RAD;
for(i =0 ; i < 6; i++ )
{
LEGPOS[0] = LEGGOAL[3*i] - cos(J3MAP[i])*20.0;
LEGPOS[1] = LEGGOAL[3*i+1] - sin(J3MAP[i])*20.0;
LEGPOS[2] = LEGGOAL[3*i+2];
Transform(BODY_YAW, BODY_PITCH, BODY_ROLL, LEGPOS);
AdjustPos(POS_GAIN, LEGPOS);
CenterToJoint3(LEGPOS,i,J3GOAL);
Joint3ToJoint2(J3GOAL, J2GOAL);
JointAngle(LEGPOS, J2GOAL,i,JANGLE);
SetJoint(JANGLE,i);
rcservo_SetAction(POSITION, STABLETIME);
STABLETIMER = timer_nowtime();
}
pre_pitch = pitch;
pre_roll = roll;
}
rcservo_PlayAction();
}
void Idle(void)
{
int i;
double temp_p, temp_r;
if(rcservo_PlayAction() == RCSERVO_PLAYEND)
{
for(i =0 ; i < 6; i++ )
{
LEGPOS[0] = LEGGOAL[3*i];
LEGPOS[1] = LEGGOAL[3*i+1];
LEGPOS[2] = LEGGOAL[3*i+2];
if(SWING == 1)
{
ALPHA = ALPHA + PI/36.0;
if(ALPHA >= 2*PI - PI/36.0)
ALPHA = 0.0;
temp_p = BODY_PITCH + gain_p*sin(ALPHA)*DEG_TO_RAD;
temp_r = BODY_ROLL+ gain_r*sin(ALPHA+PI/2)*DEG_TO_RAD;
}
else
{
temp_p = BODY_PITCH;
temp_r = BODY_ROLL;
}
Transform(BODY_YAW, temp_p, temp_r, LEGPOS);
AdjustPos(POS_GAIN, LEGPOS);
CenterToJoint3(LEGPOS,i,J3GOAL);
Joint3ToJoint2(J3GOAL, J2GOAL);
JointAngle(LEGPOS, J2GOAL,i,JANGLE);
SetJoint(JANGLE,i);
rcservo_SetAction(POSITION, IDLEPLAYTIME);
}
IDLEFRAME = 0;
}
rcservo_PlayAction();
}
void PlayMotion(void)
{
int i;
for(i =0; i < 6; i++)
{
switch(LEGFRAME[i])
{
case -1:
if(timer_nowtime() - PLAYTIMER[i] > STARTTIME[i])
{
LEGFRAME[i] = 0;
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i]++;
LEGTIMER[i] = timer_nowtime();
}
break;
case 11:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i] = 0;
LEGTIMER[i] = timer_nowtime();
}
break;
case 0:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i]++;
LEGTIMER[i] = timer_nowtime();
}
break;
case 9:
case 1:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i] = LEGFRAME[i] + STEP_1;
LEGTIMER[i] = timer_nowtime();
}
break;
case 3:
case 6:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i] = LEGFRAME[i] + STEP_2;
LEGTIMER[i] = timer_nowtime();
}
break;
default:
if(timer_nowtime() - LEGTIMER[i] > PLAYTIME)
{
SetMotion(i);
rcservo_SetAction(POSITION, PLAYTIME);
LEGFRAME[i] = LEGFRAME[i]++;
LEGTIMER[i] = timer_nowtime();
}
break;
}
}
rcservo_PlayAction();
}
void executeJointState(const sensor_msgs::JointStateConstPtr& msg, bool alternate=false)
{
// TODO: something better than this for velocity and duration.
// it could be possible to work out a scheme where each servo can get
// an individual duration. but what about the velocity interpretation?
// the concept of velocity is completely unaccounted for in the roboio.
uint32_t duration;
if (msg->velocity.size())
duration = msg->velocity[0];
else
duration = 300;
std::map<int, unsigned short> icsframe;
for (size_t i=0; i < msg->name.size(); i++)
{
Servo& servo = servos_[msg->name[i]];
if (servo.channel_ < 0 || servo.channel_ > 32)
continue;
float rot_range = servo.max_rot_ - servo.min_rot_;
float pwm_range = servo.max_pwm_ - servo.min_pwm_;
int pwm_center = (servo.max_pwm_ + servo.min_pwm_) / 2;
float pwm_per_rot = pwm_range / rot_range;
float f_pwm_val = (float)msg->position[i] * pwm_per_rot;
unsigned int i_pwm_val = (int)round(f_pwm_val) + (int)servo.trim_pwm_ + (int)pwm_center;
if (servo.bus_ == Servo::COM4)
icsframe[servo.channel_] = i_pwm_val;
else
playframe_[servo.channel_-1] = i_pwm_val;
}
// OPTIONAL?: wait for previous movement to finish:
// while (rcservo_PlayAction() != RCSERVO_PLAYEND) { }
// or rcservo_MoveTo(playframe_, duration);
if (alternate)
{
pthread_mutex_lock(&playframe_mutex_);
rcservo_MoveTo(playframe_, duration);
pthread_mutex_unlock(&playframe_mutex_);
}
else
{
pthread_mutex_lock(&playframe_mutex_);
rcservo_SetAction(playframe_, duration);
pthread_mutex_unlock(&playframe_mutex_);
}
// TODO: no concept of duration here, could use the speed parameters,
// or could write a separate thread that interpolates for the serial
pthread_mutex_lock(&com4_mutex_);
std::map<int, unsigned short>::iterator i = icsframe.begin();
for(; i != icsframe.end(); ++i)
{
int channel = i->first;
unsigned short pos = i->second;
com4_ics_pos(channel, pos);
usleep(1);
}
pthread_mutex_unlock(&com4_mutex_);
}
