Exemplo n.º 1
0
/*
 * Main Loop 
 */
void loop() {
	wdt_reset();
	mD.vals.uslCount++;									//Increment main datarecord count
	AccelerometerScaled Ascaled = accel.ReadScaledAxis();	//Get Scaled Accelerometer
	AccelerometerRaw Araw = accel.ReadRawAxis();			//Get Raw Accelerometer
	MagnetometerScaled Mscaled = compass.ReadScaledAxis();	//Get Scaled Magnetometer
	MagnetometerRaw Mraw = compass.ReadRawAxis();			//Get Raw Magnetometer
	LGgyro.read();											//Get Gyro

	// offset compass by hard iron
	Mraw.XAxis += 40;
	Mraw.YAxis += 261;
	Mraw.ZAxis += 54;

	//write Acc, Mag, & Gyro values to record
	float AxisGs = Ascaled.XAxis;
	mD.vals.AcXPayload = AxisGs * 100;
	AxisGs = Ascaled.YAxis;
	mD.vals.AcYPayload = AxisGs * 100;
	AxisGs = Ascaled.ZAxis;
	mD.vals.AcZPayload = AxisGs * 100;
	mD.vals.MgXPayload = Mscaled.XAxis;
	mD.vals.MgYPayload = Mscaled.YAxis;
	mD.vals.MgZPayload = Mscaled.ZAxis;
	mD.vals.GyXPayload = LGgyro.g.x;
	mD.vals.GyYPayload = LGgyro.g.y;
	mD.vals.GyZPayload = LGgyro.g.z;

	//Perform tilt compensation calculation save to record
	sixDOF.compCompass(Mraw.XAxis, -Mraw.YAxis, -Mraw.ZAxis, Araw.XAxis, Araw.YAxis, Araw.ZAxis, true);
	float compHeading = sixDOF.atan2Int(sixDOF.xAxisComp(), sixDOF.yAxisComp());
	compHeading = compHeading /100;
	if (compHeading < 0 ) {
		compHeading = abs(compHeading);
	} else {
		compHeading = 180 - compHeading + 180;
	}
	mD.vals.CmpssPayload = compHeading;
	
	//get BMP085 values save to record
	dps.getTemperature(&TmpPayloadFULL); 
	dps.getPressure(&mD.vals.PressurePayload);

 	mD.vals.TmpPayload = (int16_t)(TmpPayloadFULL);	
	mD.vals.TmpExternal = (int16_t)(sensors.getTempC(outsideThermometer)* 10);
	sensors.requestTemperaturesByAddress(outsideThermometer); // Send the command to get temperatures
	
	//get GPS data
	byte lcount = 0;									//reset a loop counter
	while (!NEWGPSDATA && lcount++ < 255) {				//Exit the loop if we have new data or have been round it a number of times
		NEWGPSDATA = feedgps();							
	}
	if (NEWGPSDATA) {									//We have new GPS data, get all of the fields we need.
		int tmp_year = 0;
		gps.crack_datetime(&tmp_year, &mD.vals.month, &mD.vals.day,&mD.vals.hour, &mD.vals.minute, &mD.vals.second, &mD.vals.hundredths, &mD.vals.age);
		mD.vals.year = tmp_year - 2000;
		
        if (gps.altitude() != TinyGPS_HJOE::GPS_INVALID_ALTITUDE && gps.altitude() >= 0) {
			gps.get_position(&mD.vals.iLat, &mD.vals.iLong, &mD.vals.age);
			mD.vals.iAlt = gps.altitude(); 
			mD.vals.iAngle = gps.course();
			mD.vals.iHspeed = gps.speed(); 
			mD.vals.bSats = gps.satellites();
			mD.vals.ihdop = gps.hdop();
		}
		SET_LED_Status(SET_LED_BLUE,0);					//Flash blue to show we are getting GPS data
	} else {
		SET_LED_Status(SET_LED_GREEN,0);				//Flash Green to show that we are looping but not getting GPS data
	}

	if(ETSerialIn.receiveData()){

	}
  
	//flip flop between I2C's to avoid both on one loop
	if (SENDWIRE && (millis() - elapseSIM900) > WAIT_SIM900) {
		mD.vals.tCount++;
		ETI2Cout.sendData(I2C_SLV_SIM900_ADDRESS);		
		elapseSIM900 = millis();
	}

	if (!SENDWIRE && (millis() - elapseNTXB) > WAIT_NTXB) {
		mD.vals.tCount++;
		ETI2Cout.sendData(I2C_SLV_NTXB_ADDRESS);
		elapseNTXB = millis();
		//get I2C_SLV_SIM900_ADDRESS data
	}

	writeSDData();										//Write the data record to the SD card
	SET_LED_Status(SET_LED_OFF,0);						//turn off the LED
	NEWGPSDATA = false;									//Reset the New GPS Data flag
	SENDWIRE = !SENDWIRE;								//Flipflop this 
}
// Initialize
void setup() {
	
	// Communication
	Serial.begin(9600);      // XB
    if(debug) Serial << "Beginning setup" << endl;
	nssROBOBRRD.begin(9600); // RB
    RBrx_ET.begin(details(RBdata_rx), &nssROBOBRRD);
    // for some reason it can't do this... 
    // we'll send raw data w/just delims for now
    //RBtx_ET.begin(details(RBdata_tx), &nssROBOBRRD);
    XB_ET.begin(details(XBdata), &Serial);
    
	// Interrupts
	pinMode(interruptOutgoing, OUTPUT);
    digitalWrite(interruptOutgoing, LOW);
    
	attachInterrupt(interruptIncoming, trigger, RISING);
    digitalWrite(2, LOW);
    
    pinMode(flagPin, INPUT);
    
    // Outputs
	pinMode(LED, OUTPUT);
	pinMode(STATUS, OUTPUT);
    
    pinMode(spkr, OUTPUT);
    
    // ET Data
    RBdata_rx.attention = 0;
    RBdata_rx.valence = 0;
    RBdata_rx.stance = 0;
    
    RBdata_rx.action_t = 0;
    RBdata_rx.action_t0 = 0;
    
    RBdata_rx.robot = 0;
    
    RBdata_rx.sense_1 = 0;
    RBdata_rx.sense_2 = 0;
    RBdata_rx.sense_3 = 0;
    RBdata_rx.sense_4 = 0;
    RBdata_rx.sense_5 = 0;
    
    RBdata_rx.world_L = 0;
    RBdata_rx.world_R = 0;
    RBdata_rx.world_F = 0;
    
    
    XBdata.attention = 0;
    XBdata.valence = 0;
    XBdata.stance = 0;
    
    XBdata.action_t = 0;
    XBdata.action_t0 = 0;
    
    XBdata.robot = 1;
    
    XBdata.sense_1 = 0;
    XBdata.sense_2 = 0;
    XBdata.sense_3 = 0;
    XBdata.sense_4 = 0;
    XBdata.sense_5 = 0;
    
    XBdata.world_L = 0;
    XBdata.world_R = 0;
    XBdata.world_F = 0;
    
    
    digitalWrite(LED, LOW);
    digitalWrite(STATUS, LOW);
    
    digitalWrite(LED, HIGH);
    delay(1000);
    digitalWrite(LED, LOW);
    
    
    digitalWrite(STATUS, HIGH);
    delay(1000);
    digitalWrite(STATUS, LOW);
    
    
    delay(2000);
    
    if(debug) Serial << "Done setup" << endl;
    
}
Exemplo n.º 3
0
/*
 * Setup 
 */
