Ad van der Weiden / Mbed 2 deprecated ipod

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?

UserRevisionLine numberNew 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 }