jordy morsinkhof / MotorThrottle

werkend!

Dependencies:   PID QEI_adapted

Fork of MotorThrottle by Bram S

Motor.cpp@2:1de1be9f0ab7, 2017-10-30 (annotated)

Committer:
jordymorsinkhof
Date:
Mon Oct 30 15:51:11 2017 +0000
Revision:
2:1de1be9f0ab7
Parent:
0:aefd03ad04e6 
Changed nothing;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BramS23 0:aefd03ad04e6 1 #include "Motor.h"
BramS23 0:aefd03ad04e6 2
BramS23 0:aefd03ad04e6 3 Motor::Motor(PinName PWM, PinName Direction, PinName Enc1,PinName Enc2,PinName HomingSW,float interval):
BramS23 0:aefd03ad04e6 4 encoder(Enc1,Enc2,NC,4200),
BramS23 0:aefd03ad04e6 5 MotorThrottle(PWM),
BramS23 0:aefd03ad04e6 6 MotorDirection(Direction),
BramS23 0:aefd03ad04e6 7 HomingSwitch(HomingSW),
BramS23 0:aefd03ad04e6 8 controller(1,0,0,interval)
BramS23 0:aefd03ad04e6 9 {
BramS23 0:aefd03ad04e6 10 SetPID(1,0,0);
BramS23 0:aefd03ad04e6 11 _GearRatio=1;
jordymorsinkhof 2:1de1be9f0ab7 12 MotorThrottle=0;
jordymorsinkhof 2:1de1be9f0ab7 13 MotorDirection=0;
BramS23 0:aefd03ad04e6 14 }
BramS23 0:aefd03ad04e6 15
BramS23 0:aefd03ad04e6 16 void Motor::GotoPos(float Rad){
BramS23 0:aefd03ad04e6 17 // Enter The current values
BramS23 0:aefd03ad04e6 18 controller.setSetPoint(Rad);
BramS23 0:aefd03ad04e6 19 controller.setProcessValue(GetPos());
BramS23 0:aefd03ad04e6 20
BramS23 0:aefd03ad04e6 21 // Compute controller output
BramS23 0:aefd03ad04e6 22 float OutPut=controller.compute();
BramS23 0:aefd03ad04e6 23
BramS23 0:aefd03ad04e6 24 // Direction handling
BramS23 0:aefd03ad04e6 25 float Direction=0;
BramS23 0:aefd03ad04e6 26
BramS23 0:aefd03ad04e6 27 if(OutPut<0){
BramS23 0:aefd03ad04e6 28 Direction=1;
BramS23 0:aefd03ad04e6 29 }
BramS23 0:aefd03ad04e6 30
BramS23 0:aefd03ad04e6 31 OutPut=fabs(OutPut);
BramS23 0:aefd03ad04e6 32
BramS23 0:aefd03ad04e6 33 // Output schrijven
BramS23 0:aefd03ad04e6 34 MotorThrottle.write(OutPut);
BramS23 0:aefd03ad04e6 35 MotorDirection=Direction;
BramS23 0:aefd03ad04e6 36 }
BramS23 0:aefd03ad04e6 37
BramS23 0:aefd03ad04e6 38 float Motor::GetPos(){
BramS23 0:aefd03ad04e6 39 return (encoder.getPulses()/4200.0f)*(2.0f*3.1415f)/_GearRatio;
BramS23 0:aefd03ad04e6 40 }
BramS23 0:aefd03ad04e6 41
BramS23 0:aefd03ad04e6 42
BramS23 0:aefd03ad04e6 43 void Motor::Homing(bool direction, float PWM,float HomingPos){
BramS23 0:aefd03ad04e6 44 HomingSwitch.mode(PullUp);
BramS23 0:aefd03ad04e6 45 MotorThrottle=PWM;
BramS23 0:aefd03ad04e6 46 MotorDirection=direction;
BramS23 0:aefd03ad04e6 47 while (HomingSwitch.read()==1){
BramS23 0:aefd03ad04e6 48 }
BramS23 0:aefd03ad04e6 49 MotorDirection=0;
BramS23 0:aefd03ad04e6 50 MotorThrottle=0.0f;
BramS23 0:aefd03ad04e6 51 encoder.reset((int)4200.0f*_GearRatio*(HomingPos/2.0f/3.1415));
jordymorsinkhof 2:1de1be9f0ab7 52 Stop();
BramS23 0:aefd03ad04e6 53 }
BramS23 0:aefd03ad04e6 54
BramS23 0:aefd03ad04e6 55 void Motor::SetPID(float P,float Ti,float Td){
BramS23 0:aefd03ad04e6 56 controller.setTunings(P,Ti, Td);
BramS23 0:aefd03ad04e6 57 }
BramS23 0:aefd03ad04e6 58
BramS23 0:aefd03ad04e6 59 void Motor::SetGearRatio(float GearRatio){
BramS23 0:aefd03ad04e6 60 _GearRatio=GearRatio;
BramS23 0:aefd03ad04e6 61 }
BramS23 0:aefd03ad04e6 62
BramS23 0:aefd03ad04e6 63 void Motor::SetInputLimits(float Imin, float Imax){
BramS23 0:aefd03ad04e6 64 controller.setInputLimits(Imin,Imax);
BramS23 0:aefd03ad04e6 65 }
BramS23 0:aefd03ad04e6 66
BramS23 0:aefd03ad04e6 67 void Motor::SetOutputLimits(float Omin, float Omax){
jordymorsinkhof 2:1de1be9f0ab7 68 controller.setOutputLimits(Omin,Omax);
BramS23 0:aefd03ad04e6 69 }
BramS23 0:aefd03ad04e6 70
BramS23 0:aefd03ad04e6 71 void Motor::Stop(){
BramS23 0:aefd03ad04e6 72 MotorThrottle=0;
BramS23 0:aefd03ad04e6 73 MotorDirection=0;
BramS23 0:aefd03ad04e6 74 }
BramS23 0:aefd03ad04e6 75
BramS23 0:aefd03ad04e6 76
BramS23 0:aefd03ad04e6 77
BramS23 0:aefd03ad04e6 78
BramS23 0:aefd03ad04e6 79
BramS23 0:aefd03ad04e6 80
BramS23 0:aefd03ad04e6 81
BramS23 0:aefd03ad04e6 82