void task_headingNdist(void* p){
while(1){
//Serial.print("z");
compass.read();
//dprintf("%d mem",(int)freeRAM());
//Serial.print("a");
float heading = compass.heading();
//Serial.print("b");
//dprintf("%d mem2",(int)freeRAM());
//dprintf("%d",(int)compass.a.x);
float ZaVal,XaVal,YaVal,RZaVal;
//ZaVal = compass.a.z/16.0;
XaVal = compass.a.x/16.0;
YaVal = compass.a.y/16.0;
ZaVal = compass.a.z/16.0;
RZaVal = sqrt( ((XaVal*XaVal)+(YaVal*YaVal))/4.0 + (ZaVal*ZaVal));
//ZaVal = sqrt( ((compass.a.x*compass.a.x)+(compass.a.y*compass.a.y))/4.0 + (compass.a.z*compass.a.z));
//ZaVal = ZaVal/16.0;
// dprintf("%d", (int)compass.a.x);
//dprintf("%d", (int)((compass.a.x*compass.a.x)/*+(compass.a.y*compass.a.y)/4.0*/));
// dprintf("%d x %d y %d z", (int)compass.a.x, (int)compass.a.y,(int) compass.a.z);
// dprintf("%d z val",(int) ZaVal);
// dprintf("%d",(int) ZaVal);
//Serial.print("a");
//dprintf("%d",(int)heading);
//dprintf("%d",(int)compass.heading());
//compass.heading();
//Serial.print("b");
//data[ID_DATA_HEADING] = (int) compass.heading();
/* if(ZaVal<-965){
distFromStart+=33; //1 step is 33 cm
step++;
}*/
float currentTime = millis();
if(RZaVal>1000 && (currentTime - prevTime) >= 600){
distFromStart+=33; //1 step is 33 cm
step++;
dprintf("%d_RZaval",(int)RZaVal);
dprintf("%d ",(int)step);
prevTime = currentTime;
}
data[ID_DATA_DIST] = (int) distFromStart;
//dprintf("dist is %d",(int)data[ID_DATA_DIST]);
//dprintf("a");
//delay(100);*/
delay(100);
vTaskDelay(taskDelay);
}
}
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 printdata(void)
{
//Serial.print("!");
#if PRINT_EULER == 1
Serial.print("ANG:");
Serial.print(ToDeg(roll));
Serial.print(",");
Serial.print(ToDeg(pitch));
Serial.print(",");
Serial.print(ToDeg(yaw));
Serial.print(",");
//MAG_Heading = ToDeg(MAG_Heading);
//if(MAG_Heading<0) MAG_Heading= 360+MAG_Heading;
//Serial.print(ToDeg(MAG_Heading));
//Serial.print(",");
// compass.heading((LSM303::vector<int>){0, -1, 0});
Serial.print(compass.heading());
Serial.println();
#endif
#if PRINT_STEPS_DIST==1
calculate();
#endif
#if PRINT_ANALOGS==1
Serial.print(",AN:");
Serial.print(AN[0]); //(int)read_adc(0)
Serial.print(",");
Serial.print(AN[1]);
Serial.print(",");
Serial.print(AN[2]);
Serial.print(",");
Serial.print(AN[3]);
Serial.print (",");
Serial.print(AN[4]);
Serial.print (",");
Serial.print(AN[5]);
Serial.print(",");
Serial.print(c_magnetom_x);
Serial.print (",");
Serial.print(c_magnetom_y);
Serial.print (",");
Serial.print(c_magnetom_z);
#endif
/*#if PRINT_DCM == 1
Serial.print (",DCM:");
Serial.print(convert_to_dec(DCM_Matrix[0][0]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[0][1]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[0][2]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[1][0]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[1][1]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[1][2]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[2][0]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[2][1]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[2][2]));
#endif*/
// Serial.println();
}