Diff: fader.cpp

Revision:

0:371773dd3dd1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fader.cpp	Wed May 04 15:41:13 2011 +0000
@@ -0,0 +1,134 @@
+#include "fader.h"
+#define MUSTSTOP    //some motordrivers will not change direction without a stop
+//#define GETSTAT
+
+int n =0;
+float sumprod = 0.0, sum = 0;;
+
+DigitalOut track(LED2); //debug only
+
+#ifdef USESUPPLY
+AnalogIn Vmotor(p17);
+float getVmotor() {
+    return Vmotor*13.3;    //for 3.3V referemce and 100k,33k divider
+}
+#else
+float getVmotor() {
+    return 6.0;    //assume 6V supply
+}
+#endif
+
+
+servo::servo(PinName p, PinName f, PinName r, PinName a): Motor(p, f, r, 0) {
+    const float Ts = 0.001;//sample period in seconds, 1kHz
+    deadband = 0.001; // +/- 0.5mm, noise???
+    _pwm.period_us(100); //10kHz otherwise very annoying sound
+    fb = new AnalogIn(a);
+    flt = new medianFilter(7);
+    float voltage = getVmotor(); //make the gain dependent on the supply voltage
+    float Kp = 9.0 - 0.5*voltage; //gain=6 @6V
+    pid = new PID(Kp , 0.05, 0.000002, Ts);//work well @6V motor supply
+    pid->setInputLimits(0, 1.0);
+    pid->setOutputLimits(-1.0, 1.0);
+    pid->setBias(0.0);//just to set internal feedforward variable
+    _setPoint = 0.0;
+    pid->setSetPoint(_setPoint);
+    tick.attach(this,&servo::process, Ts);
+}
+
+void servo::process() {
+    update();
+    float p = pos();//(filtered) value of the potentiometer as a value between 0.0 and 1.0
+    pid->setProcessValue(p); //set it as the value to control (Ist-wert)
+    float out = pid->compute(); //compute the motor speed
+#ifdef MUSTSTOP
+    if ((out > 0 && _out < 0) || (out < 0 && _out > 0))
+        _out = 0.0;//stop first
+    else
+        _out = out;
+#else
+    _out = out;
+#endif
+    if (fabs(p - _setPoint) < deadband) {
+        coast(); //near setpoint so disconnect motor to allow manual movement
+        coasting = true;
+    } else {
+        speed(_out);
+        coasting = false;
+    }
+}
+
+fader::fader(PinName p, PinName f, PinName r, PinName a, PinName t): servo(p,f,r,a) {
+    thres = 0.01;
+    thres2 = 0.001;
+    count = 0;
+    command = 0;
+    state = tracking;
+    if (t != NC)
+        touch = new AnalogIn(t);
+    else
+        touch = 0;
+}
+
+void fader::process() { //called by the ticker every 1 ms
+    float p;
+    switch (state) { //make sure that each branch calls either servo::process or servo::update for proper filtering
+        case tracking:
+            servo::process();
+            p = pos();
+            if (isCoasting()) { //servo is near it's setpoint, motor is off
+                state = holding;
+                lastpos = p; //save the position that was reached
+                track = 1; //debug
+            }
+            break;
+        case holding:
+            update();
+            p = pos();
+            if (fabs(lastpos - p) > thres) { //apparently position has changed (manual move)
+                state = moving;
+                printf("moving from %f to %f\n", lastpos, p);
+                lastpos = p;
+                count = 0;
+            }//if not, stay in 'holding' until next 'set' command, do not update lastpos
+            break;
+        case moving:
+            //not tracking but coasting
+            update();
+            p = pos();
+            if (fabs(lastpos - p) < thres2) {
+                count++;
+            } else {
+                count = 0;
+            }
+            if (count > 100) { //apparently movement has stopped, movement less then thres2 for 100ms
+                printf("movement stopped at %f\n", p);
+                servo::set(p); //update the servo setpoint (has no effect because state is not tracking)
+                if (command)
+                    command(p); //invoke the OnMove handler
+                state = holding; //go back to holding state to allow a new fader::set and return to tracking
+            }
+            lastpos = p;
+            break;
+        default: //cannot happen
+            update();
+            p = pos();
+    }
+    if (touch && *touch > thres) { //stub: if input fullfills some condition (touch sense on fader)
+        if (command)
+            command(p);
+    }
+#ifdef GETSTAT
+    sum += p;
+    sumprod += p*p;
+    n++;
+    if (n == 1000) {
+        float mean = sum/n;
+        float var = sumprod/n - mean*mean;
+        printf("n=%d, E=%f, Var = %f, sDev=%f\n", n, mean, var, sqrt(var));
+        n=0;
+        sum=0;
+        sumprod=0;
+    }
+#endif
+}