Revision f8cf404d components/Odometry.cpp

View differences:

components/Odometry.cpp
9 9 
#include <Matrix.h> // Matrixoperations "Matrix::*"
10 10 
#include <amiro/Constants.h> // Constants "constants::*"
11 11 
#include <chprintf.h>
12 
#include <global.hpp>
12 13 

  		




  13
  14
  
    
using namespace chibios_rt;


  		




  14
  15
  
    
using namespace amiro;


  		




  15
  16
  
    
using namespace constants::DiWheelDrive;


  		




  16
  17
  
    

  		




  
  18
  
    
extern Global global;


  		




  
  19
  
    

  		




  17
  20
  
    

  		




  18
  21
  
    
Odometry::Odometry(MotorIncrements* mi, L3G4200D* gyroscope)


  		




  19
  22
  
    
    : BaseStaticThread<1024>(),


  		




  ......




  58
  61
  
    
    robotPosition.y = this->pY * 1e6;


  		




  59
  62
  
    
    robotPosition.f_z = (int32_t(this->pPhi * 1e6) % piScaled) + ((this->pPhi < 0) ? piScaled : 0);  // Get only the postitve angel f_z in [0 .. 2 * pi]


  		




  60
  63
  
    
  chSysUnlock();


  		




  61
  
  
    
//     chprintf((BaseSequentialStream*) &SD1, "X:%d Y:%d Phi:%d", robotPosition.x,robotPosition.y, robotPosition.f_z);


  		




  62
  
  
    
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");


  		




  63
  
  
    
//     chprintf((BaseSequentialStream*) &SD1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);


  		




  64
  
  
    
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");


  		




  
  64
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "X:%d Y:%d Phi:%d", robotPosition.x,robotPosition.y, robotPosition.f_z);


  		




  
  65
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");


  		




  
  66
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);


  		




  
  67
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");


  		




  65
  68
  
    
  return robotPosition;


  		




  66
  69
  
    
}


  		




  67
  70
  
    

  		




  ......




  110
  113
  
    

  		




  111
  114
  
    
    // Calculate the odometry


  		




  112
  115
  
    
    this->updateOdometry();


  		




  113
  
  
    
		


  		




  114
  116
  
    

  		




  115
  
  
    
//     chprintf((BaseSequentialStream*) &SD1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);


  		




  116
  
  
    
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");


  		




  117
  
  
    
//     chprintf((BaseSequentialStream*) &SD1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);


  		




  118
  
  
    
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");


  		




  119
  117
  
    

  		




  120
  
  
    
	if (time >= System::getTime()) {


  		




  
  118
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);


  		




  
  119
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");


  		




  
  120
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);


  		




  
  121
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");


  		




  
  122
  
    

  		




  
  123
  
    
  if (time >= System::getTime()) {


  		




  121
  124
  
    
      chThdSleepUntil(time);


  		




  122
  125
  
    
    } else {


  		




  123
  
  
    
      chprintf((BaseSequentialStream*) &SD1, "WARNING Odometry: Unable to keep track\r\n");


  		




  124
  
  
    
	}


  		




  
  126
  
    
      chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");


  		




  
  127
  
    
  }


  		




  125
  128
  
    
  }


  		




  126
  129
  
    

  		




  127
  130
  
    
  return true;


  		




  ......




  139
  142
  
    
    Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3);


  		




  140
  143
  
    
    // Get the differentail gyro information and reset it


  		




  141
  144
  
    
    angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);


  		




  142
  
  
    
	gyro->angularReset();


  		




  
  145
  
    
  gyro->angularReset();


  		




  143
  146
  
    
  chSysUnlock();


  		




  144
  147
  
    

  		




  145
  148
  
    
  ////////////////


  		




  ......




  237
  240
  
    
  // Get the current increments of the QEI


  		




  238
  241
  
    
  MotorControl::updateIncrements(this->motorIncrements, this->increment, this->incrementDifference);


  		




  239
  242
  
    
//


  		




  240
  
  
    
//  chprintf((BaseSequentialStream*) &SD1, "\ni_right = %d \t i_left = %d", this->increment[RIGHT_WHEEL], this->increment[LEFT_WHEEL]);


  		




  241
  
  
    
//  chprintf((BaseSequentialStream*) &SD1, "\niDiff_right = %d \t iDiff_left = %d", this->incrementDifference[RIGHT_WHEEL], this->incrementDifference[LEFT_WHEEL]);


  		




  
  243
  
    
//  chprintf((BaseSequentialStream*) &global.sercanmux1, "\ni_right = %d \t i_left = %d", this->increment[RIGHT_WHEEL], this->increment[LEFT_WHEEL]);


  		




  
  244
  
    
//  chprintf((BaseSequentialStream*) &global.sercanmux1, "\niDiff_right = %d \t iDiff_left = %d", this->incrementDifference[RIGHT_WHEEL], this->incrementDifference[LEFT_WHEEL]);


  		




  242
  245
  
    

  		




  243
  246
  
    
  // Get the driven distance for each wheel


  		




  244
  247
  
    
  MotorControl::updateDistance(this->incrementDifference, this->distance);


  		




  245
  248
  
    

  		




  246
  
  
    
//  chprintf((BaseSequentialStream*) &SD1, "\nx_right = %f \t x_left = %f", this->distance[RIGHT_WHEEL], this->distance[LEFT_WHEEL]);


  		




  
  249
  
    
//  chprintf((BaseSequentialStream*) &global.sercanmux1, "\nx_right = %f \t x_left = %f", this->distance[RIGHT_WHEEL], this->distance[LEFT_WHEEL]);


  		




  247
  250
  
    
}


  		












Also available in:  Unified diff