The iPod controller that I submitted for the mbed challenge
Dependencies: mbed Motordriver PID
fader.cpp@0:371773dd3dd1, 2011-05-04 (annotated)
- Committer:
- networker
- Date:
- Wed May 04 15:41:13 2011 +0000
- Revision:
- 0:371773dd3dd1
first publication
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
networker | 0:371773dd3dd1 | 1 | #include "fader.h" |
networker | 0:371773dd3dd1 | 2 | #define MUSTSTOP //some motordrivers will not change direction without a stop |
networker | 0:371773dd3dd1 | 3 | //#define GETSTAT |
networker | 0:371773dd3dd1 | 4 | |
networker | 0:371773dd3dd1 | 5 | int n =0; |
networker | 0:371773dd3dd1 | 6 | float sumprod = 0.0, sum = 0;; |
networker | 0:371773dd3dd1 | 7 | |
networker | 0:371773dd3dd1 | 8 | DigitalOut track(LED2); //debug only |
networker | 0:371773dd3dd1 | 9 | |
networker | 0:371773dd3dd1 | 10 | #ifdef USESUPPLY |
networker | 0:371773dd3dd1 | 11 | AnalogIn Vmotor(p17); |
networker | 0:371773dd3dd1 | 12 | float getVmotor() { |
networker | 0:371773dd3dd1 | 13 | return Vmotor*13.3; //for 3.3V referemce and 100k,33k divider |
networker | 0:371773dd3dd1 | 14 | } |
networker | 0:371773dd3dd1 | 15 | #else |
networker | 0:371773dd3dd1 | 16 | float getVmotor() { |
networker | 0:371773dd3dd1 | 17 | return 6.0; //assume 6V supply |
networker | 0:371773dd3dd1 | 18 | } |
networker | 0:371773dd3dd1 | 19 | #endif |
networker | 0:371773dd3dd1 | 20 | |
networker | 0:371773dd3dd1 | 21 | |
networker | 0:371773dd3dd1 | 22 | servo::servo(PinName p, PinName f, PinName r, PinName a): Motor(p, f, r, 0) { |
networker | 0:371773dd3dd1 | 23 | const float Ts = 0.001;//sample period in seconds, 1kHz |
networker | 0:371773dd3dd1 | 24 | deadband = 0.001; // +/- 0.5mm, noise??? |
networker | 0:371773dd3dd1 | 25 | _pwm.period_us(100); //10kHz otherwise very annoying sound |
networker | 0:371773dd3dd1 | 26 | fb = new AnalogIn(a); |
networker | 0:371773dd3dd1 | 27 | flt = new medianFilter(7); |
networker | 0:371773dd3dd1 | 28 | float voltage = getVmotor(); //make the gain dependent on the supply voltage |
networker | 0:371773dd3dd1 | 29 | float Kp = 9.0 - 0.5*voltage; //gain=6 @6V |
networker | 0:371773dd3dd1 | 30 | pid = new PID(Kp , 0.05, 0.000002, Ts);//work well @6V motor supply |
networker | 0:371773dd3dd1 | 31 | pid->setInputLimits(0, 1.0); |
networker | 0:371773dd3dd1 | 32 | pid->setOutputLimits(-1.0, 1.0); |
networker | 0:371773dd3dd1 | 33 | pid->setBias(0.0);//just to set internal feedforward variable |
networker | 0:371773dd3dd1 | 34 | _setPoint = 0.0; |
networker | 0:371773dd3dd1 | 35 | pid->setSetPoint(_setPoint); |
networker | 0:371773dd3dd1 | 36 | tick.attach(this,&servo::process, Ts); |
networker | 0:371773dd3dd1 | 37 | } |
networker | 0:371773dd3dd1 | 38 | |
networker | 0:371773dd3dd1 | 39 | void servo::process() { |
networker | 0:371773dd3dd1 | 40 | update(); |
networker | 0:371773dd3dd1 | 41 | float p = pos();//(filtered) value of the potentiometer as a value between 0.0 and 1.0 |
networker | 0:371773dd3dd1 | 42 | pid->setProcessValue(p); //set it as the value to control (Ist-wert) |
networker | 0:371773dd3dd1 | 43 | float out = pid->compute(); //compute the motor speed |
networker | 0:371773dd3dd1 | 44 | #ifdef MUSTSTOP |
networker | 0:371773dd3dd1 | 45 | if ((out > 0 && _out < 0) || (out < 0 && _out > 0)) |
networker | 0:371773dd3dd1 | 46 | _out = 0.0;//stop first |
networker | 0:371773dd3dd1 | 47 | else |
networker | 0:371773dd3dd1 | 48 | _out = out; |
networker | 0:371773dd3dd1 | 49 | #else |
networker | 0:371773dd3dd1 | 50 | _out = out; |
networker | 0:371773dd3dd1 | 51 | #endif |
networker | 0:371773dd3dd1 | 52 | if (fabs(p - _setPoint) < deadband) { |
networker | 0:371773dd3dd1 | 53 | coast(); //near setpoint so disconnect motor to allow manual movement |
networker | 0:371773dd3dd1 | 54 | coasting = true; |
networker | 0:371773dd3dd1 | 55 | } else { |
networker | 0:371773dd3dd1 | 56 | speed(_out); |
networker | 0:371773dd3dd1 | 57 | coasting = false; |
networker | 0:371773dd3dd1 | 58 | } |
networker | 0:371773dd3dd1 | 59 | } |
networker | 0:371773dd3dd1 | 60 | |
networker | 0:371773dd3dd1 | 61 | fader::fader(PinName p, PinName f, PinName r, PinName a, PinName t): servo(p,f,r,a) { |
networker | 0:371773dd3dd1 | 62 | thres = 0.01; |
networker | 0:371773dd3dd1 | 63 | thres2 = 0.001; |
networker | 0:371773dd3dd1 | 64 | count = 0; |
networker | 0:371773dd3dd1 | 65 | command = 0; |
networker | 0:371773dd3dd1 | 66 | state = tracking; |
networker | 0:371773dd3dd1 | 67 | if (t != NC) |
networker | 0:371773dd3dd1 | 68 | touch = new AnalogIn(t); |
networker | 0:371773dd3dd1 | 69 | else |
networker | 0:371773dd3dd1 | 70 | touch = 0; |
networker | 0:371773dd3dd1 | 71 | } |
networker | 0:371773dd3dd1 | 72 | |
networker | 0:371773dd3dd1 | 73 | void fader::process() { //called by the ticker every 1 ms |
networker | 0:371773dd3dd1 | 74 | float p; |
networker | 0:371773dd3dd1 | 75 | switch (state) { //make sure that each branch calls either servo::process or servo::update for proper filtering |
networker | 0:371773dd3dd1 | 76 | case tracking: |
networker | 0:371773dd3dd1 | 77 | servo::process(); |
networker | 0:371773dd3dd1 | 78 | p = pos(); |
networker | 0:371773dd3dd1 | 79 | if (isCoasting()) { //servo is near it's setpoint, motor is off |
networker | 0:371773dd3dd1 | 80 | state = holding; |
networker | 0:371773dd3dd1 | 81 | lastpos = p; //save the position that was reached |
networker | 0:371773dd3dd1 | 82 | track = 1; //debug |
networker | 0:371773dd3dd1 | 83 | } |
networker | 0:371773dd3dd1 | 84 | break; |
networker | 0:371773dd3dd1 | 85 | case holding: |
networker | 0:371773dd3dd1 | 86 | update(); |
networker | 0:371773dd3dd1 | 87 | p = pos(); |
networker | 0:371773dd3dd1 | 88 | if (fabs(lastpos - p) > thres) { //apparently position has changed (manual move) |
networker | 0:371773dd3dd1 | 89 | state = moving; |
networker | 0:371773dd3dd1 | 90 | printf("moving from %f to %f\n", lastpos, p); |
networker | 0:371773dd3dd1 | 91 | lastpos = p; |
networker | 0:371773dd3dd1 | 92 | count = 0; |
networker | 0:371773dd3dd1 | 93 | }//if not, stay in 'holding' until next 'set' command, do not update lastpos |
networker | 0:371773dd3dd1 | 94 | break; |
networker | 0:371773dd3dd1 | 95 | case moving: |
networker | 0:371773dd3dd1 | 96 | //not tracking but coasting |
networker | 0:371773dd3dd1 | 97 | update(); |
networker | 0:371773dd3dd1 | 98 | p = pos(); |
networker | 0:371773dd3dd1 | 99 | if (fabs(lastpos - p) < thres2) { |
networker | 0:371773dd3dd1 | 100 | count++; |
networker | 0:371773dd3dd1 | 101 | } else { |
networker | 0:371773dd3dd1 | 102 | count = 0; |
networker | 0:371773dd3dd1 | 103 | } |
networker | 0:371773dd3dd1 | 104 | if (count > 100) { //apparently movement has stopped, movement less then thres2 for 100ms |
networker | 0:371773dd3dd1 | 105 | printf("movement stopped at %f\n", p); |
networker | 0:371773dd3dd1 | 106 | servo::set(p); //update the servo setpoint (has no effect because state is not tracking) |
networker | 0:371773dd3dd1 | 107 | if (command) |
networker | 0:371773dd3dd1 | 108 | command(p); //invoke the OnMove handler |
networker | 0:371773dd3dd1 | 109 | state = holding; //go back to holding state to allow a new fader::set and return to tracking |
networker | 0:371773dd3dd1 | 110 | } |
networker | 0:371773dd3dd1 | 111 | lastpos = p; |
networker | 0:371773dd3dd1 | 112 | break; |
networker | 0:371773dd3dd1 | 113 | default: //cannot happen |
networker | 0:371773dd3dd1 | 114 | update(); |
networker | 0:371773dd3dd1 | 115 | p = pos(); |
networker | 0:371773dd3dd1 | 116 | } |
networker | 0:371773dd3dd1 | 117 | if (touch && *touch > thres) { //stub: if input fullfills some condition (touch sense on fader) |
networker | 0:371773dd3dd1 | 118 | if (command) |
networker | 0:371773dd3dd1 | 119 | command(p); |
networker | 0:371773dd3dd1 | 120 | } |
networker | 0:371773dd3dd1 | 121 | #ifdef GETSTAT |
networker | 0:371773dd3dd1 | 122 | sum += p; |
networker | 0:371773dd3dd1 | 123 | sumprod += p*p; |
networker | 0:371773dd3dd1 | 124 | n++; |
networker | 0:371773dd3dd1 | 125 | if (n == 1000) { |
networker | 0:371773dd3dd1 | 126 | float mean = sum/n; |
networker | 0:371773dd3dd1 | 127 | float var = sumprod/n - mean*mean; |
networker | 0:371773dd3dd1 | 128 | printf("n=%d, E=%f, Var = %f, sDev=%f\n", n, mean, var, sqrt(var)); |
networker | 0:371773dd3dd1 | 129 | n=0; |
networker | 0:371773dd3dd1 | 130 | sum=0; |
networker | 0:371773dd3dd1 | 131 | sumprod=0; |
networker | 0:371773dd3dd1 | 132 | } |
networker | 0:371773dd3dd1 | 133 | #endif |
networker | 0:371773dd3dd1 | 134 | } |