void Recorder()
{
while (YesNo("Record next"))
{
if (iRecNext >= 100) { printf("Buffer Full\n"); return;}
xRec[iRecNext] = read_encoder(0);
yRec[iRecNext] = read_encoder(1);
iRecNext++;
}
}
// btnState = 0 = pressed,=1=released
// return value = encoder count
int8_t RotaryEncoder::Read(int8_t *btnState=NULL)
{
int8_t encval,btnval;
/*encoder*/
encval = read_encoder(&btnval);
if( encval ) {
m_count += encval;
}
/* encoder button */
if (btnval != m_lastbtnval) {
m_lastDebounceTime = millis();
*btnState = m_lastbtnstate;
}
else if ((millis()-m_lastDebounceTime) > DEBOUNCE_DELAY) {
*btnState = btnval;
m_lastbtnstate = btnval;
}
else {
*btnState = m_lastbtnstate;
}
m_lastbtnval = btnval;
return m_count;
}
void ReadEncoders()
{
int i;
cLLast = cL;
cRLast = cR;
cL = read_encoder(0);
cR = read_encoder(1);
/* Stalled motors */
if (cL == cLLast && cR == cRLast)
{
if (!fStalling)
msStall = mseconds() + 250L;
fStalling = 1;
}
else
fStalling = 0;
if (fStalling && mseconds() >= msStall)
{
printf("Stall!\n");
beep();beep();
fStalled = 1;
}
/* Read bump sensors and do block detection */
fBlocked = 0;
for (i = 0; i < 4; i++)
{
rgfBump[i] = digital(i+12);
if (rgfBump[i] && sgn(rgpwr[rgibumpMotor[i]]) == rgibumpSgn[i])
{
fBlocked = 1;
rgfBlock[i] = 1;
}
else
rgfBlock[i] = 0;
}
if (digital(10))
fBall = 1;
}
void calibrate_encoder(){
controll_motor(-80); //Find left boundary
delay(3000);
reset_encoder();
controll_motor(80); //Find right boundary
delay(3000);
max_encoder_val = read_encoder();
controll_motor(0);
}
float Dynamixel::getAngle(){
if(initialized_){
int counts =65532;
int reads=0;
while( (counts>countsPerRevolution_) || (counts<0) ){
counts =read_encoder(ftHandleDYNA_, motorNumber_);
reads++;
if (reads>100){
emit failedToRead();
return -1;
}
}
float angle= counts/countsPerRevolution_*360;
qDebug()<<"!!\t read: "<<reads<<"\t counts"<<counts<<" angle"<<angle;
return angle;
}
return -1;
}
double get_position(){
return (double)read_encoder()/max_encoder_val*100;
}
