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;
}
Esempio n. 3
0
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);
}
Esempio n. 5
0
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;

}
Esempio n. 7
0
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;
}
Esempio n. 8
0
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;
}
Esempio n. 9
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;
}
Esempio n. 10
0
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);
}
Esempio n. 12
0
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;
}
Esempio n. 13
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;

}
Esempio n. 14
0
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;
}
Esempio n. 15
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;
}