<commit_before>/*
*	Odometer.cpp - Handles the gyroscope (L3G4200D) sensor of the Smartcar.
*	Version: 0.3
*	Author: <NAME> (based on the bildr.org example: http://bildr.org/2011/06/l3g4200d-arduino/)
* 	License: GNU GPL v3 http://www.gnu.org/licenses/gpl-3.0.html
*/

#include "Smartcar.h"


/* ---- GYROSCOPE (L3G4200D) ---- */
const unsigned short Gyroscope::DEFAULT_GYRO_SAMPLING = 100;
static const int GYRO_OFFSET = 10; //The value that is usually given by the gyroscope when not moving. Determined experimentally, adapt accordingly.
static const float GYRO_SENSITIVITY = 0.07; //L3G4200D specific.
static const int GYRO_THRESHOLD = 12; //Tolerance threshold. Determined experimentally, adapt accordingly.
static const int CTRL_REG1 = 0x20;
static const int CTRL_REG2 =  0x21;
static const int CTRL_REG3 = 0x22;
static const int CTRL_REG4 = 0x23;
static const int CTRL_REG5 = 0x24;
static const int L3G4200D_Address = 105; //gyroscope I2C address
volatile int _angularDisplacement = 0;
volatile unsigned long _prevSample = 0;

Gyroscope::Gyroscope(){
}

void Gyroscope::attach(){
	initializeGyro();
}

void Gyroscope::initMeasurement(){
	_angularDisplacement = 0;
	_prevSample = 0;
}

void Gyroscope::begin(unsigned short samplingRate){
	initMeasurement();
	_prevSample = millis();
	_samplingRate = samplingRate;
}

int Gyroscope::getAngularDisplacement(){
	return - _angularDisplacement; //we negate the value, in order to get negative values when turning counter clockwise (local convention)
}

/* based on http://www.pieter-jan.com/node/7 integration algorithm */
void Gyroscope::update(){
	if (millis()- _prevSample > _samplingRate){
		float gyroRate = 0;
		int gyroValue = getGyroValues(); 
		short drift = GYRO_OFFSET - gyroValue;
 		if (abs(drift) > GYRO_THRESHOLD){ //if there has been a big enough drift (trying to contemplate for the noise)
			gyroRate = (gyroValue - GYRO_OFFSET) * GYRO_SENSITIVITY;
		}
		unsigned long now = millis();
		_angularDisplacement += gyroRate / (1000 / (now - _prevSample));
		_prevSample = now;
	}
}

void Gyroscope::initializeGyro(){
	Wire.begin();
	setupL3G4200D(2000); // Configure L3G4200 at 2000 deg/sec. Other options: 250, 500 (NOT suggested, will have to redetermine offset) 
}

/* based on the bildr.org example: http://bildr.org/2011/06/l3g4200d-arduino/ */
int Gyroscope::getGyroValues(){
	byte zMSB = readRegister(L3G4200D_Address, 0x2D);
	byte zLSB = readRegister(L3G4200D_Address, 0x2C);
	return ((zMSB << 8) | zLSB);
}

int Gyroscope::setupL3G4200D(int scale){
	//From  <NAME> of Sparkfun's code

	// Enable x, y, z and turn off power down:
	writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);

	// If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
	writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);

	// Configure CTRL_REG3 to generate data ready interrupt on INT2
	// No interrupts used on INT1, if you'd like to configure INT1
	// or INT2 otherwise, consult the datasheet:
	writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);

	// CTRL_REG4 controls the full-scale range, among other things:

	if(scale == 250){
		writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
	}else if(scale == 500){
		writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
	}else{
		writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
	}

	// CTRL_REG5 controls high-pass filtering of outputs, use it
	// if you'd like:
	writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}

void Gyroscope::writeRegister(int deviceAddress, byte address, byte val) {
	Wire.beginTransmission(deviceAddress); // start transmission to device 
	Wire.write(address);       // send register address
	Wire.write(val);         // send value to write
	Wire.endTransmission();     // end transmission
}

int Gyroscope::readRegister(int deviceAddress, byte address){
	int v;
	Wire.beginTransmission(deviceAddress);
	Wire.write(address); // register to read
	Wire.endTransmission();
	Wire.requestFrom(deviceAddress, 1); // read a byte

	while(!Wire.available()) {
		//waiting
 	}

	v = Wire.read();

	return v;
}
<commit_msg>various small fixes and calibration method<commit_after>/*
*	Odometer.cpp - Handles the gyroscope (L3G4200D) sensor of the Smartcar.
*	Version: 0.3
*	Author: <NAME> (based on the bildr.org example: http://bildr.org/2011/06/l3g4200d-arduino/)
* 	License: GNU GPL v3 http://www.gnu.org/licenses/gpl-3.0.html
*/

