Rauno U / Mbed 2 deprecated Miisu

Six crescent shaped legs

Dependencies:   mbed

EncoderMotor.cpp@47:4f418a4b0051, 2016-06-21 (annotated)

Committer:
sim642
Date:
Tue Jun 21 14:43:44 2016 +0000
Revision:
47:4f418a4b0051
Parent:
37:8021b3ce241a 
Byte based communication

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sim642 3:2235787e78c4 1 #include "EncoderMotor.hpp"
sim642 17:cb8ad2fc76e5 2 #include "Math.hpp"
sim642 3:2235787e78c4 3
sim642 18:1437610bea8b 4 EncoderMotor::EncoderMotor(MotorData nData, EncoderData encData, PIDData speedPIDData, PIDData turnPIDData, SyncGroup *nSync) :
sim642 12:8de7f5d56fe4 5 Motor(nData),
sim642 25:a8bb69e99d6b 6 encoder(encData), speedSmoother(0.3f),
sim642 16:4161d7af6b5a 7 mode(NoMode),
sim642 16:4161d7af6b5a 8 speedPID(speedPIDData), setSpeed(0),
sim642 18:1437610bea8b 9 turnPID(turnPIDData), setTurn(0),
sim642 18:1437610bea8b 10 sync(nSync)
sim642 3:2235787e78c4 11 {
sim642 3:2235787e78c4 12
sim642 3:2235787e78c4 13 }
sim642 3:2235787e78c4 14
sim642 16:4161d7af6b5a 15 void EncoderMotor::drive(float speed)
sim642 3:2235787e78c4 16 {
sim642 16:4161d7af6b5a 17 setSpeed = speed;
sim642 16:4161d7af6b5a 18 mode = SpeedMode;
sim642 3:2235787e78c4 19 }
sim642 3:2235787e78c4 20
sim642 17:cb8ad2fc76e5 21 void EncoderMotor::rotate(float turn, float speedLimit)
sim642 5:7f800f61cb13 22 {
sim642 37:8021b3ce241a 23 //setTurn = encoder.getTurn() + turn;
sim642 37:8021b3ce241a 24 setTurn += turn;
sim642 17:cb8ad2fc76e5 25 turnSpeedLimit = speedLimit;
sim642 16:4161d7af6b5a 26 mode = TurnMode;
sim642 5:7f800f61cb13 27 }
sim642 5:7f800f61cb13 28
sim642 37:8021b3ce241a 29 SpeedEncoder& EncoderMotor::getEncoder()
sim642 22:bfc79c6ea2fd 30 {
sim642 22:bfc79c6ea2fd 31 return encoder;
sim642 22:bfc79c6ea2fd 32 }
sim642 22:bfc79c6ea2fd 33
sim642 23:d844cc906b66 34 float EncoderMotor::getSetTurn() const
sim642 23:d844cc906b66 35 {
sim642 23:d844cc906b66 36 return setTurn;
sim642 23:d844cc906b66 37 }
sim642 23:d844cc906b66 38
sim642 23:d844cc906b66 39 float EncoderMotor::getSetSpeed() const
sim642 23:d844cc906b66 40 {
sim642 23:d844cc906b66 41 return setSpeed;
sim642 23:d844cc906b66 42 }
sim642 23:d844cc906b66 43
sim642 3:2235787e78c4 44 void EncoderMotor::tick()
sim642 3:2235787e78c4 45 {
sim642 16:4161d7af6b5a 46 switch (mode)
sim642 16:4161d7af6b5a 47 {
sim642 16:4161d7af6b5a 48 case NoMode:
sim642 16:4161d7af6b5a 49 break;
sim642 5:7f800f61cb13 50
sim642 16:4161d7af6b5a 51 case TurnMode:
sim642 17:cb8ad2fc76e5 52 {
phairero 36:906323851366 53 errorTurn = setTurn - encoder.getTurn();
sim642 17:cb8ad2fc76e5 54 setSpeed = clampAmplitude(turnPID.step(errorTurn), turnSpeedLimit);
sim642 16:4161d7af6b5a 55 // fallthrough
sim642 17:cb8ad2fc76e5 56 }
sim642 16:4161d7af6b5a 57
sim642 16:4161d7af6b5a 58 case SpeedMode:
sim642 17:cb8ad2fc76e5 59 {
sim642 18:1437610bea8b 60 float actualSpeed = speedSmoother.smooth(encoder.getTurnSpeed());
sim642 18:1437610bea8b 61 float factor = actualSpeed / setSpeed;
sim642 18:1437610bea8b 62
sim642 18:1437610bea8b 63 float desiredSpeed;
sim642 18:1437610bea8b 64 if (sync != NULL)
sim642 18:1437610bea8b 65 {
sim642 18:1437610bea8b 66 float minFactor = sync->update(this, factor);
sim642 18:1437610bea8b 67 desiredSpeed = minFactor * setSpeed;
sim642 18:1437610bea8b 68 }
sim642 18:1437610bea8b 69 else
sim642 18:1437610bea8b 70 {
sim642 18:1437610bea8b 71 desiredSpeed = setSpeed;
sim642 18:1437610bea8b 72 }
sim642 18:1437610bea8b 73
sim642 18:1437610bea8b 74 float errorSpeed = desiredSpeed - actualSpeed;
sim642 23:d844cc906b66 75 float out = speedPID.step(errorSpeed);
sim642 23:d844cc906b66 76 Motor::drive(out);
sim642 16:4161d7af6b5a 77 break;
sim642 16:4161d7af6b5a 78 }
sim642 17:cb8ad2fc76e5 79 }
sim642 3:2235787e78c4 80 }