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();
}
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;
}
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;
}
/* 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);
}
}
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);
}
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));
}
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(¤t_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;
}
}
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));
}