jordy morsinkhof / MotorThrottle

werkend!

Dependencies:   PID QEI_adapted

Fork of MotorThrottle by Bram S

Motor.cpp@0:aefd03ad04e6, 2017-10-30 (annotated)

Committer:
BramS23
Date:
Mon Oct 30 13:50:58 2017 +0000
Revision:
0:aefd03ad04e6
Child:
2:1de1be9f0ab7
Changed Homing

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;
BramS23 0:aefd03ad04e6 12 }
BramS23 0:aefd03ad04e6 13
BramS23 0:aefd03ad04e6 14 void Motor::GotoPos(float Rad){
BramS23 0:aefd03ad04e6 15 // Enter The current values
BramS23 0:aefd03ad04e6 16 controller.setSetPoint(Rad);
BramS23 0:aefd03ad04e6 17 controller.setProcessValue(GetPos());
BramS23 0:aefd03ad04e6 18
BramS23 0:aefd03ad04e6 19 // Compute controller output
BramS23 0:aefd03ad04e6 20 float OutPut=controller.compute();
BramS23 0:aefd03ad04e6 21
BramS23 0:aefd03ad04e6 22 // Direction handling
BramS23 0:aefd03ad04e6 23 float Direction=0;
BramS23 0:aefd03ad04e6 24
BramS23 0:aefd03ad04e6 25 if(OutPut<0){
BramS23 0:aefd03ad04e6 26 Direction=1;
BramS23 0:aefd03ad04e6 27 }
BramS23 0:aefd03ad04e6 28
BramS23 0:aefd03ad04e6 29 OutPut=fabs(OutPut);
BramS23 0:aefd03ad04e6 30
BramS23 0:aefd03ad04e6 31 // Output schrijven
BramS23 0:aefd03ad04e6 32 MotorThrottle.write(OutPut);
BramS23 0:aefd03ad04e6 33 MotorDirection=Direction;
BramS23 0:aefd03ad04e6 34 }
BramS23 0:aefd03ad04e6 35
BramS23 0:aefd03ad04e6 36 float Motor::GetPos(){
BramS23 0:aefd03ad04e6 37 return (encoder.getPulses()/4200.0f)*(2.0f*3.1415f)/_GearRatio;
BramS23 0:aefd03ad04e6 38 }
BramS23 0:aefd03ad04e6 39
BramS23 0:aefd03ad04e6 40
BramS23 0:aefd03ad04e6 41 void Motor::Homing(bool direction, float PWM,float HomingPos){
BramS23 0:aefd03ad04e6 42 HomingSwitch.mode(PullUp);
BramS23 0:aefd03ad04e6 43 MotorThrottle=PWM;
BramS23 0:aefd03ad04e6 44 MotorDirection=direction;
BramS23 0:aefd03ad04e6 45 while (HomingSwitch.read()==1){
BramS23 0:aefd03ad04e6 46 }
BramS23 0:aefd03ad04e6 47 MotorDirection=0;
BramS23 0:aefd03ad04e6 48 MotorThrottle=0.0f;
BramS23 0:aefd03ad04e6 49 encoder.reset((int)4200.0f*_GearRatio*(HomingPos/2.0f/3.1415));
BramS23 0:aefd03ad04e6 50 }
BramS23 0:aefd03ad04e6 51
BramS23 0:aefd03ad04e6 52 void Motor::SetPID(float P,float Ti,float Td){
BramS23 0:aefd03ad04e6 53 controller.setTunings(P,Ti, Td);
BramS23 0:aefd03ad04e6 54 }
BramS23 0:aefd03ad04e6 55
BramS23 0:aefd03ad04e6 56 void Motor::SetGearRatio(float GearRatio){
BramS23 0:aefd03ad04e6 57 _GearRatio=GearRatio;
BramS23 0:aefd03ad04e6 58 }
BramS23 0:aefd03ad04e6 59
BramS23 0:aefd03ad04e6 60 void Motor::SetInputLimits(float Imin, float Imax){
BramS23 0:aefd03ad04e6 61 controller.setInputLimits(Imin,Imax);
BramS23 0:aefd03ad04e6 62 }
BramS23 0:aefd03ad04e6 63
BramS23 0:aefd03ad04e6 64 void Motor::SetOutputLimits(float Omin, float Omax){
BramS23 0:aefd03ad04e6 65 controller.setInputLimits(Omin,Omax);
BramS23 0:aefd03ad04e6 66 }
BramS23 0:aefd03ad04e6 67
BramS23 0:aefd03ad04e6 68 void Motor::Stop(){
BramS23 0:aefd03ad04e6 69 MotorThrottle=0;
BramS23 0:aefd03ad04e6 70 MotorDirection=0;
BramS23 0:aefd03ad04e6 71 }
BramS23 0:aefd03ad04e6 72
BramS23 0:aefd03ad04e6 73
BramS23 0:aefd03ad04e6 74
BramS23 0:aefd03ad04e6 75
BramS23 0:aefd03ad04e6 76
BramS23 0:aefd03ad04e6 77
BramS23 0:aefd03ad04e6 78
BramS23 0:aefd03ad04e6 79