Diff: src/motor.cpp

Revision:

23:112c0be5a7f3

Child:

25:8a34b8d6cc6e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/motor.cpp	Tue Nov 19 19:07:35 2013 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "include/motor.hpp"
+
+Motor::Motor(PinName aPin, PinName fPin, PinName bPin, PinName pwmPin, PinName encA, PinName encB):aIn(aPin),Forward(fPin),Backward(bPin),pwmOut(pwmPin),encoder(encA, encB, NC, 1200, QEI::X4_ENCODING){ //15,17,18,21,25,26 and 16,19,20,22,23,24
+    
+    freq =.001;
+    voltage = 12.0;
+    mode=0;
+    
+    kp=3;
+    kd=0.04;
+    
+    pwmOut.period_us(.0005);
+    }
+
+void Motor::start(){
+    t.attach(this, &Motor::Control, freq);
+}
+
+void Motor::stop(){
+    t.detach();
+    pwmOut.write(0);
+}
+
+void Motor::zero(){
+    encoder.reset();
+}
+
+void Motor::Control(){
+    float preAngle=angle; // Storing the angle in the previous time step
+    angle = encoder.getPulses()/1200.0*6.283; // Updating new motor angle
+    speed =Motor::filterLowPass(speed,(angle-preAngle)/freq, .2); // Updating new motor angular vel
+    float mCurrent = getCurrent();
+    
+    float dCurrent;
+    switch(mode){
+        case 1:
+            dCurrent = kp*(dAngle-angle);
+        case 2:
+            dCurrent = kd*(dAngularVelocity-speed);
+        case 3:
+            dCurrent = kp*(dAngle-angle)+kd*(dAngularVelocity-speed);
+        case 0:
+            dCurrent = dTorque/3.27;
+    }    
+    
+    float duty=0;
+    if (dCurrent>0){
+        Forward=1;
+        Backward=0;
+        duty= (abs(dCurrent)*3.27+0.174*speed+10*(abs(dCurrent)-mCurrent))/voltage;
+    }else if (dCurrent<0){
+        Forward=0;
+        Backward=1;
+        duty= (abs(dCurrent)*3.27-0.174*speed+10*(abs(dCurrent)-mCurrent))/voltage;
+    }
+    
+    if (duty>0.99){
+        duty =1;
+    }
+    
+    pwmOut.write(duty);
+}
+
+float Motor::getCurrent()
+{
+    return (1/.140)*3.3 * aIn;
+}
+
+float Motor::getMotorPos()
+{
+    return encoder.getPulses();
+}
+
+float Motor::filterLowPass(float old, float currentIn, float alphar){
+    return (old+alphar*(currentIn-old));
+}
+
+void Motor::setTorque(float t){
+    mode = 0;
+    dTorque = t;
+}
+
+void Motor::setPos(float pos){
+    mode = 1;
+    dAngle=pos;
+}
+
+void Motor::setVel(float vel){
+    mode = 2;
+    dAngularVelocity = vel;
+}
+
+void Motor::setPosVel(float pos, float vel){
+    mode = 3;
+    dAngle = pos;
+    dAngularVelocity = vel;
+}
\ No newline at end of file