void encoderCallback(const legged_robot::Encoder::ConstPtr& msg)
{
//ROS_INFO("I heard: [%u]", msg->encoder);
//msgs++;
pwm_msg.pwm_duty = PID_controller (1.5, 0.2, 0.1, moveVelocity, desiredPos, (int32_t)msg->encoder);
sendMsg = true;
//if(msgs == 1000)
//{
//ROS_INFO("PWM: [%d]", pwm_msg.pwm_duty);
//msgs = 0;
//}
}
int main(void){
struct udp_conn udpcon;
udp_init_client(&udpcon, port, server_ip);
char buffer[255];
struct PID_parameters parameters= (struct PID_parameters){
10,
800,
0,
1,
period,
};
double y, u;
int i=0;
struct timespec next;
clock_gettime(CLOCK_REALTIME, &next);
timespec_add_us(&next, (int)(period*1000000));
udp_send(&udpcon, "START", 6);
while(1){
udp_send(&udpcon, "GET", 4);
while(1){
udp_receive(&udpcon, buffer, 255);
if (buffer[0]=='G'){
break;
}
}
y = doubleFromReply(buffer);
u = PID_controller(parameters, y);
setFromDouble(buffer,u);
udp_send(&udpcon, buffer, 255);
clock_nanosleep(&next);
timespec_add_us(&next, (int)(period*1000000));
if (i++*period >= 0.5){
break;
}
}
udp_send(&udpcon, "STOP",5);
printf("stop\n");
return 0;
}
void setFromDouble(char *buffer, double u){
buffer[0]='S';
buffer[1]='E';
buffer[2]='T';
buffer[3]=':';
char dummy[255];
sprintf(dummy, "%f", u);
char* ptr = dummy;
int i=4;
while(*ptr !='\0'){
buffer[i++]=*ptr;
ptr++;
}
buffer[i]='\0';
return;
}
