Revision 1:c97f8bcd6c0f, committed 2013-08-31

Comitter:

Reiko

Date:

Sat Aug 31 15:58:46 2013 +0000

Parent:

0:5cafacc2a607

Child:

2:3faf5dcde08f

Commit message:

Added proper speed changing methods, added initial PI controller

Changed in this revision

--- a/motor.cpp	Mon Jul 29 20:25:33 2013 +0000
+++ b/motor.cpp	Sat Aug 31 15:58:46 2013 +0000
@@ -0,0 +1,69 @@
+#include "motor.h"
+
+Motor::Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dir1Pin, unsigned int dir2Pin, PinName encA, PinName encB)
+    : pwm(PWMpin), extIO(ioExt), dir1(dir1Pin), dir2(dir2Pin), qed(encA, encB) {
+ 
+    PwmOut pwm(PWMpin);
+    pwm.period_ms(5);
+    setPoint = 0;
+    pMulti = 0.2;
+    iMulti = 0.01;
+    error = 0;
+    prevError = 0;
+    P = 0.0;
+    I = 0.0;
+    
+    currentSpeed = 0;
+}
+
+void Motor::setPWM(float newPWM) {
+    if (newPWM < 0) {
+        pwm = -1 * newPWM;
+        extIO->setPin(dir1);
+        extIO->clearPin(dir2);
+    } else {
+        pwm = newPWM;
+        extIO->clearPin(dir1);
+        extIO->setPin(dir2);
+    }    
+}
+
+int Motor::getSpeed() {
+    return currentSpeed;
+}
+
+int Motor::getDecoderCount() {
+    currentSpeed = qed.read();
+    return currentSpeed;
+}
+
+void Motor::setSpeed(int newSpeed) {
+    setPoint = newSpeed;
+}
+
+void Motor::pid() {
+    /*int setPoint = 0;
+    float pMulti = 0.1;
+    float iMulti = 0.001;
+    int error = 0;
+    int prevError = 0;
+    float P = 0.0;
+    float I = 0.0;*/
+    
+    prevError = error;
+    error = setPoint - getDecoderCount();
+    
+    float err = (float)error / 250.0;
+    
+    I += err * iMulti;
+    //constrain integral
+    if (I < -1.0) I = -1.0;
+    if (I > 1.0) I = 1.0;
+    
+    float newPWMvalue = err * pMulti + I;
+    //constrain pwm
+    if (newPWMvalue < -1.0) newPWMvalue = -1.0;
+    if (newPWMvalue > 1.0) newPWMvalue = 1.0;
+    
+    setPWM(newPWMvalue);
+}
\ No newline at end of file

--- a/motor.h	Mon Jul 29 20:25:33 2013 +0000
+++ b/motor.h	Sat Aug 31 15:58:46 2013 +0000
@@ -2,24 +2,37 @@
 #define MOTOR_H
 
 #include "mbed.h"
+#include "PCA9555.h"
+#include "qed.h"
  
 class Motor {
 public:
-    Motor(PinName PWMpin, PinName dir1Pin, PinName dir2Pin);
+    Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dir1Pin, unsigned int dir2Pin, PinName encA, PinName encB);
  
+    void setPWM(float newPWM);
     void setSpeed(int newSpeed);
     int getSpeed();
+    void pid();
  
 private:
-    void pid();
-
-    InterruptIn interruptA;
-    InterruptIn interruptB;
+    PwmOut pwm;
+    PCA9555 *extIO;
+    unsigned int dir1;
+    unsigned int dir2;
+    QED qed;
+    
+    int currentSpeed;
+    int getDecoderCount();
     
-    volatile int pulses;
+    //void pid();
     
-    int prevState;
-    int currState;
+    int setPoint;
+    float pMulti;
+    float iMulti;
+    int error;
+    int prevError;
+    float P;
+    float I;
 };
 
 #endif
\ No newline at end of file