Exemplo n.º 1
0
/**
 * @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();
}
Exemplo n.º 2
0
/**
 * @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();
}
void Accelerometer::setup(){
	Wire.begin();
	compass.init();
	compass.enableDefault();

	memset(lastValuesX,0,sizeof(lastValuesX));
	memset(lastValuesY,0,sizeof(lastValuesY));
	memset(lastValuesZ,0,sizeof(lastValuesZ));
	index =0;

}
Exemplo n.º 4
0
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);
	}
	
}
Exemplo n.º 5
0
bool Hal::initHal()
{
  debugPrintLn("Start init");
  // initialize all the hardware
  initLora();
  initSwitch();
  initLTC();
  compass.init();
  compass.enableDefault();
  htu.begin();
//  setAckOn();
  debugPrintLn("Einde init");
}
Exemplo n.º 6
0
void Accel_Init()
{
  compass.init();
  if (compass.getDeviceType() == LSM303DLHC_DEVICE)
  {
    compass.writeAccReg(LSM303_CTRL_REG1_A, 0x47); // normal power mode, all axes enabled, 50 Hz
    compass.writeAccReg(LSM303_CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10 on DLHC; high resolution output mode
  }
  else 
  {
    compass.writeAccReg(LSM303_CTRL_REG1_A, 0x27); // normal power mode, all axes enabled, 50 Hz
    compass.writeAccReg(LSM303_CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11 on DLH, DLM
  }
}
Exemplo n.º 7
0
void task_accelerometer(void* p){
	
	
	/*accelerometer code*/
	while(1)
	{
		
		compass.read();
		//snprintf(report, sizeof(report), "A: %6d %6d %6d    M: %6d %6d %6d",
		//compass.a.x, compass.a.y, compass.a.z,
		//compass.m.x, compass.m.y, compass.m.z);
		//Serial.println(report);
		//dprintf( "A: %6d %6d %6d    M: %6d %6d %6d",
		//compass.a.x, compass.a.y, compass.a.z,
		//compass.m.x, compass.m.y, compass.m.z);
		//dprintf("%d",compass.a.x);
		//dprintf("%d",compass.a.x);
		//Serial.print(compass.a.x);
	//	Serial.print("1");
		//delay(1000);
		vTaskDelay(taskDelay);
	}
	/*accelerometer code*/
	
}
Exemplo n.º 8
0
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");
}
void Accelerometer::update(){
	/*
  return
  */
	compass.read();

	lastValuesX[index] = compass.a.x;
	lastValuesY[index] = compass.a.y;
	lastValuesZ[index] = compass.a.z;

	long averagedX = averageArray(lastValuesX);
	long averagedY = averageArray(lastValuesY);
	long averagedZ = averageArray(lastValuesZ);

	index = index + 1;
	index = index % SENSITIVITY;

	long averageXY = sqrt(averagedX*averagedX + averagedY*averagedY);
	
       /*Serial.print(averagedX);
       Serial.print("      ");
       Serial.print(averagedY);
       Serial.print("      ");
       Serial.print(averageXY);
       Serial.print("      ");*/
	//int degree = atan2(averagedX, averagedY) * 180.0 / M_PI;
	//Serial.println(degree);

	if(averageXY > 3000)
	{
		Serial.println("SMASH!");
	}
}
Exemplo n.º 10
0
void Read_Compass()
{
  compass.readMag();
  magnetom_x = SENSOR_SIGN[6] * compass.m.x;
  magnetom_y = SENSOR_SIGN[7] * compass.m.y;
  magnetom_z = SENSOR_SIGN[8] * compass.m.z;
}
Exemplo n.º 11
0
void Compass_Calibrate() {
  long timer=millis();
  LSM303::vector running_min = {2047, 2047, 2047}, running_max = {-2048, -2048, -2048};   
  while((millis()-timer)<=60000){ //calibrate for 1 minutes
 
    compass.read();
    
    running_min.x = min(running_min.x, compass.m.x);
    running_min.y = min(running_min.y, compass.m.y);
    running_min.z = min(running_min.z, compass.m.z);
  
    running_max.x = max(running_max.x, compass.m.x);
    running_max.y = max(running_max.y, compass.m.y);
    running_max.z = max(running_max.z, compass.m.z);
    
    delay(100);
  }
  compass.m_max.x = running_max.x;
  compass.m_max.y = running_max.y;
  compass.m_max.z = running_max.z;
  compass.m_min.x = running_min.x;
  compass.m_min.y = running_min.y;
  compass.m_min.z = running_min.z;
  

}
Exemplo n.º 12
0
void loop() {  
  compass.read();
  
  running_min.x = min(running_min.x, compass.m.x);
  running_min.y = min(running_min.y, compass.m.y);
  running_min.z = min(running_min.z, compass.m.z);

  running_max.x = max(running_max.x, compass.m.x);
  running_max.y = max(running_max.y, compass.m.y);
  running_max.z = max(running_max.z, compass.m.z);
  
  Serial.print("M min ");
  Serial.print("X: ");
  Serial.print((int)running_min.x);
  Serial.print(" Y: ");
  Serial.print((int)running_min.y);
  Serial.print(" Z: ");
  Serial.print((int)running_min.z);

  Serial.print(" M max ");  
  Serial.print("X: ");
  Serial.print((int)running_max.x);
  Serial.print(" Y: ");
  Serial.print((int)running_max.y);
  Serial.print(" Z: ");
  Serial.println((int)running_max.z);
  
  delay(100);
}
Exemplo n.º 13
0
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};
}
Exemplo n.º 14
0
// Reads x,y and z accelerometer registers
void Read_Accel()
{
  compass.readAcc();
  
  AN[3] = compass.a.x;
  AN[4] = compass.a.y;
  AN[5] = compass.a.z;
  accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
  accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
  accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
}
Exemplo n.º 15
0
// Reads x,y and z accelerometer registers
void Read_Accel()
{
	compass.readAcc();
	
	AN[3] = compass.a.x >> 4; // shift left 4 bits to use 12-bit representation (1 g = 256)
	AN[4] = compass.a.y >> 4;
	AN[5] = compass.a.z >> 4;
	accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
	accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
	accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
}
Exemplo n.º 16
0
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");

}
Exemplo n.º 17
0
int8_t Hal::LSM303_Update()
{
  int offset = 700;
  compass.read();

  d_x = abs(compass.m.x - ax_o);
  d_y = abs(compass.m.y - ay_o);
  d_z = abs(compass.m.z - az_o);

  if ( d_x > offset || d_y > offset || d_z > offset) 
  {
    hasMoved = true;
  }
  ax_o = compass.m.x;
  ay_o = compass.m.y;
  az_o = compass.m.z;
}
Exemplo n.º 18
0
void Accel_Init()
{
	compass.init();
	compass.enableDefault();
	switch (compass.getDeviceType())
	{
		case LSM303::device_D:
		compass.writeReg(LSM303::CTRL2, 0x18); // 8 g full scale: AFS = 011
		break;
		case LSM303::device_DLHC:
		compass.writeReg(LSM303::CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10; high resolution output mode
		break;
		default: // DLM, DLH
		compass.writeReg(LSM303::CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11
	}
	//compass.heading((LSM303::vector<int>){0, 0, -1});
	//compass.m_min = (LSM303::vector<int16_t>){-2304, -2194, -2156};
	//compass.m_max = (LSM303::vector<int16_t>){+2771, +2860, +2753};
}
Exemplo n.º 19
0
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");
}
Exemplo n.º 20
0
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();    
      
}
Exemplo n.º 21
0
void Compass_Init()
{
 compass.writeMagReg(LSM303_MR_REG_M, 0x00); // continuous conversion mode
  // 15 Hz default
}
Exemplo n.º 22
0
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);
	}
}
Exemplo n.º 23
0
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);
  }
}
Exemplo n.º 24
-1
void setup() {
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
  compass.setMagGain(LSM303::magGain_47);
}