Rauno U / Mbed 2 deprecated Miisu

Six crescent shaped legs

Dependencies:   mbed

EncoderMotor.cpp@16:4161d7af6b5a, 2016-04-12 (annotated)

Committer:
sim642
Date:
Tue Apr 12 21:38:37 2016 +0000
Revision:
16:4161d7af6b5a
Parent:
14:4143ddddef1b
Child:
17:cb8ad2fc76e5
Allow EncoderMotor to work in multiple modes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sim642 3:2235787e78c4 1 #include "EncoderMotor.hpp"
sim642 3:2235787e78c4 2
sim642 4:4d54794b99a5 3 const float tickTime = 1.f / 60;
sim642 4:4d54794b99a5 4
sim642 13:dc5f5f19e4e8 5 EncoderMotor::EncoderMotor(MotorData nData, EncoderData encData, PIDData speedPIDData, PIDData turnPIDData) :
sim642 12:8de7f5d56fe4 6 Motor(nData),
sim642 12:8de7f5d56fe4 7 encoder(encData), speedSmoother(0.3f),
sim642 16:4161d7af6b5a 8 mode(NoMode),
sim642 16:4161d7af6b5a 9 speedPID(speedPIDData), setSpeed(0),
sim642 16:4161d7af6b5a 10 turnPID(turnPIDData), setTurn(0)
sim642 3:2235787e78c4 11 {
sim642 3:2235787e78c4 12
sim642 3:2235787e78c4 13 }
sim642 3:2235787e78c4 14
sim642 3:2235787e78c4 15 void EncoderMotor::setup()
sim642 3:2235787e78c4 16 {
sim642 4:4d54794b99a5 17 ticker.attach(this, &EncoderMotor::tick, tickTime);
sim642 3:2235787e78c4 18 }
sim642 3:2235787e78c4 19
sim642 16:4161d7af6b5a 20 void EncoderMotor::drive(float speed)
sim642 3:2235787e78c4 21 {
sim642 16:4161d7af6b5a 22 setSpeed = speed;
sim642 16:4161d7af6b5a 23 mode = SpeedMode;
sim642 3:2235787e78c4 24 }
sim642 3:2235787e78c4 25
sim642 16:4161d7af6b5a 26 void EncoderMotor::rotate(float turn)
sim642 5:7f800f61cb13 27 {
sim642 16:4161d7af6b5a 28 setTurn = encoder.getTurn() + turn;
sim642 16:4161d7af6b5a 29 mode = TurnMode;
sim642 5:7f800f61cb13 30 }
sim642 5:7f800f61cb13 31
sim642 3:2235787e78c4 32 void EncoderMotor::tick()
sim642 3:2235787e78c4 33 {
sim642 16:4161d7af6b5a 34 switch (mode)
sim642 16:4161d7af6b5a 35 {
sim642 16:4161d7af6b5a 36 case NoMode:
sim642 16:4161d7af6b5a 37 break;
sim642 5:7f800f61cb13 38
sim642 16:4161d7af6b5a 39 case TurnMode:
sim642 16:4161d7af6b5a 40 float errorTurn = setTurn - encoder.getTurn();
sim642 16:4161d7af6b5a 41 setSpeed = turnPID.step(errorTurn);
sim642 16:4161d7af6b5a 42 // fallthrough
sim642 16:4161d7af6b5a 43
sim642 16:4161d7af6b5a 44 case SpeedMode:
sim642 16:4161d7af6b5a 45 float errorSpeed = setSpeed - speedSmoother.smooth(encoder.getTurnSpeed());
sim642 16:4161d7af6b5a 46 Motor::drive(speedPID.step(errorSpeed));
sim642 16:4161d7af6b5a 47 break;
sim642 16:4161d7af6b5a 48 }
sim642 3:2235787e78c4 49 }