Exemplo n.º 1
0
void Replay::loop()
{
    char type[5];

    if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) {
        if (!hal.util->get_soft_armed()) {
            hal.util->set_soft_armed(true);
            ::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis());
        }
    }

    if (!logreader.update(type)) {
        ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f);
        flush_and_exit();
    }

    if (last_timestamp != 0) {
        uint64_t gap = AP_HAL::micros64() - last_timestamp;
        if (gap > 40000) {
            ::printf("Gap in log at timestamp=%lu of length %luus\n",
                     last_timestamp, gap);
        }
    }
    last_timestamp = AP_HAL::micros64();

    read_sensors(type);

    if (!streq(type,"ATT")) {
        return;
    }
}
Exemplo n.º 2
0
int main(int argc, char **argv)
{
  ros::init(argc, argv, "imu");

  ros::NodeHandle nh;
  ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000);

  std_msgs::Float64 msg;

  ros::Rate loopRate(10);

  int temp = 0;

  removegyrooff();

  while (ros::ok())
  {
    read_sensors();
    msg.data = TO_DEG(-atan2(magnetom[1], magnetom[0]));
    chatter_pub.publish(msg);
    ROS_INFO("%s %f", "send an imu message", TO_DEG(-atan2(magnetom[1], magnetom[0])));
    // ROS_INFO("%d \n",temp);
    ros::spinOnce();
    loopRate.sleep();
    temp++;
  }
}
Exemplo n.º 3
0
void readImu()
{
	timestamp_old = timestamp;
	timestamp = millis();
	if (timestamp > timestamp_old)
		G_Dt = (float)(timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
	else G_Dt = 0;

	// Update sensor readings
	read_sensors();

	if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS)  // We're in calibration mode
	{
		check_reset_calibration_session();  // Check if this session needs a reset
	}
	else if (output_mode == OUTPUT__MODE_ANGLES)  // Output angles
	{
		// Apply sensor calibration
		compensate_sensor_errors();

		// Run DCM algorithm
		Compass_Heading(); // Calculate magnetic heading
		Matrix_update();
		Normalize();
		Drift_correction();
		Euler_angles();
	}
}
Exemplo n.º 4
0
// Read every sensor and record a time stamp
// Init DCM with unfiltered orientation
// TODO re-init global vars?
void reset_sensor_fusion() {
	float temp1[3];
	float temp2[3];
	float xAxis[] = { 1.0f, 0.0f, 0.0f };

	read_sensors();
	timestamp = millis();

	// GET PITCH
	// Using y-z-plane-component/x-component of gravity vector
	pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2]));

	// GET ROLL
	// Compensate pitch of gravity vector 
	Vector_Cross_Product(temp1, accel, xAxis);
	Vector_Cross_Product(temp2, xAxis, temp1);
	// Normally using x-z-plane-component/y-component of compensated gravity vector
	// roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
	// Since we compensated for pitch, x-z-plane-component equals z-component:
	roll = atan2(temp2[1], temp2[2]);

	// GET YAW
	Compass_Heading();
	yaw = MAG_Heading;

	// Init rotation matrix
	init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
}
Exemplo n.º 5
0
static int monitor_task(void *arg)
{
	struct thermostat* th = arg;
	
	while(!kthread_should_stop()) {
		if (unlikely(freezing(current)))
			refrigerator();

		msleep_interruptible(2000);

#ifdef DEBUG
		DumpTachoRegisters();
#endif

		read_sensors(th);

		update_fan_speed(th);

#ifdef DEBUG
		/* be carefule with the stats displayed. The Fan Counter value depends 
		 * on what value is written in the register during the read sensors 
		 * call. If its in temperature read setting, the fan counter and hence 
		 * the rpm will be WRONG
		 */
		display_stats(th);
#endif
	}

	return 0;
}
Exemplo n.º 6
0
void loop()
{
  read_sensors();

  short direction = drive.forward;

  int speed = clamp(
          vertical.value(),
          drive.v_center - drive.v_gap - drive.v_range,
          drive.v_center + drive.v_gap + drive.v_range);

  // speed is from 0 to 100
  if (speed > drive.v_center + drive.v_gap) {
    speed = (speed - (drive.v_center + drive.v_gap)) / 4;
    direction = drive.backward;
  } else if (speed < drive.v_center - drive.v_gap) {
    speed = (drive.v_center - drive.v_gap - speed) / 4;
  } else {
    speed = 0;
  }

  // side is between -100 and 100
  // -100 is all the way on the right
  int side = clamp(
          horizontal.value(),
          drive.h_center - drive.h_gap - drive.h_range,
          drive.h_center + drive.h_gap + drive.h_range);

  if (side > drive.h_center + drive.h_gap) {
    side = (side - (drive.h_center + drive.h_gap)) / 4;
  } else if (side < (drive.h_center - drive.h_gap)) {
    side = (side - (drive.h_center - drive.h_gap)) / 4;
  } else {
    side = 0;
  }

  // allow more careful controls while turning
  short min_speed = -drive.turn_assist * direction;
  short max_speed = speed + (drive.turn_assist * direction);

  // get the left/right speed -- never more than limits
  long right = direction * clamp(speed - side, min_speed, max_speed);
  long left = direction * clamp(speed + side, min_speed, max_speed);

  // proportional to the max speed we want sabertooth to go
  left = drive.max_speed * left / 100;
  right = drive.max_speed * right / 100;

  send_velocity_to_computer(speed, side, left, right);
  send_velocity_to_sabertooth(left, right);

  // toggle the run led every time
  toggle_led();
}
/**
  Turn left at an intersection.
*/
void turn_left(){
	buzzer_off();
	motion_set(0x05);
	velocity(100,100);
	_delay_ms(1000);
	while(1){
		print_sensor_data();
		read_sensors();
		if(Center_white_line < W_THRESHOLD) break;
	}
	velocity(0,0);
}
Exemplo n.º 8
0
void tx_thread(void)
{
   while(1)
   {

      send_buf.size = read_sensors(&send_buf);
      
      com_send(IFACE_RADIO, &send_buf);
      printf("[TX] sent %d bytes\n", send_buf.size);
      mos_led_toggle(0);
      mos_thread_sleep(5000);
   }
}
Exemplo n.º 9
0
void
hi_reload(gint entry)
{
    switch (entry) {
    case COMPUTER_FILESYSTEMS:
	scan_filesystems();
	break;
    case COMPUTER_NETWORK:
	scan_net_interfaces();
	break;
    case COMPUTER_SENSORS:
	read_sensors();
	break;
    }
}
Exemplo n.º 10
0
static Computer *
computer_get_info(void)
{
    Computer *computer;

    computer = g_new0(Computer, 1);
    
    if (moreinfo) {
#ifdef g_hash_table_unref
	g_hash_table_unref(moreinfo);
#else
	g_free(moreinfo);
#endif
    }

    moreinfo = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, NULL);

    shell_status_update("Getting processor information...");
    computer->processor = computer_get_processor();

    shell_status_update("Getting memory information...");
    computer->memory = computer_get_memory();

    shell_status_update("Getting operating system information...");
    computer->os = computer_get_os();

    shell_status_update("Getting display information...");
    computer->display = computer_get_display();

    shell_status_update("Getting sound card information...");
    computer->alsa = computer_get_alsainfo();

    shell_status_update("Getting mounted file system information...");
    scan_filesystems();

    shell_status_update("Getting shared directories...");
    scan_shared_directories();
    
    shell_status_update("Reading sensors...");
    read_sensors();

    shell_status_update("Obtaining network information...");
    scan_net_interfaces();

    computer->date_time = "...";
    return computer;
}
Exemplo n.º 11
0
// begin code
void setup()
{
  // initialize the serial communication with the server
  Serial.begin(9600);

  // start the wire protocol
  Wire.begin();

  // initialize communication with the sabertooth motor controller
  sabertoothSerial.begin(19200);
  sabertoothSerial.write(uint8_t(0));

  // initialize the interrupts on encoders
  pinMode(LeftEncoderPin, INPUT);           // set pin to input
  digitalWrite(LeftEncoderPin, HIGH);       // turn on pullup resistors
  attachInterrupt(LeftEncoderPin - 2, left_encoder_interrupt, FALLING);

  pinMode(RightEncoderPin, INPUT);           // set pin to input
  digitalWrite(RightEncoderPin, HIGH);       // turn on pullup resistors
  attachInterrupt(RightEncoderPin - 2, right_encoder_interrupt, FALLING);

  // initialize the led pin
  pinMode(StoppedLEDPin, OUTPUT);
  pinMode(WarnLEDPin, OUTPUT);
  pinMode(RunLEDPin, OUTPUT);

  // turn on the warn led to indicate calibration
  digitalWrite(WarnLEDPin, HIGH);

  int h_reads = 0,
      v_reads = 0,
      reads = 16;

  for (int i =0; i < reads; i++) {
    read_sensors();
    h_reads += horizontal.value();
    v_reads += vertical.value();
  }

  drive.h_center = h_reads / reads;
  drive.v_center = v_reads / reads;

  // calibration complete
  digitalWrite(WarnLEDPin, LOW);
}
Exemplo n.º 12
0
void * production_thread(void * dummy)
{
	SystemState data;
	while (!done) {
		// Fill the data structure
		memset(&data,0,sizeof(SystemState));

		rtx_thread_setcancel(RTX_THREAD_CANCEL_DISABLE);
		read_uptime(&data);
		read_mem(&data);
		if (readsensors) {
			read_sensors(&data);
		}
		rtx_thread_setcancel(RTX_THREAD_CANCEL_DEFERRED);
		if (done) break;

		// Write the data to the store
		dataV.t_writefrom(data);
		rtx_timer_sleep(period);
	}
	return NULL;
}
/**
  Go forward by a certain specified number of steps.
*/
void go_distance(unsigned char x)
{
	reset_shaft_counters();
   forward();
	velocity(100,100);
	PORTJ = 0x00;
	while(1){
		read_sensors();
		print_sensor_data();
		if( Front_IR_Sensor<0xF0)
		{
			stop();
			buzzer_on();
		}
		else
		{
			forward();
			buzzer_off();
		}
		if((ShaftCountLeft + ShaftCountRight)*5 > x*10)
			break;
	}
	velocity(0,0);
}
Exemplo n.º 14
0
int main()
{

	ADCinit();
	sei();
	read_sensors();
	TWIslave_init();

	//-------------------------------
	//		Lite Test
	//-------------------------------

	DDRB = 0xFF;
	 
	direction = 0;
	for(;;)
	{	
		look_for_walls();
		PORTB = get_local_wall(1,1);
	}
	

	return 0;
}
Exemplo n.º 15
0
int main(void)
{
    CanopyContext ctx;
    CanopyResultEnum result;
    uint64_t reportTimer = 0;

    result = canopy_set_global_opt(
        CANOPY_LOG_LEVEL, 0,
        CANOPY_LOG_PAYLOADS, true);

    ctx = canopy_init_context();
    if (!ctx) {
        fprintf(stderr, "Error initializing context\n");
        return -1;
    }

    result = canopy_set_opt(ctx,
        CANOPY_DEVICE_UUID, "e6968460-f010-48ef-8e69-835543843b32",
        CANOPY_DEVICE_SECRET_KEY, "/pMdwTzEA3+d66qo3MZQ2bWjsYGXAHAb",
        CANOPY_CLOUD_SERVER, "sandbox.canopy.link"
    );

    if (result != CANOPY_SUCCESS){
        fprintf(stderr, "Failed to configure context\n");
        return -1;
    }

    result = canopy_var_init(ctx, "out float32 temperature");
    if (result != CANOPY_SUCCESS){
        fprintf(stderr, "Failed to init cloudvar 'temperature'\n");
        return -1;
    }
    result = canopy_var_init(ctx, "out float32 humidity");
    if (result != CANOPY_SUCCESS){
        fprintf(stderr, "Failed to init cloudvar 'humidity'\n");
        return -1;
    }
    result = canopy_var_init(ctx, "inout int8 fan_speed");
    if (result != CANOPY_SUCCESS){
        fprintf(stderr, "Failed to init cloudvar 'fan_speed'\n");
        return -1;
    }

    /*result = canopy_var_on_change(ctx, "fan_speed", on_fan_speed_change, NULL);
    if (result != CANOPY_SUCCESS) {
        fprintf(stderr, "Error setting up fan_speed callback\n");
        return -1;
    }*/

    // turn fan off
    init_fan_pins();
    result = canopy_var_set_int8(ctx, "fan_speed", 0);
    if (!ctx) {
        fprintf(stderr, "Error setting fan_speed\n");
        return -1;
    }

    while (1) {
        int8_t speed;
        canopy_sync_blocking(ctx, 10*CANOPY_SECONDS);

        result = canopy_var_get_int8(ctx, "fan_speed", &speed);
        if (result != CANOPY_SUCCESS) {
            fprintf(stderr, "Error reading fan_speed\n");
            return -1;
        }
        set_fan_speed(speed);

        if (canopy_once_every(&reportTimer, 60*CANOPY_SECONDS)) {
            printf("reading sensors...\n");
            float temperature, humidity;
            bool sensorOk;
            sensorOk = read_sensors(&temperature, &humidity);
            if (sensorOk) {
                printf("Temperature: %f    Humidity: %f\n", temperature, humidity);
                result = canopy_var_set_float32(ctx, "temperature", temperature);
                if (result != CANOPY_SUCCESS) {
                    fprintf(stderr, "Failed to set cloudvar 'temperature'\n");
                    return -1;
                }

                result = canopy_var_set_float32(ctx, "humidity", humidity);
                if (result != CANOPY_SUCCESS) {
                    fprintf(stderr, "Failed to set cloudvar 'humidity'\n");
                    return -1;
                }
            }
        }
    }
}
Exemplo n.º 16
0
void Replay::loop()
{
    while (true) {
        char type[5];

        if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) {
            if (!hal.util->get_soft_armed()) {
                hal.util->set_soft_armed(true);
                ::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis());
            }
        }

        if (!logreader.update(type)) {
            ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f);
            fclose(plotf);
            break;
        }
        read_sensors(type);

        if (streq(type,"ATT")) {
            Vector3f ekf_euler;
            Vector3f velNED;
            Vector3f posNED;
            Vector3f gyroBias;
            float accelWeighting;
            float accelZBias1;
            float accelZBias2;
            Vector3f windVel;
            Vector3f magNED;
            Vector3f magXYZ;
            Vector3f DCM_attitude;
            Vector3f ekf_relpos;
            Vector3f velInnov;
            Vector3f posInnov;
            Vector3f magInnov;
            float    tasInnov;
            float velVar;
            float posVar;
            float hgtVar;
            Vector3f magVar;
            float tasVar;
            Vector2f offset;
            uint16_t faultStatus;

            const Matrix3f &dcm_matrix = _vehicle.ahrs.AP_AHRS_DCM::get_rotation_body_to_ned();
            dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z);
            _vehicle.EKF.getEulerAngles(ekf_euler);
            _vehicle.EKF.getVelNED(velNED);
            _vehicle.EKF.getPosNED(posNED);
            _vehicle.EKF.getGyroBias(gyroBias);
            _vehicle.EKF.getIMU1Weighting(accelWeighting);
            _vehicle.EKF.getAccelZBias(accelZBias1, accelZBias2);
            _vehicle.EKF.getWind(windVel);
            _vehicle.EKF.getMagNED(magNED);
            _vehicle.EKF.getMagXYZ(magXYZ);
            _vehicle.EKF.getInnovations(velInnov, posInnov, magInnov, tasInnov);
            _vehicle.EKF.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
            _vehicle.EKF.getFilterFaults(faultStatus);
            _vehicle.EKF.getPosNED(ekf_relpos);
            Vector3f inav_pos = _vehicle.inertial_nav.get_position() * 0.01f;
            float temp = degrees(ekf_euler.z);

            if (temp < 0.0f) temp = temp + 360.0f;
            fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n",
                    AP_HAL::millis() * 0.001f,
                    logreader.get_sim_attitude().x,
                    logreader.get_sim_attitude().y,
                    logreader.get_sim_attitude().z,
                    _vehicle.barometer.get_altitude(),
                    logreader.get_attitude().x,
                    logreader.get_attitude().y,
                    wrap_180_cd(logreader.get_attitude().z*100)*0.01f,
                    logreader.get_inavpos().x,
                    logreader.get_inavpos().y,
                    logreader.get_ahr2_attitude().x,
                    logreader.get_ahr2_attitude().y,
                    wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f,
                    degrees(DCM_attitude.x),
                    degrees(DCM_attitude.y),
                    degrees(DCM_attitude.z),
                    degrees(ekf_euler.x),
                    degrees(ekf_euler.y),
                    degrees(ekf_euler.z),
                    inav_pos.x,
                    inav_pos.y,
                    inav_pos.z,
                    ekf_relpos.x,
                    ekf_relpos.y,
                    -ekf_relpos.z);
            fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n",
                    AP_HAL::millis() * 0.001f,
                    degrees(ekf_euler.x),
                    degrees(ekf_euler.y),
                    temp,
                    velNED.x, 
                    velNED.y, 
                    velNED.z, 
                    posNED.x, 
                    posNED.y, 
                    posNED.z, 
                    60*degrees(gyroBias.x), 
                    60*degrees(gyroBias.y), 
                    60*degrees(gyroBias.z), 
                    windVel.x, 
                    windVel.y, 
                    magNED.x, 
                    magNED.y, 
                    magNED.z, 
                    magXYZ.x, 
                    magXYZ.y, 
                    magXYZ.z,
                    logreader.get_attitude().x,
                    logreader.get_attitude().y,
                    logreader.get_attitude().z);

            // define messages for EKF1 data packet
            int16_t     roll  = (int16_t)(100*degrees(ekf_euler.x)); // roll angle (centi-deg)
            int16_t     pitch = (int16_t)(100*degrees(ekf_euler.y)); // pitch angle (centi-deg)
            uint16_t    yaw   = (uint16_t)wrap_360_cd(100*degrees(ekf_euler.z)); // yaw angle (centi-deg)
            float       velN  = (float)(velNED.x); // velocity North (m/s)
            float       velE  = (float)(velNED.y); // velocity East (m/s)
            float       velD  = (float)(velNED.z); // velocity Down (m/s)
            float       posN  = (float)(posNED.x); // metres North
            float       posE  = (float)(posNED.y); // metres East
            float       posD  = (float)(posNED.z); // metres Down
            float       gyrX  = (float)(6000*degrees(gyroBias.x)); // centi-deg/min
            float       gyrY  = (float)(6000*degrees(gyroBias.y)); // centi-deg/min
            float       gyrZ  = (float)(6000*degrees(gyroBias.z)); // centi-deg/min

            // print EKF1 data packet
            fprintf(ekf1f, "%.3f %u %d %d %u %.2f %.2f %.2f %.2f %.2f %.2f %.0f %.0f %.0f\n",
                    AP_HAL::millis() * 0.001f,
                    AP_HAL::millis(),
                    roll, 
                    pitch, 
                    yaw, 
                    velN, 
                    velE, 
                    velD, 
                    posN, 
                    posE, 
                    posD, 
                    gyrX,
                    gyrY,
                    gyrZ);

            // define messages for EKF2 data packet
            int8_t  accWeight  = (int8_t)(100*accelWeighting);
            int8_t  acc1  = (int8_t)(100*accelZBias1);
            int8_t  acc2  = (int8_t)(100*accelZBias2);
            int16_t windN = (int16_t)(100*windVel.x);
            int16_t windE = (int16_t)(100*windVel.y);
            int16_t magN  = (int16_t)(magNED.x);
            int16_t magE  = (int16_t)(magNED.y);
            int16_t magD  = (int16_t)(magNED.z);
            int16_t magX  = (int16_t)(magXYZ.x);
            int16_t magY  = (int16_t)(magXYZ.y);
            int16_t magZ  = (int16_t)(magXYZ.z);

            // print EKF2 data packet
            fprintf(ekf2f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n",
                    AP_HAL::millis() * 0.001f,
                    AP_HAL::millis(),
                    accWeight, 
                    acc1, 
                    acc2, 
                    windN, 
                    windE, 
                    magN, 
                    magE, 
                    magD, 
                    magX,
                    magY,
                    magZ);

            // define messages for EKF3 data packet
            int16_t innovVN = (int16_t)(100*velInnov.x);
            int16_t innovVE = (int16_t)(100*velInnov.y);
            int16_t innovVD = (int16_t)(100*velInnov.z);
            int16_t innovPN = (int16_t)(100*posInnov.x);
            int16_t innovPE = (int16_t)(100*posInnov.y);
            int16_t innovPD = (int16_t)(100*posInnov.z);
            int16_t innovMX = (int16_t)(magInnov.x);
            int16_t innovMY = (int16_t)(magInnov.y);
            int16_t innovMZ = (int16_t)(magInnov.z);
            int16_t innovVT = (int16_t)(100*tasInnov);

            // print EKF3 data packet
            fprintf(ekf3f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n",
                    AP_HAL::millis() * 0.001f,
                    AP_HAL::millis(),
                    innovVN, 
                    innovVE, 
                    innovVD, 
                    innovPN, 
                    innovPE, 
                    innovPD, 
                    innovMX, 
                    innovMY, 
                    innovMZ, 
                    innovVT);

            // define messages for EKF4 data packet
            int16_t sqrtvarV = (int16_t)(constrain_float(100*velVar,INT16_MIN,INT16_MAX));
            int16_t sqrtvarP = (int16_t)(constrain_float(100*posVar,INT16_MIN,INT16_MAX));
            int16_t sqrtvarH = (int16_t)(constrain_float(100*hgtVar,INT16_MIN,INT16_MAX));
            int16_t sqrtvarMX = (int16_t)(constrain_float(100*magVar.x,INT16_MIN,INT16_MAX));
            int16_t sqrtvarMY = (int16_t)(constrain_float(100*magVar.y,INT16_MIN,INT16_MAX));
            int16_t sqrtvarMZ = (int16_t)(constrain_float(100*magVar.z,INT16_MIN,INT16_MAX));
            int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX));
            int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX));
            int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX));

            // print EKF4 data packet
            fprintf(ekf4f, "%.3f %u %d %d %d %d %d %d %d %d %d %d\n",
                    AP_HAL::millis() * 0.001f,
                    (unsigned)AP_HAL::millis(),
                    (int)sqrtvarV,
                    (int)sqrtvarP,
                    (int)sqrtvarH,
                    (int)sqrtvarMX, 
                    (int)sqrtvarMY, 
                    (int)sqrtvarMZ,
                    (int)sqrtvarVT,
                    (int)offsetNorth,
                    (int)offsetEast,
                    (int)faultStatus);
        }
    }

    flush_dataflash();

    if (check_solution) {
        report_checks();
    }
    exit(0);
}
Exemplo n.º 17
0
void main(void)
{
  device_init(); 
  
  //START
  START_MOTOR;
  FORWARD;
  set_speed(SPEED_MIN);       
  
  while (1)
      {         
        //BREAKERS
        if(LEFT_BREAKER) 
        { 
          //-----------------------
          BACKWARD;
          set_speed(SPEED_BACKWARD);delay_ms(50);
          set_servo(MAX_LEFT);delay_ms(150);
          
          STOP;
                        
          set_servo(MAX_RIGHT);delay_ms(100);
          FORWARD;
          set_speed(SPEED_MIN);delay_ms(20);
          
          continue;
          //-----------------------
        };
        
        if(RIGHT_BREAKER) 
        {          
          //-----------------------          
          BACKWARD;
          set_speed(SPEED_BACKWARD);delay_ms(50);
          set_servo(MAX_RIGHT);delay_ms(150);
          
          STOP;
                        
          set_servo(MAX_LEFT);delay_ms(100);
          FORWARD;
          set_speed(SPEED_MIN);delay_ms(20);
          
          continue; 
          //----------------------- 
        }; 
                                               
        //IR_SENSOR
        if(IR_SENSOR) 
        {
          BACKWARD;
          set_speed(SPEED_BACKWARD);delay_ms(30);
          set_servo(MAX_LEFT);delay_ms(70);
          
          STOP;
                        
          set_servo(MAX_RIGHT);delay_ms(100);
          FORWARD;
          set_speed(SPEED_MIN);delay_ms(20);
          
          continue;
        };      
        
        //reading Sharp2D120X
        read_sensors();
        ////////////////////////////////////////////////
        
        //---------------------------------------------
        //bot control
        bot_control();
        
        ///////////////////////////////////////////////
        //LCD      
        to_lcd();
      };
}
Exemplo n.º 18
0
void Replay::setup()
{
    ::printf("Starting\n");

    uint8_t argc;
    char * const *argv;

    hal.util->commandline_arguments(argc, argv);

    _parse_command_line(argc, argv);

    // _parse_command_line sets up an FPE handler.  We can do better:
    signal(SIGFPE, _replay_sig_fpe);

    hal.console->printf("Processing log %s\n", filename);

    if (update_rate == 0) {
        update_rate = find_update_rate(filename);
    }

    hal.console->printf("Using an update rate of %u Hz\n", update_rate);

    if (!logreader.open_log(filename)) {
        perror(filename);
        exit(1);
    }

    _vehicle.setup();
    set_ins_update_rate(update_rate);

    logreader.wait_type("GPS");
    logreader.wait_type("IMU");
    logreader.wait_type("GPS");
    logreader.wait_type("IMU");

    feenableexcept(FE_INVALID | FE_OVERFLOW);


    plotf = fopen("plot.dat", "w");
    plotf2 = fopen("plot2.dat", "w");
    ekf1f = fopen("EKF1.dat", "w");
    ekf2f = fopen("EKF2.dat", "w");
    ekf3f = fopen("EKF3.dat", "w");
    ekf4f = fopen("EKF4.dat", "w");

    fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt AHR2.Roll AHR2.Pitch AHR2.Yaw DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n");
    fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ WN WE MN ME MD MX MY MZ E1ref E2ref E3ref\n");
    fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n");
    fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n");
    fprintf(ekf3f, "timestamp TimeMS IVN IVE IVD IPN IPE IPD IMX IMY IMZ IVT\n");
    fprintf(ekf4f, "timestamp TimeMS SV SP SH SMX SMY SMZ SVT OFN EFE FS DS\n");

    ::printf("Waiting for GPS\n");
    while (!done_home_init) {
        char type[5];
        if (!logreader.update(type)) {
            break;
        }
        read_sensors(type);
        if (streq(type, "GPS") &&
            (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) &&
            done_baro_init && !done_home_init) {
            const Location &loc = _vehicle.gps.location();
            ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", 
                     loc.lat * 1.0e-7f, 
                     loc.lng * 1.0e-7f,
                     loc.alt * 0.01f,
                     hal.scheduler->millis()*0.001f);
            _vehicle.ahrs.set_home(loc);
            _vehicle.compass.set_initial_location(loc.lat, loc.lng);
            done_home_init = true;
        }
    }
}
Exemplo n.º 19
0
void adjust()
{
	read_sensors();
	calc_fan();
	set_fan();
}
//This is a non-blocking function. It is called once on every iteration of the main loop if "white_line_flag" is ON
//It reads the whiteline sensor values and determines what it should do next to stay on the white line
void whiteline_follow_continue() {

		read_sensors();

		flag=0;
		print_sensor_data();

		if(Center_white_line<W_THRESHOLD_STOP && Left_white_line<W_THRESHOLD_STOP && Right_white_line<W_THRESHOLD_STOP ){
		 	if (whiteline_stop_intersection_flag) {
				whiteline_follow_end();
				send_char(SUCCESS);
			}
			/*else {
				forward();
				velocity(100,100);
				stop_on_timer4_overflow = 1;
				start_timer4();
				while (stop_on_timer4_overflow != 0) {;}
			}
			return;*/
		}

		
		if( Front_IR_Sensor<0xF0)
		{
			stop();
			buzzer_on();
		}
		//Sensor config : 010
		else if(Left_white_line > W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line > W_THRESHOLD)
		{
			forward();
			velocity(150,150);
			black_flag = 0;
			buzzer_off();
		}

		//Sensor config : 110
		else if(Left_white_line < W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line > W_THRESHOLD)
		{
			forward();
			velocity(120,150);
			black_flag = 0;
			buzzer_off();
		}
		
		//Sensor config : 100
		else if(Left_white_line < W_THRESHOLD && Center_white_line > W_THRESHOLD && Right_white_line > W_THRESHOLD)
		{
			PORTA = 0x05;
			velocity(50,130);
			last_on = LEFT_SENSOR;
			black_flag = 0;
			buzzer_off();
		}

		//Sensor config : 011
		else if(Left_white_line > W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line < W_THRESHOLD)
		{
			forward();
			velocity(150,120);
			black_flag = 0;
			buzzer_off();
		}

		//Sensor config : 001
		else if(Left_white_line > W_THRESHOLD && Center_white_line > W_THRESHOLD && Right_white_line < W_THRESHOLD)
		{
			PORTA = 0x0A;
			velocity(130,50);
			last_on = RIGHT_SENSOR;
			black_flag = 0;
			buzzer_off();
		}
		//Sensor config : 000
		else
		{
			buzzer_off();
			if(black_flag >= CONT_BLACK)  {
				if(last_on == LEFT_SENSOR)
					motion_set(0x05);
				else if(last_on == RIGHT_SENSOR)
					motion_set(0x0A);
				velocity(100,100);
				while(1){
					print_sensor_data();
					read_sensors();
					if(Center_white_line < W_THRESHOLD) break;
				}
			}
			black_flag = (black_flag < CONT_BLACK)?black_flag+1:CONT_BLACK;
			forward();
			velocity(0,0);
		}
}
Exemplo n.º 21
0
int main() {
    int i;
    HTS221 hts221;
    pc.baud(115200);
    
    void hts221_init(void);

    // Set LED to RED until init finishes
    SetLedColor(0x1);

    pc.printf(BLU "Hello World from AT&T Shape!\r\n\n\r");
    pc.printf(GRN "Initialize the HTS221\n\r");

    i = hts221.begin();  
    if( i ) 
        pc.printf(BLU "HTS221 Detected! (0x%02X)\n\r",i);
    else
        pc.printf(RED "HTS221 NOT DETECTED!!\n\r");

    printf("Temp  is: %0.2f F \n\r",CTOF(hts221.readTemperature()));
    printf("Humid is: %02d %%\n\r",hts221.readHumidity());
    
    sensors_init();
    read_sensors();

    // Initialize the modem
    printf(GRN "Modem initializing... will take up to 60 seconds" DEF "\r\n");
    do {
        i=mdm_init();
        if (!i) {
            pc.printf(RED "Modem initialization failed!" DEF "\n");
        }
    } while (!i);
    
    //Software init
    software_init_mdm();
 
    // Resolve URL to IP address to connect to
    resolve_mdm();

    //Create a 1ms timer tick function:
    OneMsTicker.attach(OneMsFunction, 0.001f) ;

    iTimer1Interval_ms = SENSOR_UPDATE_INTERVAL_MS;

    // Open the socket (connect to the server)
    sockopen_mdm();

    // Set LED BLUE for partial init
    SetLedColor(0x4);

    // Send and receive data perpetually
    while(1) {
        static unsigned ledOnce = 0;
        if  (bTimerExpiredFlag)
        {
            bTimerExpiredFlag = false;
            sprintf(SENSOR_DATA.Temperature, "%0.2f", CTOF(hts221.readTemperature()));
            sprintf(SENSOR_DATA.Humidity, "%02d", hts221.readHumidity());
            read_sensors(); //read available external sensors from a PMOD and the on-board motion sensor
            char modem_string[512];
            GenerateModemString(&modem_string[0]);
            printf(BLU "Sending to modem : %s" DEF "\n", modem_string); 
            sockwrite_mdm(modem_string);
            sockread_mdm(&MySocketData, 1024, 20);
            
            // If any non-zero response from server, make it GREEN one-time
            //  then the actual FLOW responses will set the color.
            if ((!ledOnce) && (MySocketData.length() > 0))
            {
                ledOnce = 1;
                SetLedColor(0x2);
            }
            
            printf(BLU "Read back : %s" DEF "\n", &MySocketData[0]);
            char * myJsonResponse;
            if (extract_JSON(&MySocketData[0], &myJsonResponse[0]))
            {
                printf(GRN "JSON : %s" DEF "\n", &myJsonResponse[0]);
                parse_JSON(&myJsonResponse[0]);
            }
            else
            {
                printf(RED "JSON : %s" DEF "\n", &myJsonResponse[0]); //most likely an incomplete JSON string
                parse_JSON(&myJsonResponse[0]); //This is risky, as the string may be corrupted
            }
        } //bTimerExpiredFlag
    } //forever loop
}
Exemplo n.º 22
0
int main(int argc, char**argv)
{
  ros::init(argc,argv,"imu");
  ros::NodeHandle n;
  ros::Publisher out_pub = n.advertise<sensor_msgs::Imu >("imuyrp", 1000);
  ros::NodeHandle nh;
  ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000);
  sensor_msgs::Imu imu_msg;
  std_msgs::Float64 msg;
   // Read sensors, init DCM algorithm
	reset_sensor_fusion();
	removegyrooff(); 
	float Y=0.0;
	int i=0;
  while(ros::ok())
	{
		
	  // Time to read the sensors again?
	  if((clock() - timestamp) >= OUTPUT__DATA_INTERVAL)
	  {
	    timestamp_old = timestamp;
	    timestamp = clock();
	    if (timestamp > timestamp_old)
	      G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro 			integration time)
	    else G_Dt = 0;
	    //cout << G_Dt << endl;
	    // Update sensor readings
	    read_sensors();

      // Run DCM algorithm
      Compass_Heading(); // Calculate magnetic heading
      Matrix_update();
      Normalize();
      Drift_correction();
      Euler_angles();
      output_angles();

      imu_msg = sensor_msgs::Imu(); 
      imu_msg.header.stamp = ros::Time::now();
      imu_msg.header.frame_id = "imu";
      imu_msg.orientation.x = TO_DEG(pitch);
      imu_msg.orientation.y = TO_DEG(roll);
      imu_msg.orientation.z = TO_DEG(yaw);
      imu_msg.orientation.w = 0.0;
      imu_msg.orientation_covariance[0] = -1;
      imu_msg.angular_velocity.x = 0.0;
      imu_msg.angular_velocity.y = 0.0;
      imu_msg.angular_velocity.z = 0.0;
      imu_msg.angular_velocity_covariance[0] = -1;
      imu_msg.linear_acceleration.x = 0.0;
      imu_msg.linear_acceleration.y = 0.0;
      imu_msg.linear_acceleration.z = 0.0;
      imu_msg.linear_acceleration_covariance[0] = -1;

      msg.data = TO_DEG(-atan2(magnetom[1],magnetom[0]));
      chatter_pub.publish(msg);
      ROS_INFO("%s %f", "send an imu message",TO_DEG(-atan2(magnetom[1],magnetom[0])));
      //ros::spinOnce();
	  }
	}
}
Exemplo n.º 23
0
// Main loop
void razor_loop(void)
{
	razor_loop_input();

	// Time to read the sensors again?
	if ((timer_systime() - timestamp) >= OUTPUT__DATA_INTERVAL)
	{
		timestamp_old = timestamp;
		timestamp = timer_systime();
		if (timestamp > timestamp_old)
			G_Dt = (double)MS2S(timestamp - timestamp_old); // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
		else
			G_Dt = 0.0;

		// Update sensor readings
		read_sensors();

		switch (output_mode)
		{
			case OUTPUT__MODE_CALIBRATE_SENSORS:
			// We're in calibration mode
			{
				check_reset_calibration_session();  // Check if this session needs a reset
				if (output_stream_on || output_single_on) output_calibration(curr_calibration_sensor);
				break;
			}
			case OUTPUT__MODE_YPR_RAZOR:
			{
				// Apply sensor calibration
				compensate_sensor_errors();

				// Run DCM algorithm
				Compass_Heading(); // Calculate magnetic heading
				Matrix_update();
				Normalize();
				Drift_correction();
				Euler_angles();

				if (output_stream_on || output_single_on) output_angles();
				break;
			}
			case OUTPUT__MODE_YPR_MADGWICK:
			{
				compensate_sensor_errors();
				gyro[0] = GYRO_SCALED_RAD(gyro[0]);
				gyro[1] = GYRO_SCALED_RAD(gyro[1]);
				gyro[2] = GYRO_SCALED_RAD(gyro[2]);
				// acc and mag already scaled in compensate_sensor_errors()

				// Apply Madgwick algorithm
				MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]);
				//MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]);
				//MadgwickAHRSupdate(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, magnetom[0], magnetom[1], magnetom[2]);
				MadgwickYawPitchRoll(&yaw, &pitch, &roll);
				if (output_stream_on || output_single_on) output_angles();
				break;
			}
			case OUTPUT__MODE_YPR_FREEIMU:
			{
				compensate_sensor_errors();
				gyro[0] = GYRO_SCALED_RAD(gyro[0]);
				gyro[1] = GYRO_SCALED_RAD(gyro[1]);
				gyro[2] = GYRO_SCALED_RAD(gyro[2]);
				// acc and mag already scaled in compensate_sensor_errors()

				// get sensorManager and initialise sensor listeners
				//mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
				//initListeners();

				// wait for one second until gyroscope and magnetometer/accelerometer
				// data is initialised then scedule the complementary filter task
				//fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT);

				//public void onSensorChanged(SensorEvent event) {
					//switch(event.sensor.getType()) {
						//case Sensor.TYPE_ACCELEROMETER:
						// copy new accelerometer data into accel array and calculate orientation
						//System.arraycopy(event.values, 0, accel, 0, 3);
						calculateAccMagOrientation();

						//case Sensor.TYPE_GYROSCOPE:
						// process gyro data
						//gyroFunction(event);
						gyroFunction();

						//case Sensor.TYPE_MAGNETIC_FIELD:
						// copy new magnetometer data into magnet array
						//System.arraycopy(event.values, 0, magnet, 0, 3);

						calculateFusedOrientation();

						yaw = -fusedOrientation[0];
						roll = -fusedOrientation[1];
						pitch = fusedOrientation[2];

				//if (output_stream_on || output_single_on) output_angles_freedom();
				if (output_stream_on || output_single_on) output_angles();
				break;
			}
			case OUTPUT__MODE_SENSORS_RAW:
			case OUTPUT__MODE_SENSORS_CALIB:
			case OUTPUT__MODE_SENSORS_BOTH:
			{
				if (output_stream_on || output_single_on) output_sensors();
				break;
			}
		}

		output_single_on = false;

		#if DEBUG__PRINT_LOOP_TIME == true
		//printf("loop time (ms) = %lu\r\n", timer_systime() - timestamp);
		// Not really useful since RTC measures only 4 ms.
		#endif
	}
	#if DEBUG__PRINT_LOOP_TIME == true
	else
	{
		printf("waiting...\r\n");
	}
	#endif

	return;
}