Rauno U
/
Miisu
Six crescent shaped legs
EncoderMotor.cpp
- Committer:
- sim642
- Date:
- 2016-04-14
- Revision:
- 18:1437610bea8b
- Parent:
- 17:cb8ad2fc76e5
- Child:
- 22:bfc79c6ea2fd
File content as of revision 18:1437610bea8b:
#include "EncoderMotor.hpp" #include "Math.hpp" const float tickTime = 1.f / 60; EncoderMotor::EncoderMotor(MotorData nData, EncoderData encData, PIDData speedPIDData, PIDData turnPIDData, SyncGroup *nSync) : Motor(nData), encoder(encData), speedSmoother(0.3f), mode(NoMode), speedPID(speedPIDData), setSpeed(0), turnPID(turnPIDData), setTurn(0), sync(nSync) { } void EncoderMotor::setup() { ticker.attach(this, &EncoderMotor::tick, tickTime); } void EncoderMotor::drive(float speed) { setSpeed = speed; mode = SpeedMode; } void EncoderMotor::rotate(float turn, float speedLimit) { setTurn = encoder.getTurn() + turn; turnSpeedLimit = speedLimit; mode = TurnMode; } void EncoderMotor::tick() { switch (mode) { case NoMode: break; case TurnMode: { float errorTurn = setTurn - encoder.getTurn(); setSpeed = clampAmplitude(turnPID.step(errorTurn), turnSpeedLimit); // fallthrough } case SpeedMode: { float actualSpeed = speedSmoother.smooth(encoder.getTurnSpeed()); float factor = actualSpeed / setSpeed; float desiredSpeed; if (sync != NULL) { float minFactor = sync->update(this, factor); desiredSpeed = minFactor * setSpeed; } else { desiredSpeed = setSpeed; } float errorSpeed = desiredSpeed - actualSpeed; Motor::drive(speedPID.step(errorSpeed)); break; } } }