void Gyro_Init()
{
gyro.init();
gyro.enableDefault();
gyro.writeReg(L3G::CTRL_REG4, 0x20); // 2000 dps full scale
gyro.writeReg(L3G::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
}
/**
* @brief Initializes the gyro/accelerometer and the magnetometer unit of the imu.
* As well as the arduino subsystem
*/
void setup() {
Wire.begin();
delay(1500);
/********/
/* GYRO */
/********/
gyro.init();
gyro.writeReg(L3G_CTRL_REG4, 0x00); // 245 dps scale
gyro.writeReg(L3G_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
//8.75 mdps/LSB
/****************/
/* MAGNETOMETER */
/****************/
compass.init();
compass.enableDefault();
compass.writeReg(LSM303::CTRL2, 0x08); // 4 g scale: AFS = 001
//0.122 mg/LSB
compass.writeReg(LSM303::CTRL5, 0x10); // Magnetometer Low Resolution 50 Hz
//Magnetometer 4 gauss scale : 0.16mgauss/LSB
//ROS-TF base frame of the imu_data
imu_msg.header.frame_id="base_imu_link";
//Register ROS messages
nh.initNode();
nh.advertise(imu_pub);
nh.advertise(mag_pub);
//starting value for the timer
timer=millis();
}
int main() {
pc.printf("Starting \r\n");
setup(); //initializes sensors
t.start();
timeLastPoll = t.read_ms();
while(button){
altitude = ps.pressureToAltitudeMeters(ps.readPressureMillibars());
gyr.read();
acc.read();
fprintf(fp, "%f, %d, %d, %d \r\n",
altitude,gyr.g.x,gyr.g.y,gyr.g.z);
pc.printf("%d Att: %2.2f \tGyr: %d %d %d \tAcc: %d %d %d \tT: %d\r\n",
iter,
altitude,
gyr.g.x,gyr.g.y,gyr.g.z,
acc.a.x,acc.a.y,acc.a.z,
t.read_ms()-timeLastPoll);
while( (t.read_ms() - timeLastPoll) < MBED_POLLING_PERIOD){
}
// pc.printf("Loop Time: %d",t.read_ms()-timeLastPoll);
timeLastPoll = t.read_ms();
iter++;
}
fclose(fp);
pc.printf("File successfully written! \r\n");
printf("End of Program. \r\n");
}
/**
* @brief provides imu readings in a 50 Hz rate.
*
*/
void loop() {
if((millis()-timer)>=20) { // Main loop runs at 50Hz
timer=millis();
//Read data from the hardware
gyro.read();
compass.readAcc();
compass.readMag();
//Assign read data to the ros messages
imu_msg.angular_velocity.x=gyro.g.x;
imu_msg.angular_velocity.y=gyro.g.y;
imu_msg.angular_velocity.z=gyro.g.z;
imu_msg.linear_acceleration.x=compass.a.x;
imu_msg.linear_acceleration.y=compass.a.y;
imu_msg.linear_acceleration.z=compass.a.z;
mag_msg.magnetic_field.x=compass.m.x;
mag_msg.magnetic_field.y=compass.m.y;
mag_msg.magnetic_field.z=compass.m.z;
//Publish the data to the ros message system
imu_pub.publish( &imu_msg );
mag_pub.publish( &mag_msg);
nh.spinOnce();
}
nh.spinOnce();
}
void setup(){
Serial.begin(115200);
Wire.begin(); //gyro code
gyro.init();//gyro code
gyro.enableDefault(); //gyro code
ps.enableDefault(); //baro code
handshaken = 0;
/*accelerometer code*/
compass.init();
compass.enableDefault();
/*accelerometer code*/
/*sonar code*/
pinMode(TRIGGER_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
pinMode(TRIGGER_PIN2, OUTPUT);
pinMode(ECHO_PIN2, INPUT);
pinMode(TRIGGER_PIN3, OUTPUT);
pinMode(ECHO_PIN3, INPUT);
pinMode(TRIGGER_PIN4, OUTPUT);
pinMode(ECHO_PIN4, INPUT);
pinMode(TRIGGER_PIN5, OUTPUT);
pinMode(ECHO_PIN5, INPUT);
/*sonar code*/
pinMode(MOTOR, OUTPUT);
/*
Calibration values; the default values of +/-32767 for each axis
lead to an assumed magnetometer bias of 0. Use the ACCalibrate
program to determine appropriate values for your particular unit.
*/
//compass.m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
//compass.m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
//LSM303::vector<int16_t> running_min = {-2872, -3215, -1742}, running_max = {+3019, +3108, +3570}; //calibration init data for compass
compass.m_min = (LSM303::vector<int16_t>){-2872, -3215, -1742};
compass.m_max = (LSM303::vector<int16_t>) {+3019, +3108, +3570};
}
void Read_Gyro()
{
gyro.read();
AN[0] = gyro.g.x;
AN[1] = gyro.g.y;
AN[2] = gyro.g.z;
gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
}
void setupSensor(){
// Sensor Set Up
rled = 1; gled = 1; bled = 1;
autoSD.ready_datalogger(); // Prepares a datalog filename according to index.txt
pc.printf("Index: %d\r\n",autoSD.curr_index());
fp = fopen(autoSD.filepath, "w");
if (fp == NULL) {
pc.printf("Unable to write the file \r\n");
while(1){ bled = !bled; wait_ms(100);}
}
if(!ps.init()){ //enable pressure sensor
pc.printf("Unable to talk to Barometer\r\n");
while(1){ rled = !rled; wait_ms(100);}
}
else{
ps.enableFIFO();
}
if(!gyr.init()){
pc.printf("Unable to talk to Gyroscope\r\n");
while(1){ rled = !rled; wait_ms(100);}
}
else{
gyr.enableFIFO();
}
if(!acc.init()){
pc.printf("Unable to talk to Accelerometer\r\n");
while(1){ rled = !rled; wait_ms(100);}
}
else{
acc.enableFIFO();
}
pc.printf("Sensor set up successfully\r\n");
}
void task_gyro(void* p){
/*gyro code*/
while(1){
gyro.read();// once gyro read is inside, code stops output. without gyro.read(), output is 0
// dprintf("X is %d, Y is %d, Z is %d", (int)gyro.g.x,(int)gyro.g.y,(int)gyro.g.z);
//delay(1000);
vTaskDelay(taskDelay);
}
/* gyro code*/
}
int main() {
pc.printf("Starting \r\n");
setup(); //initializes all hardware sensors
t.start();
while(button){
timeLastPoll = t.read_ms();
altitude = ps.pressureToAltitudeMeters(ps.readPressureMillibars());
acc.readFIFO();
gyr.readFIFO();
// pc.printf("%d Att: %2.2f \tGyr: %2.2f %2.2f %2.2f \tAcc: %2.2f %2.2f %2.2f \tT: %d\r\n",
// iter,
// altitude,
// gyr.g.x,gyr.g.y,gyr.g.z,
// acc.a.x,acc.a.y,acc.a.z,
// t.read_ms()-timeLastPoll);
if (iter != 0){
calcMotionData(a);
}
else{
calcMotionData(1.0);
}
r_altitude = roundData(altitude);
r_incline = roundData(incline);
r_dist = roundData(tot_dist/50.0);
r_speed = roundData(speed);
// pc.printf("%d \t%d \t%d \t%d\r\n",r_altitude,r_incline,r_dist,r_speed);
fprintf(fp, "%d, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %d, %d, %d \r\n",
(t.read_ms() - timeLastPoll),
altitude,
gyr.g.x,gyr.g.y,gyr.g.z,
acc.a.x,acc.a.y,acc.a.z,
incline,tot_dist,speed,
r_incline,
r_dist,
r_speed);
static unsigned long lastSendTime = millis();
if (lastSendTime + 250 < millis()) {
BTModu.sendData(String("x")+packageData2String(r_altitude)+
packageData2String(r_incline)+
packageData2String(r_dist)+
packageData2String(r_speed) );
lastSendTime += 250;
}
saveLastData();
while( (t.read_ms() - timeLastPoll) < MBED_POLLING_PERIOD_MS){
gled = 0;
}
// pc.printf("LT: %d\r\n",t.read_ms()-timeLastPoll);
gled = 1; iter++;
}
fclose(fp);
pc.printf("File successfully written! \r\n");
printf("End of Program. \r\n");
}
void task_poll_sensor(void* p){
while(1){
//unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
//unsigned int uS2 = sonar2.ping();
/* Serial.print("Sonar 1: ");
Serial.print(sonar.convert_cm(uS)); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
Serial.println("cm");
Serial.print("Sonar 2: ");
Serial.print(sonar2.convert_cm(uS2));
Serial.println("cm");*/
// dprintf("%d",(int)sonar.convert_cm(uS));
// vTaskDelay(1000);
// dprintf("%d",(int)sonar.convert_cm(uS2));
/* if(sonar.convert_cm(uS)<50){
digitalWrite(MOTOR, HIGH); // sets the LED on
delay(100); // waits for a second
//digitalWrite(MOTOR, LOW); // sets the LED off
//delay(1000); // waits for a second
}else{
digitalWrite(MOTOR, LOW);
delay(100);
}
*/
/* digitalWrite(TRIGGER_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN, LOW);
pinMode(ECHO_PIN,INPUT);
duration = pulseIn(ECHO_PIN, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 1", (int)distance);
digitalWrite(TRIGGER_PIN2, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN2, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN2, LOW);
pinMode(ECHO_PIN2,INPUT);
duration = pulseIn(ECHO_PIN2, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 2", (int)distance);
digitalWrite(TRIGGER_PIN3, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN3, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN3, LOW);
pinMode(ECHO_PIN3,INPUT);
duration = pulseIn(ECHO_PIN3, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 3", (int)distance);
digitalWrite(TRIGGER_PIN4, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN4, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN4, LOW);
pinMode(ECHO_PIN4,INPUT);
duration = pulseIn(ECHO_PIN4, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 4", (int)distance);
digitalWrite(TRIGGER_PIN5, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN5, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN5, LOW);
pinMode(ECHO_PIN5,INPUT);
duration = pulseIn(ECHO_PIN5, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
// dprintf("%d 5", (int)distance);
//Calculate the distance (in cm) based on the speed of sound.
/*distance = duration/58.2;*/
float distance1,distance2,distance3,distance4,distance5;
distance1 = sonar_read(TRIGGER_PIN,ECHO_PIN);
distance2= sonar_read(TRIGGER_PIN2,ECHO_PIN2);
distance3 = sonar_read(TRIGGER_PIN3,ECHO_PIN3);
distance4 = sonar_read(TRIGGER_PIN4,ECHO_PIN4);
distance5 = sonar_read(TRIGGER_PIN5,ECHO_PIN5);
dprintf("%d %d %d %d %d",(int)distance1,(int)distance2,(int)distance3,(int)distance4,(int)distance5);
/*dprintf("%d", (int) sonar_read(TRIGGER_PIN,ECHO_PIN));
dprintf("%d", (int) sonar_read(TRIGGER_PIN2,ECHO_PIN2));
dprintf("%d", (int) sonar_read(TRIGGER_PIN3,ECHO_PIN3));
dprintf("%d", (int) sonar_read(TRIGGER_PIN4,ECHO_PIN4));
dprintf("%d", (int) sonar_read(TRIGGER_PIN5,ECHO_PIN5));*/
/*sonar final code
digitalWrite(TRIGGER_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN, LOW);
pinMode(ECHO_PIN,INPUT);
duration = pulseIn(ECHO_PIN, HIGH,100000);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
*/
/*
pinMode(ECHO_PIN,INPUT);
digitalWrite(TRIGGER_PIN,HIGH);
delayMicroseconds(1000);
digitalWrite(TRIGGER_PIN,LOW);
duration = pulseIn(ECHO_PIN,HIGH);
distance = (duration/2)/29.1;*/
/* if(distance>10 && distance < 60){
digitalWrite(MOTOR, HIGH); // sets the LED on
// delay(100); // waits for a second
//digitalWrite(MOTOR, LOW); // sets the LED off
//delay(1000); // waits for a second
}else{
digitalWrite(MOTOR, LOW);
// delay(100);
}*/
// dprintf("%d",(int)distance);
/***********************************
** reading sensors
************************************/
compass.read();
dprintf("%d", int(compass.heading()));
//dprintf("%d z",(int)(compass.a.z/16.0));
/*if(compass.a.z/16.0<-1000){
distFromStart += 33;
step++;
dprintf("%d step",step);
}*/
/* float heading = compass.heading();
float XaVal, YaVal, ZaVal, fXa, fYa,fZa, pitch, roll,pitch_print, roll_print;
const float alpha = 0.15;
XaVal = compass.a.x/16.0; //Acceleration data registers contain a left-aligned 12-bit number, so values should be shifted right by 4 bits (divided by 16)
YaVal = compass.a.y/16.0; //unit is in cm/s2
ZaVal = compass.a.z/16.0;
/***********************************
** keypad
************************************/
char key = keypad.getKey();
//print out the key that is pressed
if (key != NO_KEY){
// Serial.print("You have pressed ");
Serial.println(key);
}
/***********************************
** altitude
************************************/
float pressure = ps.readPressureMillibars() + 248.5;
float altitude = ps.pressureToAltitudeMeters(pressure);
//dprintf("alt %d , pres %d",(int)altitude,(int)pressure);
// Serial.print("Pressure is ");
// Serial.print(pressure);
// Serial.println(" mbar");
// Serial.print("Altitude is ");
// Serial.print(altitude);// causes error
// Serial.println(" m.");
//dprintf("%d",(int)pressure);
//dprintf("%d",(int)altitude);
/******************************************************
** gyro meter reading
******************************************************/
gyro.read();
/*Serial.println("Gyro meter ");
Serial.print("X: ");
Serial.print((int)gyro.g.x * 8.75 /1000);
Serial.println(" degree/second");
Serial.print("Y: ");
Serial.print((int)gyro.g.y * 8.75 /1000);
Serial.println(" degree/second");
Serial.print("Z: ");
Serial.print((int)gyro.g.z * 8.75 /1000);
Serial.println(" degree/second");
Serial.println("");*/
//dprintf("x: %d",(int)(gyro.g.x * 8.75 /1000));
//dprintf("y: %d",(int)(gyro.g.y * 8.75 /1000));
//dprintf("z: %d",(int)(gyro.g.z * 8.75 /1000));
/*******************************************************************
get Headings
When given no arguments, the heading() function returns the angular
difference in the horizontal plane between a default vector and
north, in degrees.
/*
When given no arguments, the heading() function returns the angular
difference in the horizontal plane between a default vector and
north, in degrees.
The default vector is chosen by the library to point along the
surface of the PCB, in the direction of the top of the text on the
silkscreen. This is the +X axis on the Pololu LSM303D carrier and
the -Y axis on the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH
carriers.
To use a different vector as a reference, use the version of heading()
that takes a vector argument; for example, use
compass.heading((LSM303::vector<int>){0, 0, 1});
to use the +Z axis as a reference.
*******************************************************************/
// String direction = "";
/*if(heading>=340 || heading <= 20)
dprintf("North"); // direction = "North";
else if (heading>=70 && heading <= 110)
dprintf("East"); // direction = "East";
else if (heading>=160 && heading <= 200)
dprintf("South"); //direction = "South";
else if (heading>=250 && heading <= 290)
dprintf("West"); // direction = "West";
else if (heading>20 && heading < 70)
dprintf("North East"); // direction = "North East";
else if (heading>110 && heading < 160)
dprintf("South East"); // direction = "South East";
else if (heading>200 && heading < 250)
dprintf("South West");// direction = "South West";
else if (heading>290 && heading < 340)
dprintf("North West"); // direction = "North West";
// Serial.print("Heading is ");
//Serial.println(direction);
//Serial.println("degree.");
/******************************************************
** Method 1 to calculate distance: using steps
******************************************************/
// a step and distance using Z-ACCELERATION
/* if(ZaVal<-950){
distFromStart+=33; //1 step is 33 cm
step++;
}
/* Serial.print("X accel is ");Serial.print(XaVal); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Y accel is ");Serial.print(YaVal); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Z accel is ");Serial.print(ZaVal);Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("1. You have walked ");
Serial.print(step);
Serial.print(" steps and distance is ");
Serial.print(distFromStart);
Serial.println(" cm from start");*/
/*dprintf("x accel %d", (int)XaVal);
dprintf("y accel %d",(int) YaVal);
dprintf("z accel %d", (int)ZaVal); */
/******************************************************
** pitch and roll
******************************************************/
// Low-Pass filter accelerometer
/* fXa = XaVal * alpha + (fXa * (1.0 - alpha));
fYa = YaVal * alpha + (fYa * (1.0 - alpha));
fZa = ZaVal * alpha + (fZa * (1.0 - alpha));
/* Serial.print("Low pass X accel is ");Serial.print(fXa); Serifal.print(" cm/s2"); Serial.println(" ");
Serial.print("Low pass Y accel is ");Serial.print(fYa); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Low pass Z accel is ");Serial.print(fZa);Serial.print(" cm/s2"); Serial.println(" "); */
/* roll = atan2(fYa, sqrt(fXa*fXa + fZa*fZa));
pitch = atan2(fXa, sqrt(fYa*fYa + fZa*fZa));
roll_print = roll*180.0/M_PI;
pitch_print = pitch*180.0/M_PI;
/* Serial.print("pitch(Y) is ");
Serial.print(pitch_print);
Serial.println("degree ");
Serial.print("roll(X) is ");
Serial.print(roll_print);
Serial.println("degree ");*/
/******************************************************
** Method 2 to calculate distance: using accelerations
******************************************************/
/* newTime = millis();
deltaTime = newTime - oldTime;
XaVal = XaVal - (1000 * (sin(pitch)));//offsetting pitch
// estimate the average acceleration since the previous sample, by averaging the two samples
long avgAccel = (oldXaVal + XaVal) / 2;
//if ((XaVal < 50 && XaVal > -50) && (oldXaVal < 50 && oldXaVal > -50))
// avgAccel = 0;
/* working
Serial.print("the avgAccel is ");
Serial.print(avgAccel);
Serial.println(" cm/s2");*/
// integrate the average accel and add it to the previous speed to calculate the new speed
// long newVelocity = oldVelocity + (avgAccel * deltaTime/1000);
//estimate the average speed since the previous sample, by averaging the two speeds
//long avgVelocity = (oldVelocity + newVelocity) / 2;
// if ((XaVal < 50 && XaVal > -50) && (oldXaVal < 50 && oldXaVal > -50))
// avgVelocity = 0;
// integrate the average speed and add it to the previous displacement to get the new displacement
/* long newDisplacement = oldDis + (avgVelocity * deltaTime/1000);
oldTime = newTime;
oldVelocity = newVelocity ;
oldDis = newDisplacement;
oldXaVal = XaVal;*/
/*working
Serial.print("2. You have walked ");
Serial.print(newDisplacement);
Serial.println("cm from start"); */
/******************************************************
** IR sensor meter reading
******************************************************/
sensorValue = analogRead(sensorIR);
cm = 10650.08 * pow(sensorValue,-0.935) - 10;
/* Serial.print("IR sensor reads ");
Serial.print(cm);
Serial.println(" Cm");*/
//delay(100);
vTaskDelay(100);
}
}
void task_sensor_poll(void* p){
while(1){
/***********************************
** reading sensors
************************************/
compass.read();
/* float heading = compass.heading();
float XaVal, YaVal, ZaVal, fXa, fYa,fZa, pitch, roll,pitch_print, roll_print;
const float alpha = 0.15;
XaVal = compass.a.x/16.0; //Acceleration data registers contain a left-aligned 12-bit number, so values should be shifted right by 4 bits (divided by 16)
YaVal = compass.a.y/16.0; //unit is in cm/s2
ZaVal = compass.a.z/16.0;
/*
/***********************************
** keypad
************************************/
char key = keypad.getKey();
//print out the key that is pressed
if (key != NO_KEY){
Serial.print("You have pressed ");
Serial.println(key);
}
/***********************************
** altitude
************************************/
float pressure = ps.readPressureMillibars() + 248.5;
float altitude = ps.pressureToAltitudeMeters(pressure);
/* Serial.print("Pressure is ");
Serial.print(pressure);
Serial.println(" mbar");
Serial.print("Altitude is ");
Serial.print(altitude);
Serial.println(" m.");
/******************************************************
** gyro meter reading
******************************************************/
gyro.read();
/* Serial.println("Gyro meter ");
Serial.print("X: ");
Serial.print((int)gyro.g.x * 8.75 /1000);
Serial.println(" degree/second");
Serial.print("Y: ");
Serial.print((int)gyro.g.y * 8.75 /1000);
Serial.println(" degree/second");
Serial.print("Z: ");
Serial.print((int)gyro.g.z * 8.75 /1000);
Serial.println(" degree/second");
Serial.println("");
/*******************************************************************
get Headings
When given no arguments, the heading() function returns the angular
difference in the horizontal plane between a default vector and
north, in degrees.
/*
When given no arguments, the heading() function returns the angular
difference in the horizontal plane between a default vector and
north, in degrees.
The default vector is chosen by the library to point along the
surface of the PCB, in the direction of the top of the text on the
silkscreen. This is the +X axis on the Pololu LSM303D carrier and
the -Y axis on the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH
carriers.
To use a different vector as a reference, use the version of heading()
that takes a vector argument; for example, use
compass.heading((LSM303::vector<int>){0, 0, 1});
to use the +Z axis as a reference.
*******************************************************************/
/* String direction = "";
if(heading>=340 || heading <= 20)
direction = "North";
else if (heading>=70 && heading <= 110)
direction = "East";
else if (heading>=160 && heading <= 200)
direction = "South";
else if (heading>=250 && heading <= 290)
direction = "West";
else if (heading>20 && heading < 70)
direction = "North East";
else if (heading>110 && heading < 160)
direction = "South East";
else if (heading>200 && heading < 250)
direction = "South West";
else if (heading>290 && heading < 340)
direction = "North West";
Serial.print("Heading is ");
Serial.println(direction);
//Serial.println("degree.");
/******************************************************
** Method 1 to calculate distance: using steps
******************************************************/
// a step and distance using Z-ACCELERATION
/* if(ZaVal<-965){
distFromStart+=33; //1 step is 33 cm
step++;
}
Serial.print("X accel is ");Serial.print(XaVal); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Y accel is ");Serial.print(YaVal); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Z accel is ");Serial.print(ZaVal);Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("1. You have walked ");
Serial.print(step);
Serial.print(" steps and distance is ");
Serial.print(distFromStart);
Serial.println(" cm from start");
/******************************************************
** pitch and roll
******************************************************/
// Low-Pass filter accelerometer
/* fXa = XaVal * alpha + (fXa * (1.0 - alpha));
fYa = YaVal * alpha + (fYa * (1.0 - alpha));
fZa = ZaVal * alpha + (fZa * (1.0 - alpha));
Serial.print("Low pass X accel is ");Serial.print(fXa); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Low pass Y accel is ");Serial.print(fYa); Serial.print(" cm/s2"); Serial.println(" ");
Serial.print("Low pass Z accel is ");Serial.print(fZa);Serial.print(" cm/s2"); Serial.println(" ");
roll = atan2(fYa, sqrt(fXa*fXa + fZa*fZa));
pitch = atan2(fXa, sqrt(fYa*fYa + fZa*fZa));
roll_print = roll*180.0/M_PI;
pitch_print = pitch*180.0/M_PI;
Serial.print("pitch(Y) is ");
Serial.print(pitch_print);
Serial.println("degree ");
Serial.print("roll(X) is ");
Serial.print(roll_print);
Serial.println("degree ");
/******************************************************
** Method 2 to calculate distance: using accelerations
******************************************************/
//newTime = millis();
/* deltaTime = newTime - oldTime;
XaVal = XaVal - (1000 * (sin(pitch)));//offsetting pitch
// estimate the average acceleration since the previous sample, by averaging the two samples
long avgAccel = (oldXaVal + XaVal) / 2;
//if ((XaVal < 50 && XaVal > -50) && (oldXaVal < 50 && oldXaVal > -50))
// avgAccel = 0;
/* Serial.print("the avgAccel is ");
Serial.print(avgAccel);
Serial.println(" cm/s2");
// integrate the average accel and add it to the previous speed to calculate the new speed
long newVelocity = oldVelocity + (avgAccel * deltaTime/1000);
//estimate the average speed since the previous sample, by averaging the two speeds
long avgVelocity = (oldVelocity + newVelocity) / 2;
// if ((XaVal < 50 && XaVal > -50) && (oldXaVal < 50 && oldXaVal > -50))
// avgVelocity = 0;
// integrate the average speed and add it to the previous displacement to get the new displacement
long newDisplacement = oldDis + (avgVelocity * deltaTime/1000);
oldTime = newTime;
oldVelocity = newVelocity ;
oldDis = newDisplacement;
oldXaVal = XaVal;
Serial.print("2. You have walked ");
Serial.print(newDisplacement);
Serial.println("cm from start");
/******************************************************
** IR sensor meter reading
******************************************************/
sensorValue = analogRead(sensorIR);
cm = 10650.08 * pow(sensorValue,-0.935) - 10;
/* Serial.print("IR sensor reads ");
Serial.print(cm);
Serial.println(" Cm");
/***********************************
** reading sensors
************************************/
/*
//digitalWrite(ECHO_PIN2 ,LOW);
unsigned int uS2 = sonar2.ping();
Serial.print("Sonar 2: ");
Serial.print(sonar2.convert_cm(uS2));
Serial.println("cm");
if(sonar2.convert_cm(uS2)<50){
digitalWrite(MOTOR, HIGH); // waits for a second
// sets the LED off
//delay(1000); // waits for a second
}
else{
digitalWrite(MOTOR, LOW);
}
//delay(100);
*/
/* The following trigPin/echoPin cycle is used to determine the
distance of the nearest object by bouncing soundwaves off of it. */
digitalWrite(TRIGGER_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN, LOW);
duration = pulseIn(ECHO_PIN, HIGH);
//Calculate the distance (in cm) based on the speed of sound.
distance = duration/58.2;
dprintf("%d",(int)distance);
/* Serial.print("sonar distance is ");
Serial.println(distance);
Serial.println();*/
if (distance >= 10 && distance <= 70){
/* Send a negative number to computer and Turn LED ON
to indicate "out of range" */
//Serial.println("-1");
digitalWrite(MOTOR, HIGH);
}
else {
/* Send the distance to the computer using Serial protocol, and
turn LED OFF to indicate successful reading. */
//Serial.println(distance);
digitalWrite(MOTOR, LOW);
}
//Delay 50ms before next reading.
delay(50);
}
}
/*
* 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
}
/*
* 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();
}
