Rauno U / Mbed 2 deprecated Miisu

Six crescent shaped legs

Dependencies:   mbed

EncoderMotor.cpp

Committer:
sim642
Date:
2016-04-12
Revision:
14:4143ddddef1b
Parent:
13:dc5f5f19e4e8
Child:
16:4161d7af6b5a

File content as of revision 14:4143ddddef1b:

#include "EncoderMotor.hpp"

const float tickTime = 1.f / 60;

EncoderMotor::EncoderMotor(MotorData nData, EncoderData encData, PIDData speedPIDData, PIDData turnPIDData) :
    Motor(nData),
    encoder(encData), speedSmoother(0.3f),
    speedPID(speedPIDData), setPower(0),
    turnPID(turnPIDData), setRot(0)
{
    
}

void EncoderMotor::setup()
{
    ticker.attach(this, &EncoderMotor::tick, tickTime);
}

void EncoderMotor::drive(float power)
{
    setPower = power;
    //Motor::drive(power);
}

void EncoderMotor::rotate(float rot)
{
    setRot += rot;
}

void EncoderMotor::tick()
{
    float errorRot = setRot - encoder.getTurn();
    drive(turnPID.step(errorRot));
    
    float error = setPower - speedSmoother.smooth(encoder.getTurnSpeed());
    out = speedPID.step(error);
    Motor::drive(out);
}