Diff: Motor.cpp

Revision:

0:aefd03ad04e6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp	Mon Oct 30 13:50:58 2017 +0000
@@ -0,0 +1,79 @@
+#include "Motor.h"
+
+Motor::Motor(PinName PWM, PinName Direction, PinName Enc1,PinName Enc2,PinName HomingSW,float interval):
+    encoder(Enc1,Enc2,NC,4200),
+    MotorThrottle(PWM),
+    MotorDirection(Direction),
+    HomingSwitch(HomingSW),
+    controller(1,0,0,interval)
+{
+    SetPID(1,0,0);
+    _GearRatio=1;
+}
+
+void Motor::GotoPos(float Rad){
+    // Enter The current values
+    controller.setSetPoint(Rad);
+    controller.setProcessValue(GetPos());
+    
+    // Compute controller output
+    float OutPut=controller.compute();
+    
+    // Direction handling
+    float Direction=0;
+    
+    if(OutPut<0){
+        Direction=1;
+    }
+    
+    OutPut=fabs(OutPut);
+    
+    // Output schrijven
+    MotorThrottle.write(OutPut);
+    MotorDirection=Direction;
+}
+
+float Motor::GetPos(){
+    return (encoder.getPulses()/4200.0f)*(2.0f*3.1415f)/_GearRatio;
+}
+
+
+void Motor::Homing(bool direction, float PWM,float HomingPos){
+    HomingSwitch.mode(PullUp);
+    MotorThrottle=PWM;
+    MotorDirection=direction;
+    while (HomingSwitch.read()==1){
+    }
+    MotorDirection=0;
+    MotorThrottle=0.0f;
+    encoder.reset((int)4200.0f*_GearRatio*(HomingPos/2.0f/3.1415));
+}
+
+void Motor::SetPID(float P,float Ti,float Td){
+    controller.setTunings(P,Ti, Td);
+}
+
+void Motor::SetGearRatio(float GearRatio){
+    _GearRatio=GearRatio;
+}
+
+void Motor::SetInputLimits(float Imin, float Imax){
+    controller.setInputLimits(Imin,Imax);
+}
+
+void Motor::SetOutputLimits(float Omin, float Omax){
+    controller.setInputLimits(Omin,Omax);
+}
+
+void Motor::Stop(){
+    MotorThrottle=0;
+    MotorDirection=0;
+}
+    
+
+
+
+
+
+
+