void rotate_right_slowly()
{
	//velocity(150,150);

	while(1)
	{
		stop();
		read_sensor();

		// For center black line
		if(Center_white_line>0x20)
		{
			break;	
		}

		else
		{
			right_degrees(10);			
		}

	}

	//velocity(130,130);
	//stop();

}
/*---------------------------------------------------------------------------*/
static void
tcpip_handler(void)
{
  memset(buf, 0, MAX_PAYLOAD_LEN);

  if(uip_newdata()) {
    len = uip_datalen();
    memcpy(buf, uip_appdata, len);
    PRINTF("%u bytes from [", len);
    PRINT6ADDR(&UIP_IP_BUF->srcipaddr);
    PRINTF("]:%u\n", UIP_HTONS(UIP_UDP_BUF->srcport));
    len = read_sensor(buf);
    if( len ) {
      server_conn->rport = UIP_UDP_BUF->srcport;
      uip_ipaddr_copy(&server_conn->ripaddr, &UIP_IP_BUF->srcipaddr);
      uip_udp_packet_send(server_conn, buf, len);
      PRINTF("Sent %u bytes\n", len);
    }

    /* Restore server connection to allow data from any node */
    uip_create_unspecified(&server_conn->ripaddr);
    server_conn->rport = 0;
  }
  return;
}
	void turnRight()
	{
	forward_mm(50);
	stop();
	right();
	_delay_ms(200);
	read_sensor();
	 while(Right_white_line <0x40 )
	 {
	 read_sensor();
	 right();
	 }
	 stop();
	_delay_ms(200);
	read_sensor();
	forward();
	}
