void* arm_commander(void *data) {
int hz = 30;
state_t *state = (state_t*) data;
double angles[NUM_SERVOS];
double speed;
dynamixel_command_list_t cmds;
cmds.len = NUM_SERVOS;
cmds.commands = (dynamixel_command_t*) malloc(sizeof(dynamixel_command_t)*NUM_SERVOS);
while (state->running) {
if(!state->interpolate_angles) {
if (state->update_arm_cont) {
state->arm->getTargetAngles(angles);
state->arm->getTargetSpeed(speed);
for (int id = 0; id < NUM_SERVOS; id++) {
cmds.commands[id].utime = utime_now();
cmds.commands[id].position_radians = angles[id];
cmds.commands[id].speed = speed;
cmds.commands[id].max_torque = 0.7;
}
pthread_mutex_lock(&state->lcm_mutex);
dynamixel_command_list_t_publish(state->lcm, ARM_COMMAND_CHANNEL, &cmds);
pthread_mutex_unlock(&state->lcm_mutex);
} else {
if (state->update_arm_cont) {
double old_angles[NUM_SERVOS];
memcpy(old_angles, angles, sizeof(angles));
state->arm->getTargetAngles(angles);
state->arm->getTargetSpeed(speed);
const int steps = 5;
for(int i = 1; i <= steps; ++i) {
for(int id = 0; id < NUM_SERVOS; ++id) {
cmds.commands[id].utime = utime_now();
cmds.commands[id].position_radians = old_angles[id] + (i * (angles[id] - old_angles[id])) / steps;
cmds.commands[id].speed = speed;
cmds.commands[id].max_torque = 0.7;
}
pthread_mutex_lock(&state->lcm_mutex);
dynamixel_command_list_t_publish(state->lcm, ARM_COMMAND_CHANNEL, &cmds);
pthread_mutex_unlock(&state->lcm_mutex);
usleep(100000);
}
}
}
}
usleep(1000000/hz);
}
free(cmds.commands);
return NULL;
}
void *aciThread(void)
{
int result = 0;
unsigned char data = 0;
while(1)
{
result = read(fd, &data, 1);
while (result > 0) {
aciReceiveHandler(data);
result = read(fd, &data, 1);
}
if(var_getted) {
aciSynchronizeVars();
state_msg.timestamp = utime_now();
state_msg.position[0] = 0;
state_msg.position[1] = 0;
state_msg.position[2] = 0;
state_msg.velocity[0] = 0;
state_msg.velocity[1] = 0;
state_msg.velocity[2] = 0;
state_msg.accel[0] = acc_x;
state_msg.accel[1] = acc_y;
state_msg.accel[2] = acc_z;
state_msg.angle[0] = angle_roll;
state_msg.angle[1] = angle_pitch;
state_msg.angle[2] = angle_yaw;
state_msg.angular_vel[0] = roll_vel;
state_msg.angular_vel[1] = pitch_vel;
state_msg.angular_vel[2] = yaw_vel;
state_msg.angular_accel[0] = 0;
state_msg.angular_accel[1] = 0;
state_msg.angular_accel[2] = 0;
trans_msg.timestamp = utime_now();
trans_msg.ch0 = ch0;
trans_msg.ch1 = ch1;
trans_msg.ch2 = ch2;
trans_msg.ch3 = ch3;
trans_msg.ch4 = ch4;
trans_msg.ch5 = ch5;
trans_msg.ch6 = ch6;
trans_msg.ch7 = ch7;
state_t_publish(lcm, "state", &state_msg);
transmitter_t_publish(lcm, "transmitter", &trans_msg);
}
aciEngine();
usleep(10000);
}
return NULL;
}
void *fetchDataThread(void) {
while(1) {
if(var_getted) {
pthread_mutex_lock(&USB_port_mutex);
aciSynchronizeVars();
pthread_mutex_unlock(&USB_port_mutex);
state_msg.timestamp = utime_now();
state_msg.position[0] = 0;
state_msg.position[1] = 0;
state_msg.position[2] = 0;
state_msg.velocity[0] = 0;
state_msg.velocity[1] = 0;
state_msg.velocity[2] = 0;
state_msg.accel[0] = acc_x;
state_msg.accel[1] = acc_y;
state_msg.accel[2] = acc_z;
state_msg.angle[0] = angle_roll;
state_msg.angle[1] = angle_pitch;
state_msg.angle[2] = angle_yaw;
state_msg.angular_vel[0] = roll_vel;
state_msg.angular_vel[1] = pitch_vel;
state_msg.angular_vel[2] = yaw_vel;
state_msg.angular_accel[0] = 0;
state_msg.angular_accel[1] = 0;
state_msg.angular_accel[2] = 0;
trans_msg.timestamp = utime_now();
trans_msg.ch0 = ch0;
trans_msg.ch1 = ch1;
trans_msg.ch2 = ch2;
trans_msg.ch3 = ch3;
trans_msg.ch4 = ch4;
trans_msg.ch5 = ch5;
trans_msg.ch6 = ch6;
trans_msg.ch7 = ch7;
state_t_publish(lcm, "state", &state_msg);
transmitter_t_publish(lcm, "transmitter", &trans_msg);
}
usleep(10000);
}
return NULL;
}
void publish_pid_state(pid_state_t* pid){
pid_status_t pid_msg;
pid_msg.utime = ((double)utime_now())/1000000.0;
pid_msg.kp = pid->kp;
pid_msg.ki = pid->ki;
pid_msg.kd = pid->kd;
pid_msg.iterm = pid->iterm;
pid_msg.prev_time = pid->prev_time;
// Control Flags
pid_msg.reached_target = pid->reached_target;
pid_msg.first_time = pid->first_time;
pid_msg.output = pid->output;
pid_msg.error = pid->error;
pid_msg.error_dot = pid->error_dot;
pid_msg.x_ref = pid->x_ref;
pid_msg.x_ref_dot = pid->x_ref_dot;
// send lcm message to motors
//puts("Publishing..");
pid_status_t_publish((lcm_t *)lcm, pid->name, &pid_msg);
}
int main()
{
lcm = lcm_create(NULL);
channels_t tx_msg;
channels_t_subscribe(lcm, "CHANNELS_.*_RX", handler_channels_lcm, rxdata);
for(;;) {
lcm_handle(lcm);
//do transformation of incoming data here
for(int i=0; i<8; i++){
txdata[i] = rxdata[i]-500;
}
//make tx msg
tx_msg.utime = utime_now();
tx_msg.num_channels = 8;
for(int i=0; i < tx_msg.num_channels; i++){
tx_msg.channels[i] = txdata[i];
}
//pblish transmint msg
channels_t_publish(lcm, "CHANNELS_1_TX", &tx_msg);
}
lcm_destroy(lcm);
return 0;
}
void *fetchDataThread(void) {
while(1) {
if(var_getted) {
aciSynchronizeVars();
var_msg.timestamp = utime_now();
var_msg.accel[0] = acc_x;
var_msg.accel[1] = acc_y;
var_msg.accel[2] = acc_z;
var_msg.angle[0] = angle_roll;
var_msg.angle[1] = angle_pitch;
var_msg.angle[2] = angle_yaw;
var_msg.angular_vel[0] = roll_vel;
var_msg.angular_vel[1] = pitch_vel;
var_msg.angular_vel[2] = yaw_vel;
var_t_publish(lcm, "variables", &var_msg);
}
usleep(10000);
}
return NULL;
}
void *
diff_drive_thread (void *arg)
{
lcm_t *lcm = lcm_create (NULL);
uint64_t utime_start;
while(1) {
utime_start = utime_now ();
pthread_mutex_lock (&msg_mutex);
maebot_motor_command_t_publish (lcm, "MAEBOT_MOTOR_COMMAND", &msg);
pthread_mutex_unlock (&msg_mutex);
usleep (CMD_PRD - (utime_now() - utime_start));
}
return NULL;
}
static int get_frame(image_source_t *isrc, image_source_data_t * frmd)//void **imbuf, int *buflen)
{
assert(isrc->impl_type == IMPL_TYPE);
impl_v4l2_t *impl = (impl_v4l2_t*) isrc->impl;
memset(frmd, 0, sizeof(image_source_data_t));
fd_set fds;
FD_ZERO (&fds);
FD_SET (impl->fd, &fds);
int r = select(impl->fd + 1, &fds, NULL, NULL, NULL);
if (-1 == r) {
perror("select");
return -2;
}
if (0 == r) {
// timeout
return -1;
}
// Read the frame
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(struct v4l2_buffer));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
if (-1 == ioctl (impl->fd, VIDIOC_DQBUF, &buf)) {
switch (errno) {
case EAGAIN:
return 0;
case EIO:
// Could ignore EIO, see spec.
// fall through
default:
perror("VIDIOC_DQBUF");
return -1;
}
}
frmd->datalen = buf.bytesused;
frmd->data = impl->buffers[buf.index].start;
frmd->ifmt.width = impl->formats[impl->current_format_idx].width;
frmd->ifmt.height = impl->formats[impl->current_format_idx].height;
strcpy(frmd->ifmt.format, impl->formats[impl->current_format_idx].format);
frmd->utime = utime_now(); // Can we get better timing than this from v4l2?
return 0;
}
void *
status_loop (void *user)
{
printf ("NFO: Starting status loop.\n");
arm_state_t *arm_state = user;
dynamixel_status_list_t stats;
stats.len = arm_state->num_servos;
stats.statuses = calloc (arm_state->num_servos, sizeof (*stats.statuses));
while (1) {
int64_t utime = utime_now ();
pthread_mutex_lock (&arm_state->serial_lock);
for (int id = 0; id < arm_state->num_servos; id++) {
stats.statuses[id].utime = utime_now ();
dynamixel_device_status_t *stat = arm_state->servos[id]->get_status (arm_state->servos[id]);
stats.statuses[id].error_flags = stat->error_flags;
stats.statuses[id].position_radians = stat->position_radians;
stats.statuses[id].speed = stat->speed;
stats.statuses[id].load = stat->load;
stats.statuses[id].voltage = stat->voltage;
stats.statuses[id].temperature = stat->temperature;
dynamixel_device_status_destroy (stat);
}
pthread_mutex_unlock (&arm_state->serial_lock);
// Publish
dynamixel_status_list_t_publish (arm_state->lcm, arm_state->status_channel, &stats);
// Attempt to send messages at a fixed rate
int hz = 15;
int64_t max_delay = (1000000 / hz);
int64_t now = utime_now ();
int64_t delay = imin64 (now - utime, max_delay);
utime = now;
usleep (max_delay - delay);
}
return NULL;
}
void *
diff_drive_thread (void *arg)
{
lcm_t *lcm = lcm_create (NULL);
uint64_t utime_start;
while(1) {
utime_start = utime_now ();
pthread_mutex_lock (&msg_mutex);
{
msg.utime = utime_now ();
maebot_diff_drive_t_publish (lcm, "MAEBOT_DIFF_DRIVE", &msg);
}
pthread_mutex_unlock (&msg_mutex);
usleep (CMD_PRD - (utime_now() - utime_start));
}
return NULL;
}
void publish_state(void){
pose_t pose_msg;
pose_msg.utime = ((double)utime_now())/1000000.0;
pose_msg.num_channels = 8;
pose_msg.channels = (float*) malloc(pose_msg.num_channels*sizeof(float));
float tmp;
for(int i = 0; i < pose_msg.num_channels; i++){
tmp = state->pose[i];
pose_msg.channels[i] = tmp;
}
pose_t_publish((lcm_t *)lcm, "POSE", &pose_msg);
free(pose_msg.channels);
}
int main()
{
int8_t buffer[MAX_PACKET];
pthread_t serial_monitor_thread;
state_t *state = calloc(1, sizeof(state_t));
state->atmegafd = open("/dev/ttyATH0", O_RDWR | O_NOCTTY | O_NDELAY);
pthread_mutex_init(&state->atmegafp_mutex, NULL);
pthread_mutex_init(&state->clientfd_mutex, NULL);
pthread_mutex_init(&state->sockfd_mutex, NULL);
pthread_mutex_init(&state->last_utime_mutex, NULL);
//pthread_create(&serial_monitor_thread, NULL, serial_monitor, state);
while(1){
network_connect(state);
while (1) {
int len;
if ((len = recv(state->sockfd, buffer, sizeof(buffer), 0)) <= 0) {
if(debug){
perror("recv");
printf("[atherosd] Connection Broken\n");
}
break;
}
pthread_mutex_lock(&state->last_utime_mutex);
state->last_utime = utime_now();
pthread_mutex_unlock(&state->last_utime_mutex);
process_message(state, buffer, len);
}
close(state->sockfd);
close(state->clientfd);
sleep(1);
if(debug)
printf("[atherosd] Restarting Connection\n");
}
return 0;
}
void *publishCmmdThread(void) {
while(1){
cmmdMsg.timestamp = utime_now();
cmmdMsg.thrust = thrust;
cmmdMsg.roll = roll;
cmmdMsg.pitch = pitch;
cmmdMsg.yaw = yaw;
command_t_publish(lcm, "command_msg", &cmmdMsg);
usleep(10000);
}
return NULL;
}
int _db_gc(pgctx_t *ctx, gcstats_t *stats)
{
int64_t t0, t1, t2, t3, t4, t5;
memheap_t *heap = _ptr(ctx, ctx->root->heap);
t0 = utime_now();
pmem_gc_mark(&ctx->mm, heap, 0);
t1 = utime_now();
// Synchronize here. All this does is make sure anyone who was
// in the database during the mark phase is out before we do the
// walk phase.
_dblockop(ctx, MLCK_WR, ctx->root->lock);
_dblockop(ctx, MLCK_UN, ctx->root->lock);
// Eliminate the structures used by the memory subsystem itself
gc_keep(ctx, heap);
gc_keep(ctx, _ptr(ctx, heap->pool));
// Eliminated references in the meta table
if (isPtr(ctx->root->meta.id.type)) {
gc_keep(ctx, dbptr(ctx, ctx->root->meta.id));
}
t2 = utime_now();
gc_walk(ctx, ctx->cache);
t3 = utime_now();
// Eliminate references that have parents that extend back to
// the root "data" objects
gc_walk(ctx, ctx->data);
// Also any references owned by all currently running processes
gc_walk(ctx, ctx->root->pidcache);
t4 = utime_now();
// Free everything that remains
//pmem_gc_free(&ctx->mm, heap, 0, (gcfreecb_t)dbcache_del, ctx);
pmem_gc_free(&ctx->mm, heap, 0, NULL, ctx);
t5 = utime_now();
log_debug("GC timing:");
log_debug(" mark: %lldus", t1-t0);
log_debug(" sync: %lldus", t2-t1);
log_debug(" cache: %lldus", t3-t2);
log_debug(" walk: %lldus", t4-t3);
log_debug(" free: %lldus", t5-t4);
log_debug(" total: %lldus", t5-t0);
return 0;
}
double pid_get_output(pid_ctrl_t *pid, double meas) {
clock_t cur_clock = clock();
double cur_time = utime_now()/1000000.0;
//printf("clock: %d, prev_clock: %d, utime: %f\n",cur_clock, pid->prev_clk, utime_now()/1000000.0);
//double dt = (cur_clock - pid->prev_clk + 0.0)/CLOCKS_PER_SEC;
double dt = cur_time - pid->prev_time;
double err = pid->goal - meas;
//printf("%f, %f\n",dt,err);
/*
if(err < 0) {
printf("ERROR is < 0\n");
}
else if(err > 0) {
printf("ERROR is > 0\n");
}
*/
double derivative = 0;
if(pid->first_meas) {
pid->first_meas = 0;
} else {
//Don't want any nasty nan's
pid->integral += err*dt;
if(dt == 0) {
derivative = 0;
} else {
derivative = (err - pid->prev_err)/dt;
}
//printf("err: %f, prev_err: %f\n",err, pid->prev_err);
//printf("derivative: %f, dt: %f\n",derivative, dt);
}
//If passes, get rid of integral
if(sign(pid->prev_err) != sign(err)) {
//printf("SWITCH\n");
pid->integral = 0;
}
double proportion = pid->P * err;
double integral = pid->I * pid->integral;
double deriv_out = pid->D * derivative;
/*
printf("proportion: %f\n",proportion);
printf("integral : %f\n",integral);
printf("derivative: %f\n",deriv_out);
*/
double output = proportion +
integral +
deriv_out;
if(fabs(output) > pid->max_val) {
output = sign(output) * pid->max_val;
}
//STOP when within error bounds
if(fabs(err) < pid->min_output) {
pid->integral = 0;
output = 0;
}
pid->prev_err = err;
pid->prev_clk = cur_clock;
pid->prev_time = cur_time;
return output;
}
