forked from logzhan/RobotHardware-UESTC
53 lines
1.1 KiB
C++
53 lines
1.1 KiB
C++
|
#include "encoder_implement.h"
|
||
|
|
#include "board.h"
|
||
|
|
#include "print.h"
|
||
|
|
EncoderImp::EncoderImp(unsigned char _num, bool _reverse) : reverse(_reverse), num(_num)
|
||
|
|
{
|
||
|
|
clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
void EncoderImp::init()
|
||
|
|
{
|
||
|
|
Board::get()->encoder_init(num);
|
||
|
|
clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
void EncoderImp::clear()
|
||
|
|
{
|
||
|
|
total_count = 0;
|
||
|
|
pid_pos = odom_pos = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
long EncoderImp::get_total_count()
|
||
|
|
{
|
||
|
|
if (reverse)
|
||
|
|
total_count += Board::get()->getEncoderCount(num);
|
||
|
|
else
|
||
|
|
total_count -= Board::get()->getEncoderCount(num);
|
||
|
|
|
||
|
|
return total_count;
|
||
|
|
}
|
||
|
|
|
||
|
|
long EncoderImp::get_increment_count_for_dopid()
|
||
|
|
{
|
||
|
|
if (reverse)
|
||
|
|
total_count += Board::get()->getEncoderCount(num);
|
||
|
|
else
|
||
|
|
total_count -= Board::get()->getEncoderCount(num);
|
||
|
|
long l = total_count-pid_pos;
|
||
|
|
pid_pos = total_count;
|
||
|
|
return l;
|
||
|
|
}
|
||
|
|
|
||
|
|
long EncoderImp::get_increment_count_for_odom()
|
||
|
|
{
|
||
|
|
if (reverse)
|
||
|
|
total_count += Board::get()->getEncoderCount(num);
|
||
|
|
else
|
||
|
|
total_count -= Board::get()->getEncoderCount(num);
|
||
|
|
|
||
|
|
long l = total_count-odom_pos;
|
||
|
|
odom_pos = total_count;
|
||
|
|
return l;
|
||
|
|
}
|