#include "Smartcar.h"


/* ---- GYROSCOPE (L3G4200D) ---- */
const unsigned short Gyroscope::DEFAULT_GYRO_SAMPLING = 100;
const int GYRO_OFFSET = 8; //value that's usually given by the gyroscope when not moving. Use calibrate method to get an approximation
const float GYRO_SENSITIVITY = 0.07; //L3G4200D specific.
const int GYRO_THRESHOLD = 12; //Tolerance threshold. Determined experimentally, adapt accordingly.
const int CTRL_REG1 = 0x20;
const int CTRL_REG2 = 0x21;
const int CTRL_REG3 = 0x22;
const int CTRL_REG4 = 0x23;
const int CTRL_REG5 = 0x24;
const int L3G4200D_Address = 105; //gyroscope I2C address

Gyroscope::Gyroscope(){
}

void Gyroscope::attach(){
	initializeGyro();
}

void Gyroscope::begin(unsigned short samplingRate){
	_angularDisplacement = 0;
	_prevSample = millis();
	_samplingRate = samplingRate;
}

int Gyroscope::getAngularDisplacement(){
	return - _angularDisplacement; //we negate the value, in order to get negative values when turning counter clockwise (local convention)
}

/* based on http://www.pieter-jan.com/node/7 integration algorithm */
void Gyroscope::update(){
	if (millis()- _prevSample > _samplingRate){
		float gyroRate = 0;
		int gyroValue = getGyroValues(); 
		short drift = GYRO_OFFSET - gyroValue;
 		if (abs(drift) > GYRO_THRESHOLD){ //if there has been a big enough drift (trying to contemplate for the noise)
			gyroRate = (gyroValue - GYRO_OFFSET) * GYRO_SENSITIVITY;
		}
		unsigned long now = millis();
		_angularDisplacement += gyroRate / (1000 / (now - _prevSample));
		_prevSample = now;
	}
}

void Gyroscope::initializeGyro(){
	Wire.begin();
	setupL3G4200D(2000); // Configure L3G4200 at 2000 deg/sec. Other options: 250, 500 (NOT suggested, will have to redetermine offset) 
}

/* based on the bildr.org example: http://bildr.org/2011/06/l3g4200d-arduino/ */
int Gyroscope::getGyroValues(){
	byte zMSB = readRegister(L3G4200D_Address, 0x2D);
	byte zLSB = readRegister(L3G4200D_Address, 0x2C);
	return ((zMSB << 8) | zLSB);
}

int Gyroscope::setupL3G4200D(int scale){
	//From  <NAME> of Sparkfun's code

	// Enable x, y, z and turn off power down:
	writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);

	// If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
	writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);

	// Configure CTRL_REG3 to generate data ready interrupt on INT2
	// No interrupts used on INT1, if you'd like to configure INT1
	// or INT2 otherwise, consult the datasheet:
	writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);

	// CTRL_REG4 controls the full-scale range, among other things:

	if(scale == 250){
		writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
	}else if(scale == 500){
		writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
	}else{
		writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
	}

	// CTRL_REG5 controls high-pass filtering of outputs, use it
	// if you'd like:
	writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}

void Gyroscope::writeRegister(int deviceAddress, byte address, byte val) {
	Wire.beginTransmission(deviceAddress); // start transmission to device 
	Wire.write(address);       // send register address
	Wire.write(val);         // send value to write
	Wire.endTransmission();     // end transmission
}

int Gyroscope::readRegister(int deviceAddress, byte address){
	int v;
	Wire.beginTransmission(deviceAddress);
	Wire.write(address); // register to read
	Wire.endTransmission();
	Wire.requestFrom(deviceAddress, 1); // read a byte

	while(!Wire.available()) {
		//waiting
 	}

	v = Wire.read();

	return v;
}

unsigned int Gyroscope::calibrate(unsigned int measurements){ //use this function in order to determine the offset and change GYRO_OFFSET accordingly
	unsigned int sum = 0;
	for (int i = 0; i < measurements; i++){	
		sum += getGyroValues();
		delay(50);
	}
	return sum/measurements; //return the average	
}

{'fraction_non_alphanumeric': 0.11208659049909801, 'fraction_numerical': 0.06133493686109441, 'mean_word_length': 4.529255319148936, 'pattern_counts': {'":': 0, '<': 12, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 4, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 6, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4573166', 'n_tokens_mistral': 3370, 'n_tokens_neox': 2910, 'n_words': 820}