Exemple #4
0
static int handle_sensor_get(coap_rw_buffer_t *scratch, const coap_packet_t *inpkt,
                             coap_packet_t *outpkt, uint8_t id_hi, uint8_t id_lo)
{
        int val = read_sensor();
        
        return coap_make_pb_response(scratch, outpkt, (const uint8_t *)&val, sizeof(int),
                                     id_hi, id_lo, &inpkt->token, COAP_RSPCODE_CONTENT,
                                     COAP_CONTENTTYPE_APPLICATION_LINKFORMAT);
}
uint8_t application_function_light_read(uint8_t nargs, uint8_t args[application_num_args][application_length_args], uint16_t size, uint8_t *dst)
{
	static const __flash char ok[]				= "> light sensor %d ok light [%.3f] Lux\n";
	static const __flash char error_bounds[]	= "> invalid sensor\n";
	static const __flash char twi_error[]		= "> twi error\n";
	static const __flash char overflow[]		= "> sensor overflow\n";

	uint8_t	sensor;
	float	light = 0;

	sensor = atoi((const char *)args[1]);

	switch(sensor)
	{
		case(0):	// tsl2550 standard range
		case(1):	// tsl2550 extended range
		{
			switch(read_sensor(&light, (sensor == 1)))
			{
				case(read_io_error):
				{
					strcpy_P((char *)dst, twi_error);
					return(1);
				}

				case(read_overflow):
				{
					strcpy_P((char *)dst, overflow);
					return(1);
				}
			}

			break;
		}

		default:
		{
			strlcpy_P((char *)dst, error_bounds, (size_t)size);
			return(1);
		}
	}

	light *= eeprom_read_light_cal_factor(sensor);
	light += eeprom_read_light_cal_offset(sensor);

	snprintf_P((char *)dst, size, ok, sensor, light);

	return(1);
}
void GetNode::GetINA231()
{
    read_sensor(&sensor[SENSOR_ARM]);
    read_sensor(&sensor[SENSOR_MEM]);
    read_sensor(&sensor[SENSOR_KFC]);
    read_sensor(&sensor[SENSOR_G3D]);

    armuV = (float)(sensor[SENSOR_ARM].data.cur_uV / 100000) / 10;
    armuA = (float)(sensor[SENSOR_ARM].data.cur_uA / 1000) / 1000;
    armuW = (float)(sensor[SENSOR_ARM].data.cur_uW / 1000) / 1000;

    memuV = (float)(sensor[SENSOR_MEM].data.cur_uV / 100000) / 10;
    memuA = (float)(sensor[SENSOR_MEM].data.cur_uA / 1000) / 1000;
    memuW = (float)(sensor[SENSOR_MEM].data.cur_uW / 1000) / 1000;

    kfcuV = (float)(sensor[SENSOR_KFC].data.cur_uV / 100000) / 10;
    kfcuA = (float)(sensor[SENSOR_KFC].data.cur_uA / 1000) / 1000;
    kfcuW = (float)(sensor[SENSOR_KFC].data.cur_uW / 1000) / 1000;

    g3duV = (float)(sensor[SENSOR_G3D].data.cur_uV / 100000) / 10;
    g3duA = (float)(sensor[SENSOR_G3D].data.cur_uA / 1000) / 1000;
    g3duW = (float)(sensor[SENSOR_G3D].data.cur_uW / 1000) / 1000;

}
Exemple #7
0
int main(void){
    int i;                                 //Program to read IR sensor values
    printf("IR Sensor Values \n");
    printf("IR1  IR2  IR3  IR4  IR5\n");	
    if(init()==-1)
        exit(1);
    while(1){
	read_sensor();
        printf("\n");
	for(i=0;i<5;i++){                    // To convert the 10 bit analog reading of each sensor to decimal and store it in read_val[]
                printf("%d ",read_val[i]);  // Values less than 100 - White, Values greater than 800- Black, Value -1 -Error

        }
	//sleep_ms(500); // Uncomment the sleep_ms() if the values run fast on the screen 
    }
    return 0;
}
Exemple #8
0
/* Funktionen genomför en PID-reglering */
void pid(void *p)
{
	// Initiering av variabler för PID-reglering.
	float P = 0.0;
	float I = 0.0;
	float D = 0.0;
	
	for(;;) 
	{
		// Säkerställer jämn samplingstid.
		portTickType xLastWakeTime;
		const portTickType xTimeIncrement = SAMPLING_TIME;
			
			
		/* PID-REGLERING */
			
		// Avstånd läses av och beräknas.
		read_sensor();		// Avläser nytt sensorvärde
		set_global_d();		// Beräknar nytt avståndsvärde
			
		// Variabler till regleringen beräknas.
		eDif = global_e;						// Förbereder beräkningen av skillnaden mellan föregående och nuvarande felvärde.
		global_e = global_Bv - global_d;		// Beräknar felvärdet.	
		eDif = global_e - eDif;					// Beräknar skillnaden mellan föregående och nuvarande felvärde.
			
		// Motverkar skenande I-del.
		if(global_u <= 1000.0 && global_u >= 0)
		{
			eSum = eSum + global_e;				// Beräknar summan av samtliga felvärden.
			I = (global_dT/global_Ti) * eSum;	// Beräknar I-delen.
		}
			
		P = global_e;							// Beräknar P-delen.
		D = (global_Td/global_dT) * eDif;		// Beräknar D-delen.
			
		// Styrsignalen beräknas (PID-reglering).
		global_u = global_Kp * (P+I+D);
			
		// Styrsignalen skickas som PWM-signal.
		update_pwm((uint32_t)global_u);
		
		// Paus som motsvarar samplingstiden.
		vTaskDelayUntil(&xLastWakeTime, xTimeIncrement);
	}
}
Exemple #9
0
int read_and_send_sensor_event()
{
	char buf[100];
	read_sensor(buf, 100);
    printf("sensor data: %s\n", buf);

    Yodiwo_Plegma_PortEvent_t event;
    Array_Yodiwo_Plegma_PortEvent_t array_events;

    event.PortKey = sensor->Ports.elems[0].PortKey;
    event.State = buf;
    event.RevNum = 1;
    event.Timestamp = 0;

    array_events.num = 1;
    array_events.elems = &event;

    return portevents(&array_events);
}
Exemple #10
0
void WaterLevel::init(void)
{
	// read water level n amount of times, with 
	int i;
	for (i = 0; i < WL_NUMBER_OF_READINGS; i++)
	{
		water_level_reading[i] = read_sensor();
		delay(WL_WATER_LEVEL_DELAY);
	}	
	
	// sum the n number of readings
	float sum = 0;
	for (i = 0; i < WL_NUMBER_OF_READINGS; i++)
		sum = sum + water_level_reading[i];
		
	// find the average reading and init buffer with this
	float avg;
	avg = (sum+1) / WL_NUMBER_OF_READINGS;
	for (i = 0; i < WL_NUMBER_OF_READINGS; i++)
		water_level_reading[i] = avg;	
}
void ioi_handler(QueuePacket fifo_packet){
	set_debug(DEBUG1, true);

	unsigned int returnData[4]; // Data to send back to controller on FIFO

	switch (fifo_packet.command){
		case (PLANT): // SPI
			// SPI Filter
			if(!plant_filter(&fifo_packet)) return; // ERROR HANDLING HERE FOR FILTERING SPI
			if(!fifo_packet.bytes)	return;	// ERROR HANDLING IF TRANSFER BYTES IS 0

			// only accept voltage writes or sensor reads
			if( (fifo_packet.slave == SS_DAC) && (fifo_packet.operation == WRITE) && (fifo_packet.bytes == BITS_16)
					&& ((fifo_packet.data[0] & DAC_CONFIG_BITS) == DAC_CONFIG_BITS) ) // write voltage
				write_voltage(fifo_packet.data[0]);
			else if( (fifo_packet.slave == SS_ENCODER_S || fifo_packet.slave == SS_ENCODER_P) && (fifo_packet.operation == READ)
					&& (fifo_packet.bytes ==  BITS_32) && (fifo_packet.data[0]  == (READ_CNTR << 24))) // read encoder
				returnData[0] = read_sensor(fifo_packet.slave, fifo_packet.data[0]);
			break;
		case (SET_POINT): // SET_POINT
			returnData[0] = (unsigned int)get_set_point();
			break;
		case (STATE_VECTOR): // STATE_INFORMATION
			get_state_vector(returnData);
			break;
		default:
			break;
	}

	// Send the return data back to controller; maximum of 4 bytes returned
	enqueue(returnData, fifo_packet.operation > 4 ? 4 : fifo_packet.operation);

	//assert_trigger(check_wdt()); // backup was not working with this; initially had it to detect livelock attack

	set_debug(DEBUG1, false);
}
int main(void)
{



	//-------------------------USART INTERRUPT REGISTRATION.------------//
	// Set Clock: Oscillator needs to initialized once: First
		 pcl_switch_to_osc(PCL_OSC0, FOSC0, OSC0_STARTUP);

		 // --------------		USART INIT		-----------------------------------------------
		 static const gpio_map_t USART_GPIO_MAP =
		  {
			{AVR32_USART0_RXD_0_0_PIN, AVR32_USART0_RXD_0_0_FUNCTION},
			{AVR32_USART0_TXD_0_0_PIN, AVR32_USART0_TXD_0_0_FUNCTION}
		  };

		  // USART options.
		  static const usart_options_t USART_OPTIONS =
		  {
			.baudrate     = USART_BAUDRATE,
			.charlength   = 8,
			.paritytype   = USART_NO_PARITY,
			.stopbits     = USART_1_STOPBIT,
			.channelmode  = USART_NORMAL_CHMODE
		  };

		// Assign GPIO to USART
		gpio_enable_module(USART_GPIO_MAP, sizeof(USART_GPIO_MAP) / sizeof(USART_GPIO_MAP[0]));

		// Init USART
		usart_init_rs232(USART_0, &USART_OPTIONS, FOSC0);

		Disable_global_interrupt();
		INTC_init_interrupts();			// Init Interrupt Table: Once at first

		// Register USART Interrupt (hinzufügen)
		INTC_register_interrupt(&usart_int_handler, AVR32_USART0_IRQ, AVR32_INTC_INT0);

		USART_0->ier = AVR32_USART_IER_RXRDY_MASK; // Activate ISR on RX Line
		Enable_global_interrupt();
	// -----------------------------------------------------------------------------------

		// --------------------------		Display INIT		----------------------------------
			// Map SPI Pins
			static const gpio_map_t DIP204_SPI_GPIO_MAP =
			  {
				{DIP204_SPI_SCK_PIN,  DIP204_SPI_SCK_FUNCTION },  // SPI Clock.
				{DIP204_SPI_MISO_PIN, DIP204_SPI_MISO_FUNCTION},  // MISO.
				{DIP204_SPI_MOSI_PIN, DIP204_SPI_MOSI_FUNCTION},  // MOSI.
				{DIP204_SPI_NPCS_PIN, DIP204_SPI_NPCS_FUNCTION}   // Chip Select NPCS.
			  };

			// add the spi options driver structure for the LCD DIP204
			  spi_options_t spiOptions =
			  {
				.reg          = DIP204_SPI_NPCS,
				.baudrate     = 1000000,
				.bits         = 8,
				.spck_delay   = 0,
				.trans_delay  = 0,
				.stay_act     = 1,
				.spi_mode     = 0,
				.modfdis      = 1
			  };


			// SPI Inits: Assign I/Os to SPI
			gpio_enable_module(DIP204_SPI_GPIO_MAP,
			                     sizeof(DIP204_SPI_GPIO_MAP) / sizeof(DIP204_SPI_GPIO_MAP[0]));

			// Initialize as master
			spi_initMaster(DIP204_SPI, &spiOptions);

			// Set selection mode: variable_ps, pcs_decode, delay
			spi_selectionMode(DIP204_SPI, 0, 0, 0);

			// Enable SPI
			spi_enable(DIP204_SPI);

			// setup chip registers
			spi_setupChipReg(DIP204_SPI, &spiOptions, FOSC0);

			// initialize delay driver: Muss vor dip204_init() ausgeführt werden
			delay_init( FOSC0 );

			// initialize LCD
			dip204_init(backlight_PWM, TRUE);
			// ---------------------------------------------------------------------------------------

			// -----------------			Timer Counter Init		---------------------------------
				// Timer Configs:  Options for waveform generation.
				static const tc_waveform_opt_t WAVEFORM_OPT =
				{
				.channel  = TC_CHANNEL,                        // Channel selection.

				.bswtrg   = TC_EVT_EFFECT_NOOP,                // Software trigger effect on TIOB.
				.beevt    = TC_EVT_EFFECT_NOOP,                // External event effect on TIOB.
				.bcpc     = TC_EVT_EFFECT_NOOP,                // RC compare effect on TIOB.
				.bcpb     = TC_EVT_EFFECT_NOOP,                // RB compare effect on TIOB.

				.aswtrg   = TC_EVT_EFFECT_NOOP,                // Software trigger effect on TIOA.
				.aeevt    = TC_EVT_EFFECT_NOOP,                // External event effect on TIOA.
				.acpc     = TC_EVT_EFFECT_NOOP,                // RC compare effect on TIOA: toggle.
				.acpa     = TC_EVT_EFFECT_NOOP,                // RA compare effect on TIOA: toggle

				.wavsel   = TC_WAVEFORM_SEL_UP_MODE_RC_TRIGGER,//  Count till RC and reset (S. 649): Waveform selection
				.enetrg   = FALSE,                             // External event trigger enable.
				.eevt     = 0,                                 // External event selection.
				.eevtedg  = TC_SEL_NO_EDGE,                    // External event edge selection.
				.cpcdis   = FALSE,                             // Counter disable when RC compare.
				.cpcstop  = FALSE,                             // Counter clock stopped with RC compare.

				.burst    = FALSE,                             // Burst signal selection.
				.clki     = FALSE,                             // Clock inversion.
				.tcclks   = TC_CLOCK_SOURCE_TC3                // Internal source clock 3, connected to fPBA / 8.
				};


				// TC Interrupt Enable Register
				static const tc_interrupt_t TC_INTERRUPT =
				{ .etrgs = 0, .ldrbs = 0, .ldras = 0, .cpcs  = 1, .cpbs  = 0, .cpas  = 0, .lovrs = 0, .covfs = 0
				};
				// 0 = No Effect | 1 = Enable ( CPCS = 1 enables the RC Compare Interrupt )

				// *****************   Timer Setup ***********************************************
				// Initialize the timer/counter.
				tc_init_waveform(tc, &WAVEFORM_OPT);         // Initialize the timer/counter waveform.

				// Set the compare triggers.
				tc_write_rc(tc, TC_CHANNEL, RC); // Set RC value.

				tc_configure_interrupts(tc, TC_CHANNEL, &TC_INTERRUPT);

				// Start the timer/counter.
				tc_start(tc, TC_CHANNEL);                    // And start the timer/counter.
				// *******************************************************************************

				Disable_global_interrupt();
				// Register TC Interrupt
				INTC_register_interrupt(&tc_irq, AVR32_TC_IRQ0, AVR32_INTC_INT3);

				Enable_global_interrupt();
				// ---------------------------------------------------------------------------------------



				imu_init();

//-------------------------------TWI R/W ---------------------------------------------------



  sensorDaten imu_data = {0};
  char disp1[30], disp2[30], disp3[30], disp4[30];
  short GX,GY,GZ, AX, AY, AZ;			//shifted comlete Data
  RPY currMoveRPY;
  Quaternion currQuat;
  currQuat.q0 = 1.0;
  currQuat.q1 = 0;
  currQuat.q2 = 0;
  currQuat.q3 = 0;
  Quaternion deltaQuat;
  RotMatrix rot = {0};
  RPY reconverted;


  calibrate_all(&imu_data);


  while(1){
	  if(exe){
		  exe = false;
		  read_sensor(&imu_data);




		  AX = imu_data.acc_x + imu_data.acc_x_bias;
		  AY = imu_data.acc_y + imu_data.acc_y_bias;
		  AZ = imu_data.acc_z + imu_data.acc_z_bias;

		  GX = imu_data.gyro_x + imu_data.gyro_x_bias;
		  GY = imu_data.gyro_y + imu_data.gyro_y_bias;
		  GZ = imu_data.gyro_z + imu_data.gyro_z_bias;




		  //convert to 1G
		  float ax = (float)AX * (-4.0);
		  float ay = (float)AY * (-4.0); //wegen 2^11= 2048, /2 = 1024 entspricht 4G -> 1G = (1024/4)
		  float az = (float)AZ * (-4.0);


		  //convert to 1°/s
		  gx = ((float)GX/ 14.375); // in °/s
		  gy = ((float)GY/ 14.375);
		  gz = ((float)GZ/ 14.375);

		  //Integration over time
		  dGx = (gx*0.03);
		  dGy = (gy*0.03);
		  dGz = (gz*0.03);

		  currMoveRPY.pitch = -dGx;
		  currMoveRPY.roll = dGy;
		  currMoveRPY.yaw = dGz;


		  //aufaddieren auf den aktuellen Winkel IN GRAD
			gxDeg += dGx;
			gyDeg += dGy;
			gzDeg += dGz;


			//RPY in Quaternion umwandeln
			RPYtoQuat(&deltaQuat, &currMoveRPY);


			//normieren
			normQuat(&deltaQuat);


			//aufmultiplizeiren
			quatMultiplication(&deltaQuat, &currQuat, &currQuat);



			//nochmal normieren
			normQuat(&currQuat);

			//rücktransformation nicht nötig!!


			char send[80];
			sprintf(send,"$,%f,%f,%f,%f,#", currQuat.q0, currQuat.q1, currQuat.q2, currQuat.q3);
		 	usart_write_line(USART_0,send);



		   sprintf(disp1,"q0:%.3f, GX:%3.0f",currQuat.q0,gxDeg);
		   sprintf(disp2,"q1:%.3f, GY:%3.0f",currQuat.q1, gyDeg);
		   sprintf(disp3,"q2:%.3f, GZ:%3.0f",currQuat.q2, gzDeg);
		   sprintf(disp4,"q3:%.3f",currQuat.q3);



		   dip204_clear_display();

		   dip204_set_cursor_position(1,1);
		   dip204_write_string(disp1);
		   dip204_set_cursor_position(1,2);
		   dip204_write_string(disp2);
		   dip204_set_cursor_position(1,3);
		   dip204_write_string(disp3);
		   dip204_set_cursor_position(1,4);
		   dip204_write_string(disp4);











			//sprintf(data,"TEST:%s",high);
		   //print_dbg(data);
	  }

  }
}
void pulsing_pressure_mode()
{
	valve_1 = 0;
	valve_2 = 0;
	valve_3 = 0;
	valve_4 = 0;
	valve_5 = 0;
	valve_6 = 0;
	valve_7 = 0;
	pump = 0;
	air_in = 0;
	air_out = 0;
	read_sensor();
	wait(0.5);

	while (1)
	{
		/***********************************************************************
		PUMP UP THE CUSHION
		***********************************************************************/
		while (chamber1 <= chamb_1_expect)
		{
			valve_1 = 1;
			pump = 1;
			air_in = 1;
			air_out = 0;
			read_sensor();
			wait(0.5);
		}
		valve_1 = 0;

		while (chamber2 <= chamb_2_expect)
		{
			valve_2 = 1;
			pump = 1;
			air_in = 1;
			air_out = 0;
			read_sensor();
			wait(0.5);
		}
		valve_2 = 0;

		while (chamber3 <= chamb_3_expect)
		{
			valve_3 = 1;
			pump = 1;
			air_in = 1;
			air_out = 0;
			read_sensor();
			wait(0.5);
		}
		valve_3 = 0;

		while (chamber4 <= chamb_4_expect)
		{
			valve_4 = 1;
			pump = 1;
			air_in = 1;
			air_out = 0;
			read_sensor();
			wait(0.5);
		}
		valve_4 = 0;

		while (chamber5 <= chamb_5_expect)
		{
			valve_5 = 1;
			pump = 1;
			air_in = 1;
			air_out = 0;
			read_sensor();
			wait(0.5);
		}
		valve_5 = 0;

		while (chamber6 <= chamb_6_expect)
		{
			valve_6 = 1;
			pump = 1;
			air_in = 1;
			air_out = 0;
			read_sensor();
			wait(0.5);
		}
		valve_6 = 0;

		while (chamber7 <= chamb_7_expect)
		{
			valve_7 = 1;
			pump = 1;
			air_in = 1;
			air_out = 0;
			read_sensor();
			wait(0.5);
		}
		valve_7 = 0;
		pump = 0;
		air_in = 0;

		/***********************************************************************
		Take one chamber - let air out - pump it up again
		***********************************************************************/
		while (chamber1 >= 5)
		{
			valve_1 = 1;
			air_out = 1;
			read_sensor();
			wait(0.5);
		}
		air_out = 0;

		while (chamber1 <= chamb_1_expect)
		{
			air_in = 1;
				pump = 1;
			read_sensor();
			wait(0.5);
		}
		pump = 0;
		valve_1 = 0;
		air_in = 0;

		while (chamber2 >= 5)
		{
			valve_2 = 1;
			air_out = 1;
			read_sensor();
			wait(0.5);
		}
		air_out = 0;

		while (chamber2 <= chamb_2_expect)
		{
			air_in = 1;
			pump = 1;
			read_sensor();
			wait(0.5);
		}
		pump = 0;
		valve_2 = 0;
		air_in = 0;


		while (chamber3 >= 5)
		{
			valve_3 = 1;
			air_out = 1;
			read_sensor();
			wait(0.5);
		}
		air_out = 0;

		while (chamber3 <= chamb_3_expect)
		{
			air_in = 1;
			pump = 1;
			read_sensor();
			wait(0.5);
		}
		pump = 0;
		valve_3 = 0;
		air_in = 0;

		while (chamber4 >= 5)
		{
			valve_4 = 1;
			air_out = 1;
			read_sensor();
			wait(0.5);
		}
		air_out = 0;

		while (chamber4 <= chamb_4_expect)
		{
			air_in = 1;
			pump = 1;
			read_sensor();
			wait(0.5);
		}
		pump = 0;
		valve_4 = 0;
		air_in = 0;

		while (chamber5 >= 5)
		{
			valve_5 = 1;
			air_out = 1;
			read_sensor();
			wait(0.5);
		}
		air_out = 0;

		while (chamber5 <= chamb_5_expect)
		{
			air_in = 1;
			pump = 1;
			read_sensor();
			wait(0.5);
		}
		pump = 0;
		valve_5 = 0;
		air_in = 0;

		while (chamber6 >= 5)
		{
			valve_6 = 1;
			air_out = 1;
			read_sensor();
			wait(0.5);
		}
		air_out = 0;

		while (chamber6 <= chamb_6_expect)
		{
			air_in = 1;
			pump = 1;
			read_sensor();
			wait(0.5);
		}
		pump = 0;
		valve_6 = 0;
		air_in = 0;

		while (chamber7 >= 5)
		{
			valve_7 = 1;
			air_out = 1;
			read_sensor();
			wait(0.5);
		}
		air_out = 0;

		while (chamber7 <= chamb_7_expect)
		{
			air_in = 1;
			pump = 1;
			read_sensor();
			wait(0.5);
		}
		pump = 0;
		valve_7 = 0;
		air_in = 0;
	}
}
void humidity_thread(void* arg) {
  read_sensor(sensirionSht11_humidity_read, &(sensor_data->hum));
}
Exemple #15
0
int main(void)
{
  //*******************************Timer setup**********************************
  WDTCTL = WDTPW + WDTHOLD;            // Stop Watchdog Timer

  P1SEL = 0;
  P2SEL = 0;

  P1IE = 0;
  P1IFG = 0;
  P2IFG = 0;

  DRIVE_ALL_PINS // set pin directions correctly and outputs to low.

  // Check power on bootup, decide to receive or sleep.
  if(!is_power_good())
    sleep();

  RECEIVE_CLOCK;

#if DEBUG_PINS_ENABLED
#if USE_2618
  DEBUG_PIN5_LOW;
#endif
#endif

#if ENABLE_SLOTS
  // setup int epc
  epc = ackReply[2]<<8;
  epc |= ackReply[3];

  // calculate RN16_1 table
  for (Q = 0; Q < 16; Q++)
  {
    rn16 = epc^Q;
    lfsr();

    if (Q > 8)
    {
      RN16[(Q<<1)-9] = __swap_bytes(rn16);
      RN16[(Q<<1)-8] = rn16;
    }
    else
    {
      RN16[Q] = rn16;
    }
  }
#endif

  TACTL = 0;

  asm("MOV #0000h, R9");
  // dest = destorig;

#if READ_SENSOR
  init_sensor();
#endif

#if !(ENABLE_SLOTS)
  queryReplyCRC = crc16_ccitt(&queryReply[0],2);
  queryReply[3] = (unsigned char)queryReplyCRC;
  queryReply[2] = (unsigned char)__swap_bytes(queryReplyCRC);
#endif

#if SENSOR_DATA_IN_ID
  // this branch is for sensor data in the id
  ackReply[2] = SENSOR_DATA_TYPE_ID;
  state = STATE_READ_SENSOR;
  timeToSample++;
#else
  ackReplyCRC = crc16_ccitt(&ackReply[0], 14);
  ackReply[15] = (unsigned char)ackReplyCRC;
  ackReply[14] = (unsigned char)__swap_bytes(ackReplyCRC);
#endif

#if ENABLE_SESSIONS
  initialize_sessions();
#endif

  state = STATE_READY;

  setup_to_receive();

  while (1)
  {

    // TIMEOUT!  reset timer
    if (TAR > 0x256 || delimiterNotFound)   // was 0x1000
    {
      if(!is_power_good()) {
        sleep();
      }

#if SENSOR_DATA_IN_ID
    // this branch is for sensor data in the id
      if ( timeToSample++ == 10 ) {
        state = STATE_READ_SENSOR;
        timeToSample = 0;
      }
#elif SENSOR_DATA_IN_READ_COMMAND
      if ( timeToSample++ == 10 ) {
        state = STATE_READ_SENSOR;
        timeToSample = 0;
      }
#else
#if !(ENABLE_READS)
    if(!is_power_good())
        sleep();
#endif
    inInventoryRound = 0;
    state = STATE_READY;

#endif

#if ENABLE_SESSIONS
    handle_session_timeout();
#endif

#if ENABLE_SLOTS
    if (shift < 4)
        shift += 1;
    else
        shift = 0;
#endif

      setup_to_receive();
    }

    switch (state)
    {
      case STATE_READY:
      {
        inInventoryRound = 0;
        //////////////////////////////////////////////////////////////////////
        // process the QUERY command
        //////////////////////////////////////////////////////////////////////
        if ( bits == NUM_QUERY_BITS  && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
        {
          handle_query(STATE_REPLY);
          setup_to_receive();
        }
        //////////////////////////////////////////////////////////////////////
        // process the SELECT command
        //////////////////////////////////////////////////////////////////////
        // @ short distance has slight impact on performance
        else if ( bits >= 44  && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
        {
          handle_select(STATE_READY);
          delimiterNotFound = 1;
        } // select command
        //////////////////////////////////////////////////////////////////////
        // got >= 22 bits, and it's not the beginning of a select. just reset.
        //////////////////////////////////////////////////////////////////////
        else if ( bits >= MAX_NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) != 0xA0 ) )
        {
          do_nothing();
          state = STATE_READY;
          delimiterNotFound = 1;
        }
        break;
      }
      case STATE_ARBITRATE:
      {
        //////////////////////////////////////////////////////////////////////
        // process the QUERY command
        //////////////////////////////////////////////////////////////////////
        if ( bits == NUM_QUERY_BITS  && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
        {
          handle_query(STATE_REPLY);
          setup_to_receive();
        }
        //////////////////////////////////////////////////////////////////////
        // got >= 22 bits, and it's not the beginning of a select. just reset.
        //////////////////////////////////////////////////////////////////////
        //else if ( bits >= NUM_QUERY_BITS )
        else if ( bits >= MAX_NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) != 0xA0 ) )
        {
          do_nothing();
          state = STATE_READY;
          delimiterNotFound = 1;
        }
        // this state handles query, queryrep, queryadjust, and select commands.
        //////////////////////////////////////////////////////////////////////
        // process the QUERYREP command
        //////////////////////////////////////////////////////////////////////
        else if ( bits == NUM_QUERYREP_BITS && ( ( cmd[0] & 0x06 ) == 0x00 ) )
        {
          handle_queryrep(STATE_REPLY);
          delimiterNotFound = 1;
        } // queryrep command
        //////////////////////////////////////////////////////////////////////
        // process the QUERYADJUST command
        //////////////////////////////////////////////////////////////////////
        else if ( bits == NUM_QUERYADJ_BITS  && ( ( cmd[0] & 0xF8 ) == 0x48 ) )
        {
          handle_queryadjust(STATE_REPLY);
          setup_to_receive();
        } // queryadjust command
        //////////////////////////////////////////////////////////////////////
        // process the SELECT command
        //////////////////////////////////////////////////////////////////////
        // @ short distance has slight impact on performance
        else if ( bits >= 44  && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
        {
          handle_select(STATE_READY);
          delimiterNotFound = 1;
        } // select command

      break;
      }

      case STATE_REPLY:
      {
        // this state handles query, query adjust, ack, and select commands
        ///////////////////////////////////////////////////////////////////////
        // process the ACK command
        ///////////////////////////////////////////////////////////////////////
        if ( bits == NUM_ACK_BITS  && ( ( cmd[0] & 0xC0 ) == 0x40 ) )
        {
#if ENABLE_READS
          handle_ack(STATE_ACKNOWLEDGED);
          setup_to_receive();
#elif SENSOR_DATA_IN_ID
          handle_ack(STATE_ACKNOWLEDGED);
          delimiterNotFound = 1; // reset
#else
          // this branch for hardcoded query/acks
          handle_ack(STATE_ACKNOWLEDGED);
          //delimiterNotFound = 1; // reset
          setup_to_receive();
#endif
        }
        //////////////////////////////////////////////////////////////////////
        // process the QUERY command
        //////////////////////////////////////////////////////////////////////
        if ( bits == NUM_QUERY_BITS  && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
        {
          // i'm supposed to stay in state_reply when I get this, but if I'm
          // running close to 1.8v then I really need to reset and get in the
          // sleep, which puts me back into state_arbitrate. this is complete
          // a violation of the protocol, but it sure does make everything
          // work better. - polly 8/9/2008
          handle_query(STATE_REPLY);
          setup_to_receive();
        }
        //////////////////////////////////////////////////////////////////////
        // process the QUERYREP command
        //////////////////////////////////////////////////////////////////////
        else if ( bits == NUM_QUERYREP_BITS && ( ( cmd[0] & 0x06 ) == 0x00 ) )
        {
			do_nothing();
			state = STATE_ARBITRATE;
			setup_to_receive();
        } // queryrep command
        //////////////////////////////////////////////////////////////////////
        // process the QUERYADJUST command
        //////////////////////////////////////////////////////////////////////
        else if ( bits == NUM_QUERYADJ_BITS  && ( ( cmd[0] & 0xF8 ) == 0x48 ) )
        {
          handle_queryadjust(STATE_REPLY);
          delimiterNotFound = 1;
        } // queryadjust command
        //////////////////////////////////////////////////////////////////////
        // process the SELECT command
        //////////////////////////////////////////////////////////////////////
        else if ( bits >= 44  && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
        {
          handle_select(STATE_READY);
          delimiterNotFound = 1;
        } // select command
        else if ( bits >= MAX_NUM_QUERY_BITS && ( ( cmd[0] & 0xF0 ) != 0xA0 ) &&
                ( ( cmd[0] & 0xF0 ) != 0x80 ) )
        {
          do_nothing();
          state = STATE_READY;
          delimiterNotFound = 1;
        }
        break;
      }
      case STATE_ACKNOWLEDGED:
      {
        // responds to query, ack, request_rn cmds
        // takes action on queryrep, queryadjust, and select cmds
        /////////////////////////////////////////////////////////////////////
        // process the REQUEST_RN command
        //////////////////////////////////////////////////////////////////////
        if ( bits >= NUM_REQRN_BITS && ( cmd[0] == 0xC1 ) )
        {
#if 1
          handle_request_rn(STATE_OPEN);
          setup_to_receive();
#else
          handle_request_rn(STATE_READY);
          delimiterNotFound = 1;
#endif
        }

#if 1
        //////////////////////////////////////////////////////////////////////
        // process the QUERY command
        //////////////////////////////////////////////////////////////////////
        if ( bits == NUM_QUERY_BITS  && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
        {
          handle_query(STATE_REPLY);
          delimiterNotFound = 1;
        }
        ///////////////////////////////////////////////////////////////////////
        // process the ACK command
        ///////////////////////////////////////////////////////////////////////
        // this code doesn't seem to get exercised in the real world. if i ever
        // ran into a reader that generated an ack in an acknowledged state,
        // this code might need some work.
        //else if ( bits == 20  && ( ( cmd[0] & 0xC0 ) == 0x40 ) )
        else if ( bits == NUM_ACK_BITS  && ( ( cmd[0] & 0xC0 ) == 0x40 ) )
        {
          handle_ack(STATE_ACKNOWLEDGED);
          setup_to_receive();
        }
        //////////////////////////////////////////////////////////////////////
        // process the QUERYREP command
        //////////////////////////////////////////////////////////////////////
        else if ( bits == NUM_QUERYREP_BITS && ( ( cmd[0] & 0x06 ) == 0x00 ) )
        {
          // in the acknowledged state, rfid chips don't respond to queryrep
          // commands
          do_nothing();
          state = STATE_READY;
          delimiterNotFound = 1;
        } // queryrep command

        //////////////////////////////////////////////////////////////////////
        // process the QUERYADJUST command
        //////////////////////////////////////////////////////////////////////
        else if ( bits == NUM_QUERYADJ_BITS  && ( ( cmd[0] & 0xF8 ) == 0x48 ) )
        {
          do_nothing();
          state = STATE_READY;
          delimiterNotFound = 1;
        } // queryadjust command
        //////////////////////////////////////////////////////////////////////
        // process the SELECT command
        //////////////////////////////////////////////////////////////////////
        else if ( bits >= 44  && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
        {
          handle_select(STATE_READY);
          delimiterNotFound = 1;
        } // select command
        //////////////////////////////////////////////////////////////////////
        // process the NAK command
        //////////////////////////////////////////////////////////////////////
        else if ( bits >= 10 && ( cmd[0] == 0xC0 ) )
        {
          do_nothing();
          state = STATE_ARBITRATE;
          delimiterNotFound = 1;
        }
        //////////////////////////////////////////////////////////////////////
        // process the READ command
        //////////////////////////////////////////////////////////////////////
        // warning: won't work for read addrs > 127d
        if ( bits == NUM_READ_BITS && ( cmd[0] == 0xC2 ) )
        {
          handle_read(STATE_ARBITRATE);
          state = STATE_ARBITRATE;
          delimiterNotFound = 1 ;
        }
        // FIXME: need write, kill, lock, blockwrite, blockerase
        //////////////////////////////////////////////////////////////////////
        // process the ACCESS command
        //////////////////////////////////////////////////////////////////////
        if ( bits >= 56  && ( cmd[0] == 0xC6 ) )
        {
          do_nothing();
          state = STATE_ARBITRATE;
          delimiterNotFound = 1 ;
        }
#endif
        else if ( bits >= MAX_NUM_READ_BITS )
        {
          state = STATE_ARBITRATE;
          delimiterNotFound = 1 ;
        }

#if 0
        // kills performance ...
        else if ( bits >= 44 )
        {
          do_nothing();
          state = STATE_ARBITRATE;
          delimiterNotFound = 1;
        }
#endif
        break;
      }
      case STATE_OPEN:
      {
        // responds to query, ack, req_rn, read, write, kill, access,
        // blockwrite, and blockerase cmds
        // processes queryrep, queryadjust, select cmds
        //////////////////////////////////////////////////////////////////////
        // process the READ command
        //////////////////////////////////////////////////////////////////////
        // warning: won't work for read addrs > 127d
        if ( bits == NUM_READ_BITS  && ( cmd[0] == 0xC2 ) )
        {
          handle_read(STATE_OPEN);
          // note: setup_to_receive() et al handled in handle_read
        }
        //////////////////////////////////////////////////////////////////////
        // process the REQUEST_RN command
        //////////////////////////////////////////////////////////////////////
        else if ( bits >= NUM_REQRN_BITS  && ( cmd[0] == 0xC1 ) )
        {
          handle_request_rn(STATE_OPEN);
          setup_to_receive();
         }
        //////////////////////////////////////////////////////////////////////
        // process the QUERY command
        //////////////////////////////////////////////////////////////////////
        if ( bits == NUM_QUERY_BITS  && ( ( cmd[0] & 0xF0 ) == 0x80 ) )
        {
          handle_query(STATE_REPLY);
          delimiterNotFound = 1;
        }
        //////////////////////////////////////////////////////////////////////
        // process the QUERYREP command
        //////////////////////////////////////////////////////////////////////
        else if ( bits == NUM_QUERYREP_BITS && ( ( cmd[0] & 0x06 ) == 0x00 ) )
        {
          do_nothing();
          state = STATE_READY;
          setup_to_receive();
        } // queryrep command
        //////////////////////////////////////////////////////////////////////
        // process the QUERYADJUST command
        //////////////////////////////////////////////////////////////////////
          else if ( bits == 9  && ( ( cmd[0] & 0xF8 ) == 0x48 ) )
        {
          do_nothing();
          state = STATE_READY;
          delimiterNotFound = 1;
        } // queryadjust command
        ///////////////////////////////////////////////////////////////////////
        // process the ACK command
        ///////////////////////////////////////////////////////////////////////
        else if ( bits == NUM_ACK_BITS  && ( ( cmd[0] & 0xC0 ) == 0x40 ) )
        {
          handle_ack(STATE_OPEN);
          delimiterNotFound = 1;
        }
        //////////////////////////////////////////////////////////////////////
        // process the SELECT command
        //////////////////////////////////////////////////////////////////////
        else if ( bits >= 44  && ( ( cmd[0] & 0xF0 ) == 0xA0 ) )
        {
          handle_select(STATE_READY);
          delimiterNotFound = 1;
        } // select command
        //////////////////////////////////////////////////////////////////////
        // process the NAK command
        //////////////////////////////////////////////////////////////////////
        else if ( bits >= 10 && ( cmd[0] == 0xC0 ) )
        {
          handle_nak(STATE_ARBITRATE);
          delimiterNotFound = 1;
        }

        break;
      }

    case STATE_READ_SENSOR:
      {
#if SENSOR_DATA_IN_READ_COMMAND
        read_sensor(&readReply[0]);
        // crc is computed in the read state
        RECEIVE_CLOCK;
        state = STATE_READY;
        delimiterNotFound = 1; // reset
#elif SENSOR_DATA_IN_ID
        read_sensor(&ackReply[3]);
        RECEIVE_CLOCK;
        ackReplyCRC = crc16_ccitt(&ackReply[0], 14);
        ackReply[15] = (unsigned char)ackReplyCRC;
        ackReply[14] = (unsigned char)__swap_bytes(ackReplyCRC);
        state = STATE_READY;
        delimiterNotFound = 1; // reset
#endif

        break;
      } // end case
    } // end switch

  } // while loop
}
//Main Function
int main()
{
	init_devices();
	init_encoders();
	lcd_set_4bit();
	lcd_init();
	int value=0;
	forward();
	velocity(130,130);
	lcd_print(2,1,130,3);
	lcd_print(2,5,130,3);
	lcd_print(2,9,pathindex,2);
	lcd_print(2,13,dirn,3);


	while(1)
	{	
		read_sensor();
		follow();
		
		if(isPlus())
		{	
			read_sensor();
			value = path[pathindex++];
			
			// Code inserted for calculation of actual location wrt initial starting point , 
			// It will consider direction also.

			if (value == F)
			{
				// Move the bot forward, for location only , No movement on ground.
				move_bot(FR);
			}
			else if (value == L)
			{
				// Move the bot left , for location only , No movement on ground.
				move_bot(LT);
			}
			else if (value == R)
			{
				// Move the bot right, for location only , No movement on ground.
				move_bot(RT);
			}
			else if (value == M)
			{
				// To stop the Bot and then break out
				stop();
				break;
			}
					
			orient(value);

/*			lcd_print(2,9,pathindex,2);
			lcd_print(2,13,dirn,3);
			lcd_print(1,13,turnL,1);
			lcd_print(1,15,turnR,1);
*/
			
		}
		
		if(turnL == 1)
		{/*
		lcd_print(1,13,turnL,1);
		forward_mm(20);
		stop();
		velocity(180,180);
		left_degrees(95);
		//_delay_ms(120);
		read_sensor();
		//	 while(Left_white_line <0x40)
		// {
		//	read_sensor();
		//	left();
		// }
		 stop();
	 	 forward();
		velocity(180,180);
		 turnL = 0;
		 */

		 back_mm(50);
		//stop();
		//velocity(130,130);
		stop();
		left_degrees(50);
		rotate_left_slowly();
	 	forward();
		velocity(130,130);
		turnL = 0;
		}
		
		if(turnR == 1)
		{
		/*
		lcd_print(1,15,turnR,1);
		forward_mm(20);
		stop();
		velocity(180,180);
		right_degrees(95);
		//_delay_ms(200);
		read_sensor();
		// while(Right_white_line <0x30)
		// {
		// read_sensor();
		// right();
		// }
		stop();
		forward();
		//follow();
		velocity(180,180);
		 turnR = 0;
		*/

		back_mm(50);
		//stop();
		//velocity(130,130);
		stop();
		right_degrees(50);
		rotate_right_slowly();
	 	forward();
		velocity(130,130);
		turnR = 0;
		}
	
	}


	// Three Beeps for Interval
	buzzer_on();
	_delay_ms(100);
	buzzer_off();
	_delay_ms(100);

	buzzer_on();
	_delay_ms(100);
	buzzer_off();
	_delay_ms(100);

	buzzer_on();
	_delay_ms(100);
	buzzer_off();
	_delay_ms(100);

	//code to head-back to starting position , i.e. Origin
	return_path_counter = reach_origin();
	
	forward();
	velocity(130,130);
	int counter = 0;
	int intermediate_value = 0;

	while(counter < return_path_counter)
	{	
		read_sensor();
		follow();
		
		if(isPlus())
		{	
			read_sensor();
			value = path_to_origin[counter];
			counter++;
			
			// Code inserted for calculation of actual location wrt initial starting point , 
			// It will consider direction also.

			if (intermediate_value == FR)
			{
				// Move the bot forward, for location only , No movement on ground.
				value = F;
			}
			else if (value == LT)
			{
				// Move the bot left , for location only , No movement on ground.
				value = L;
			}
			else if (value == RT)
			{
				// Move the bot right, for location only , No movement on ground.
				value = R;
			}
			else if (value == ST)
			{
				value = M;
				// specially inserted as break will not allow the bot to stop using "orient(value)".
				orient(value);
				break;
			}
					
			orient(value);
		
		}
		
		if(turnL == 1)
		{
		 back_mm(50);
		//stop();
		//velocity(130,130);
		stop();
		left_degrees(50);
		rotate_left_slowly();
	 	forward();
		velocity(130,130);
		turnL = 0;
		}
		
		if(turnR == 1)
		{
		back_mm(50);
		//stop();
		//velocity(130,130);
		stop();
		right_degrees(50);
		rotate_right_slowly();
	 	forward();
		velocity(130,130);
		turnR = 0;
		}
	
	}


	// Three beeps for Finish
	buzzer_on();
	_delay_ms(100);
	buzzer_off();
	_delay_ms(100);

	buzzer_on();
	_delay_ms(100);
	buzzer_off();
	_delay_ms(100);

	buzzer_on();
	_delay_ms(100);
	buzzer_off();
	_delay_ms(100);

}
void temperature_thread(void* arg) {
  read_sensor(sensirionSht11_temperature_read, &(sensor_data->temp));
}
void orient(int value)
{
	switch(value)
	{

		case F:
			
		
			while(isPlus())
			{
				read_sensor();
				follow();
			}

				break;

		case L:
				//turnLeft();
					turnL =1;
			if(dirn == N)
			{
			//	turnLeft();
				dirn = W;
			}
			else if (dirn == E)
			{
			//	turnBack();
				dirn = N;
			}
			else if (dirn == W)
			{
				dirn=S;
			}
			else if (dirn == S)
			{
			//	turnRight();
				dirn = E;
			}
				break;
		case R:
		//turnRight();
		turnR =1;
		if(dirn == N)
			{
				//turnRight();
				dirn = E;
			}
			else if (dirn == E)
			{
				dirn =S;
			}
			else if (dirn == W)
			{
				//turnBack();
				dirn = N;
			}
			else if (dirn == S)
			{
				//turnLeft();
				dirn = W;
			}
			
				break;
		case M:
		stop();
		
	}
	

}
void total_solar_thread(void* arg) {
  read_sensor(hamamatsuS10871_tsr_read, &(sensor_data->tsr));
}
int main() {
   // Local Declarations
   int i, p;
   unsigned char addr, reg;
   unsigned char data[9];// Read 9 bytes of data
   SENSOR_BUF accel;
   SENSOR_BUF magn;
   SENSOR_BUF gyro;
   struct timeval tvstart, tvstop;
   time_t current_time;
   char* c_time_string;
   FILE *f = fopen("matlabtestdata.txt", "w");
   if (f == NULL){
       printf("Error opening file!\n");
       exit(1);
   }
   
   // Initialize 9-axis Sensors
	DEBUG_PRINT( "running...\n" );
	system( "i2cset -y 1 0x1d 0x20 0x57" ); // ctrl1	
   system( "i2cset -y 1 0x1d 0x21 0x20" ); // ctrl2
	system( "i2cset -y 1 0x1d 0x24 0xF0" ); // ctrl5 Enables Temp and Sets magn data rate to 50Hz
	system( "i2cset -y 1 0x1d 0x25 0x20" ); // ctrl6
	system( "i2cset -y 1 0x1d 0x26 0x80" ); // ctrl7 Normal Mode
	DEBUG_PRINT( "accel_mag on\n" );
	system( "i2cset -y 1 0x6b 0x20 0x0f" ); // ctrl1
	DEBUG_PRINT( "gyro on\n" );
   
   // Write Header line to data collection file
   fprintf(f, "epochtime,accelX,accelY,accelZ,magnX,magnY,magnZ,gyroX,gyroY,gyroZ\n");
   
   
   // Start clock to check cpu_time used
   gettimeofday(&tvstart, NULL);
   // Collect Data p times
	for ( p = 0; p < 1000; p++ ){
      DEBUG_PRINT( "\n*********************** LOOP **********************\n" );
      
      // Get current time
      current_time = time(NULL);
      // Convert to local time format.
      c_time_string = ctime(&current_time);
      // Display time
      DEBUG_PRINT("Current date time is %s\n", c_time_string);
      DEBUG_PRINT("Current epoch time is %d\n", current_time);
      // Write time to data collection file
      fprintf(f, "%d,", current_time);
      
		
      // Get Acceleration XYZ Data from I2C port
      DEBUG_PRINT( "\n************** ACCELEROMETER DATA ************\n" );
		addr = 0x1d;// accel_mag
		reg = 0x28; // Device register to access accel data
		read_sensor( data , addr, reg ); //send device address and register to start read from (data= 8 bytes read)
		for (i = 0; i < 6; i++ ){
         // Copy xyz data from buffer to struct
         accel.byt[i] = data[i];
			//DEBUG_PRINT(":%X", accel.byt[i]);		
      }
      //DEBUG_PRINT(":\n");
      // convert from two's complement
      accel.data.x.val = ~accel.data.x.val + 1;
      accel.data.y.val = ~accel.data.y.val + 1;
      accel.data.z.val = ~accel.data.z.val + 1;
      // Display Data
      DEBUG_PRINT(" X val = %d\n Y val = %d\n Z val = %d \n",accel.data.x.val,accel.data.y.val, accel.data.z.val);
      // Write accel values to data collection file
      fprintf(f, "%d,%d,%d,", accel.data.x.val,accel.data.y.val, accel.data.z.val);
      //data[0] = 0;data[1] = 0;data[2] = 0;
      //data[3] = 0;data[4] = 0;data[5] = 0;
      
		
      // Get Magnetometer XYZ Data from I2C port
      DEBUG_PRINT("\n************** MAGNETOMETER DATA *************\n");
		addr = 0x1d;// accel_mag
		reg = 0x05; // Device register to access mag data
		read_sensor( data , addr, reg ); //send device address and register to start read from (data= 8 bytes read)	
		for ( i = 0; i < 6; i++ ){
         // Copy xyz data from buffer to struct
         magn.byt[i] = data[i+3];
         //DEBUG_PRINT(":%X", magn.byt[i]);	
		}
      //DEBUG_PRINT(":\n");
      // convert from two's complement
      magn.data.x.val = ~magn.data.x.val + 1;
      magn.data.y.val = ~magn.data.y.val + 1;
      magn.data.z.val = ~magn.data.z.val + 1;
      // Display Data
      DEBUG_PRINT(" X val = %d\n Y val = %d\n Z val = %d \n",magn.data.x.val,magn.data.y.val, magn.data.z.val);      
      // Write magn values to data collection file
      fprintf(f, "%d,%d,%d,", magn.data.x.val,magn.data.y.val, magn.data.z.val);
      //data[0] = 0;data[1] = 0;data[2] = 0;
      //data[3] = 0;data[4] = 0;data[5] = 0;		
      
		
      // Get Gyroscope XYZ Data from I2C port
      DEBUG_PRINT("\n************** GYROSCOPE DATA *************\n");
		addr = 0x6b;// gyro
		reg = 0x28; // Device register to access gyro data
		read_sensor( data , addr, reg ); //send device address and register to start read from (data= 8 bytes read)	
		for (i = 0; i < 6; i++ ){
         // Copy xyz data from buffer to struct
         gyro.byt[i] = data[i];
			//DEBUG_PRINT(":%X", gyro.byt[i]);		
      }      
      //DEBUG_PRINT(":\n");
      // convert from two's complement
      gyro.data.x.val = ~gyro.data.x.val + 1;
      gyro.data.y.val = ~gyro.data.y.val + 1;
      gyro.data.z.val = ~gyro.data.z.val + 1;
      // Display Data
      DEBUG_PRINT(" X val = %d\n Y val = %d\n Z val = %d \n",gyro.data.x.val,gyro.data.y.val, gyro.data.z.val); 
      // Write gyro values to data collection file
      fprintf(f, "%d,%d,%d\n", gyro.data.x.val,gyro.data.y.val, gyro.data.z.val);
      //data[0] = 0;data[1] = 0;data[2] = 0;
      //data[3] = 0;data[4] = 0;data[5] = 0;
      
      
	}// End for loop
   
   // Close file used for data collection
   fclose(f);
   // Stop Clock time and save to time spent variable
   gettimeofday(&tvstop, NULL);
   // Display time taken
   printf("Total time = %f seconds\n\n",
         (double) (tvstop.tv_usec - tvstart.tv_usec) / 1000000 +
         (double) (tvstop.tv_sec - tvstart.tv_sec));
   
}// End main()
void static_pressure_mode()
{

	while (1) {

		valve_1 = 0;
		valve_2 = 0;
		valve_3 = 0;
		valve_4 = 0;
		valve_5 = 0;
		valve_6 = 0;
		valve_7 = 0;
		pump = 0;
		air_in = 0;
		air_out = 0;
		read_sensor();
		wait(10);

		/***********************************************************************
		LOW PRESSURE ( Pump it up )
		***********************************************************************/
		if (chamber1 < (chamb_1_expect - 5))
		{
			while (chamber1 < chamb_1_expect + 5)
			{
				valve_1 = 1;
				pump = 1;
				air_in = 1;
				air_out = 0;
				read_sensor();
				wait(0.2);
			}
		}
		valve_1 = 0;

		if (chamber2 < (chamb_2_expect - 5))
		{
			while (chamber2 < chamb_2_expect + 5)
			{
				valve_2 = 1;
				pump = 1;
				air_in = 1;
				air_out = 0;
				read_sensor();
				wait(0.2);
			}
		}
		valve_2 = 0;

		if (chamber3 < (chamb_3_expect - 5))
		{
			while (chamber3 < chamb_3_expect + 5)
			{
				valve_3 = 1;
				pump = 1;
				air_in = 1;
				air_out = 0;
				read_sensor();
				wait(0.2);
			}
		}
		valve_3 = 0;

		if (chamber4 < (chamb_4_expect - 5))
		{
			while (chamber4 < chamb_4_expect + 5)
			{
				valve_4 = 1;
				pump = 1;
				air_in = 1;
				air_out = 0;
				read_sensor();
				wait(0.2);
			}
		}
		valve_4 = 0;

		if (chamber5 < (chamb_5_expect - 5))
		{
			while (chamber5 < chamb_5_expect + 5)
			{
				valve_5 = 1;
				pump = 1;
				air_in = 1;
				air_out = 0;
				read_sensor();
				wait(0.2);
			}
		}
		valve_5 = 0;

		if (chamber6 < (chamb_6_expect - 5))
		{
			while (chamber6 < chamb_6_expect + 5)
			{
				valve_6 = 1;
				pump = 1;
				air_in = 1;
				air_out = 0;
				read_sensor();
				wait(0.2);
			}
		}
		valve_6 = 0;

		if (chamber7 < (chamb_7_expect - 5))
		{
			while (chamber7 < chamb_7_expect + 5)
			{
				valve_7 = 1;
				pump = 1;
				air_in = 1;
				air_out = 0;
				read_sensor();
				wait(0.2);
			}
		}
		valve_7 = 0;

		/***********************************************************************
		TOO HIGH PRESSURE ( Let air out )
		***********************************************************************/
		if (chamber1 > (chamb_1_expect + 10))
		{
			while (chamber1 >= chamb_1_expect)
			{
				valve_1 = 1;
				pump = 0;
				air_in = 0;
				air_out = 1;
				read_sensor();
				wait(0.2);
			}
		}
		valve_1 = 0;

		if (chamber2 > (chamb_2_expect + 10))
		{
			while (chamber2 >= chamb_2_expect)
			{
				valve_2 = 1;
				pump = 0;
				air_in = 0;
				air_out = 1;
				read_sensor();
				wait(0.2);
			}
		}
		valve_2 = 0;

		if (chamber3 > (chamb_3_expect + 10))
		{
			while (chamber3 >= chamb_3_expect)
			{
				valve_3 = 1;
				pump = 0;
				air_in = 0;
				air_out = 1;
				read_sensor();
				wait(0.2);
			}
		}
		valve_3 = 0;

		if (chamber4 > (chamb_4_expect + 10))
		{
			while (chamber4 >= chamb_4_expect)
			{
				valve_4 = 1;
				pump = 0;
				air_in = 0;
				air_out = 1;
				read_sensor();
				wait(0.2);
			}
		}
		valve_4 = 0;

		if (chamber5 > (chamb_5_expect + 10))
		{
			while (chamber5 >= chamb_5_expect)
			{
				valve_5 = 1;
				pump = 0;
				air_in = 0;
				air_out = 1;
				read_sensor();
				wait(0.2);
			}
		}
		valve_5 = 0;

		if (chamber6 > (chamb_6_expect + 10))
		{
			while (chamber6 >= chamb_6_expect)
			{
				valve_6 = 1;
				pump = 0;
				air_in = 0;
				air_out = 1;
				read_sensor();
				wait(0.2);
			}
		}
		valve_6 = 0;

		if (chamber7 > (chamb_7_expect + 10))
		{
			while (chamber7 >= chamb_7_expect)
			{
				valve_7 = 1;
				pump = 0;
				air_in = 0;
				air_out = 1;
				read_sensor();
				wait(0.2);
			}
		}
		valve_7 = 0;
	}
}
Exemple #22
0
void getData(){
   // Local Declarations
   int i, p;
   unsigned char addr, reg;
   unsigned char data[9];// Read 9 bytes of data
   
   // Collect Data p times
//	for ( p = 0; p < 1; p++ ){
      DEBUG_PRINT( "\n*********************** GetData() **********************\n" );
/*
      // Get Acceleration XYZ Data from I2C port
      DEBUG_PRINT( "************** ACCELEROMETER DATA ************\n" );
		addr = 0x1d;// accel_mag
		reg = 0x28; // Device register to access accel data
		read_sensor( data , addr, reg ); //send device address and register to start read from (data= 8 bytes read)
		for (i = 0; i < 6; i++ ){
         // Copy xyz data from buffer to struct
         accel.byt[i] = data[i];
			//DEBUG_PRINT(":%X", accel.byt[i]);		
      }
      //DEBUG_PRINT(":\n");
      // convert from two's complement
      accel.data.x.val = ~accel.data.x.val + 1;
      accel.data.y.val = ~accel.data.y.val + 1;
      accel.data.z.val = ~accel.data.z.val + 1;
      // Display Data
      DEBUG_PRINT(" X val = %d\n Y val = %d\n Z val = %d \n",accel.data.x.val,accel.data.y.val, accel.data.z.val);
*/      
		
      // Get Magnetometer XYZ Data from I2C port
      DEBUG_PRINT("************** MAGNETOMETER DATA *************\n");
		addr = 0x1d;// accel_mag
		reg = 0x05; // Device register to access mag data
		read_sensor( data , addr, reg ); //send device address and register to start read from (data= 8 bytes read)	
		for ( i = 0; i < 6; i++ ){
         // Copy xyz data from buffer to struct
         magn.byt[i] = data[i+3];
         //DEBUG_PRINT(":%X", magn.byt[i]);	
		}
      //DEBUG_PRINT(":\n");
      // convert from two's complement
      magn.data.x.val = ~magn.data.x.val + 1;
      magn.data.y.val = ~magn.data.y.val + 1;
      magn.data.z.val = ~magn.data.z.val + 1;
      // Display Data
      DEBUG_PRINT(" X val = %d\n Y val = %d\n Z val = %d \n",magn.data.x.val,magn.data.y.val, magn.data.z.val);      
      
/*		
      // Get Gyroscope XYZ Data from I2C port
      DEBUG_PRINT("************** GYROSCOPE DATA *************\n");
		addr = 0x6b;// gyro
		reg = 0x28; // Device register to access gyro data
		read_sensor( data , addr, reg ); //send device address and register to start read from (data= 8 bytes read)	
		for (i = 0; i < 6; i++ ){
         // Copy xyz data from buffer to struct
         gyro.byt[i] = data[i];
			//DEBUG_PRINT(":%X", gyro.byt[i]);		
      }      
      //DEBUG_PRINT(":\n");
      // convert from two's complement
      gyro.data.x.val = ~gyro.data.x.val + 1;
      gyro.data.y.val = ~gyro.data.y.val + 1;
      gyro.data.z.val = ~gyro.data.z.val + 1;
      // Display Data
      DEBUG_PRINT(" X val = %d\n Y val = %d\n Z val = %d \n",gyro.data.x.val,gyro.data.y.val, gyro.data.z.val); 
      
*/      
//	}// End for loop
      
}// End getData()
void photo_active_thread(void* arg) {
  read_sensor(hamamatsuS1087_par_read, &(sensor_data->par));
}