void setup() {
	wdt_enable(WDTO_8S);
	wdt_reset();
	//Setup Ports
	Serial.begin(115200);				//Start Debug Serial 0
	Serial1.begin(9600); 				//Start GPS Serial 1
	Serial2.begin(9600);
 
	pinMode(PIN_LED_GREEN, OUTPUT);		//Blue GREEN
	pinMode(PIN_LED_RED, OUTPUT);		//Blue RED
	pinMode(PIN_LED_BLUE, OUTPUT);		//Blue LED
	pinMode(PIN_SPI_CS,OUTPUT);  		//Chip Select Pin for the SD Card
	pinMode(10, OUTPUT);				//SDcard library expect 10 to set set as output.
	
	// Initialise the GPS
	wdt_disable();
	gps.init();						
	gps.configureUbloxSettings();		// Configure Ublox for MY_HIGH altitude mode
	wdt_enable(WDTO_8S);
	// join I2C bus //start I2C transfer to the Module/Transmitter
	Wire.begin();
	//Set up the two EasyTransfer methods
	ETI2Cout.begin(details(mD.i2cOut), &Wire);	//setup the data structure to transfer out
	ETSerialIn.begin(details(vals), &Serial2);
	
	//Start up the LGgyro
    if (LGgyro.init()) {
		#ifdef DEBUG_ON	
			Serial.println("LGgyro OK");
		#endif
		LGgyro.enableDefault();
	} else {
		#ifdef DEBUG_ON	
			Serial.println("LGgyro not working");
		#endif
		SET_LED_Status(SET_LED_WHITE,500); 	//White LED
		SET_LED_Status(SET_LED_RED,1000); 	//Red LED 
	}

	//Start up the accelerometer
	accel = ADXL345(); 						// Create an instance of the accelerometer
	if(accel.EnsureConnected()) {			// Check that the accelerometer is connected.
		#ifdef DEBUG_ON	
			Serial.println("Connected to ADXL345.");
		#endif		
		accel.SetRange(2, true);				// Set the range of the accelerometer to a maximum of 2G.
		accel.EnableMeasurements();				// Tell the accelerometer to start taking measurements.		
	} else{
		#ifdef DEBUG_ON	
			Serial.println("Could not connect to ADXL345.");
		#endif
		SET_LED_Status(SET_LED_WHITE,500); 	//White LED
		SET_LED_Status(SET_LED_RED,2000); 	//Red LED 
	}

	//Start up the compass
	compass = HMC5883L(); 						// Construct a new HMC5883 compass.
	#ifdef DEBUG_ON	
		if(compass.EnsureConnected() == 1) {
			Serial.println("Connected to HMC5883L.");
		} else {
			Serial.println("Not Connected to HMC5883L.");
		}
	#endif
	error = compass.SetScale(1.3); 				// Set the scale of the compass.
	#ifdef DEBUG_ON	
		if(error != 0) {							// If there is an error, print it out.
			Serial.println("Compass Error 1");
			Serial.println(compass.GetErrorText(error));
		} else {
			Serial.println("Compass Ok 1");
		}
	#endif
	error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
	#ifdef DEBUG_ON	
		if(error != 0) {							// If there is an error, print it out.
			Serial.println("Compass error 2");
			Serial.println(compass.GetErrorText(error));
		} else {
			Serial.println("Compass Ok 2");
		}
	#endif	
	
	//Start up the Pressure Sensor
	dps = BMP085();
	dps.init(); 
	#ifdef DEBUG_ON
		Serial.print("BMP Mode ");
		Serial.println(dps.getMode());
	#endif	
	wdt_reset();
	// Start up the OneWire Sensors library and turn off blocking takes too long!
	sensors.begin();
	sensors.setWaitForConversion(false);
  	sensors.requestTemperaturesByAddress(outsideThermometer); // Send the command to get temperature
	
	//Initialise all of the record values
	mD.vals.tCount = 0;
	mD.vals.uslCount = 0;
	mD.vals.year = 0;
	mD.vals.month = 0;
	mD.vals.day = 0;
	mD.vals.hour = 0;
	mD.vals.minute = 0;
	mD.vals.second = 0;
	mD.vals.hundredths = 0;
	mD.vals.iLat = 0;
	mD.vals.iLong = 0;
	mD.vals.iAlt = 0;
	mD.vals.bSats = 0;
	mD.vals.iAngle = 0;
	mD.vals.iHspeed = 0;
	mD.vals.iVspeed = 0;
	mD.vals.age = 0;
	mD.vals.ihdop = 0;
	mD.vals.AcXPayload = 0;
	mD.vals.AcYPayload = 0;
	mD.vals.AcZPayload = 0;
	mD.vals.GyXPayload = 0;
	mD.vals.GyYPayload = 0;
	mD.vals.GyZPayload = 0;
	mD.vals.MgXPayload = 0;
	mD.vals.MgYPayload = 0;
	mD.vals.MgZPayload = 0;
	mD.vals.TmpPayload = 0;
	
	//Connect to the SD Card	
	if(!SD.begin(PIN_SPI_CS, SPI_HALF_SPEED)) {
		#ifdef DEBUG_ON	
			Serial.println("SD not working!!");
		#endif 
		SET_LED_Status(SET_LED_WHITE,500); 	//White LED
		SET_LED_Status(SET_LED_RED,3000); 	//Red LED 
	} else {
		#ifdef DEBUG_ON	
			Serial.println("SD OK");
		#endif 	
		dataFile.open(SD_LOG_FILE, O_CREAT | O_WRITE | O_APPEND);	    //Open Logfile
		if (!dataFile.isOpen()) {
			#ifdef DEBUG_ON	
				Serial.println("SD Data File Not Opened");
			#endif 	
			SET_LED_Status(SET_LED_WHITE,500);
			SET_LED_Status(SET_LED_RED,3000);
		}
	}

	//Cycle lights
	SET_LED_Status(SET_LED_OFF,0);  
	SET_LED_Status(SET_LED_RED,500);
	SET_LED_Status(SET_LED_GREEN,500);
	SET_LED_Status(SET_LED_BLUE,500);
	SET_LED_Status(SET_LED_OFF,0);  
	
	elapseSIM900 = millis();				//Elapse counter for data to SIM900
	elapseNTXB = millis();					//Elapse counter for data to NTXB
	NEWGPSDATA = false;
	wdt_enable(WDTO_2S);
	wdt_reset();
}