Andreas Steffen / Mbed 2 deprecated PIservo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:3e6ae21a6bb1, 2012-08-06 (annotated)

Committer:
Reignbow
Date:
Mon Aug 06 12:53:57 2012 +0000
Revision:
0:3e6ae21a6bb1
Child:
1:dc799997391e
Initial commit. Working PI with interrupts for integrator limiting and serial communication.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reignbow 0:3e6ae21a6bb1 1 /* PIservo
Reignbow 0:3e6ae21a6bb1 2 implement a PI controller with USB serial gain setting to stabilize dipole trap power.
Reignbow 0:3e6ae21a6bb1 3 Two analog inputs (control and sensor), one analog output (to RF VCA).
Reignbow 0:3e6ae21a6bb1 4
Reignbow 0:3e6ae21a6bb1 5 20120806 Andreas Steffen
Reignbow 0:3e6ae21a6bb1 6 */
Reignbow 0:3e6ae21a6bb1 7
Reignbow 0:3e6ae21a6bb1 8
Reignbow 0:3e6ae21a6bb1 9 #include "mbed.h"
Reignbow 0:3e6ae21a6bb1 10
Reignbow 0:3e6ae21a6bb1 11 AnalogIn ctl(p20);
Reignbow 0:3e6ae21a6bb1 12 AnalogIn pd(p16);
Reignbow 0:3e6ae21a6bb1 13 AnalogOut vca(p18);
Reignbow 0:3e6ae21a6bb1 14 DigitalOut led1(LED1);
Reignbow 0:3e6ae21a6bb1 15 Serial pc(USBTX, USBRX);
Reignbow 0:3e6ae21a6bb1 16
Reignbow 0:3e6ae21a6bb1 17
Reignbow 0:3e6ae21a6bb1 18 Timer t;
Reignbow 0:3e6ae21a6bb1 19 Ticker commTick;
Reignbow 0:3e6ae21a6bb1 20 Ticker intTick;
Reignbow 0:3e6ae21a6bb1 21
Reignbow 0:3e6ae21a6bb1 22 float pGain = 1.;
Reignbow 0:3e6ae21a6bb1 23 float iGain = 1000.;
Reignbow 0:3e6ae21a6bb1 24 float integrator = 0.; //the building up integrator value
Reignbow 0:3e6ae21a6bb1 25
Reignbow 0:3e6ae21a6bb1 26 void serialComm(){
Reignbow 0:3e6ae21a6bb1 27 char c;
Reignbow 0:3e6ae21a6bb1 28 float f;
Reignbow 0:3e6ae21a6bb1 29
Reignbow 0:3e6ae21a6bb1 30 if (pc.readable()) {
Reignbow 0:3e6ae21a6bb1 31 c = pc.getc();
Reignbow 0:3e6ae21a6bb1 32 pc.scanf("%f", &f);
Reignbow 0:3e6ae21a6bb1 33 switch (c) {
Reignbow 0:3e6ae21a6bb1 34 case 'p':
Reignbow 0:3e6ae21a6bb1 35 pGain = f;
Reignbow 0:3e6ae21a6bb1 36 pc.printf("Changed p gain to %f.\n",pGain);
Reignbow 0:3e6ae21a6bb1 37 break;
Reignbow 0:3e6ae21a6bb1 38 case 'i':
Reignbow 0:3e6ae21a6bb1 39 iGain = f;
Reignbow 0:3e6ae21a6bb1 40 pc.printf("Changed i gain to %f.\n",iGain);
Reignbow 0:3e6ae21a6bb1 41 break;
Reignbow 0:3e6ae21a6bb1 42 default:
Reignbow 0:3e6ae21a6bb1 43 pc.printf("Command not understood.\n",iGain);
Reignbow 0:3e6ae21a6bb1 44 pc.printf("Read %c and %lf.\n",c,f);
Reignbow 0:3e6ae21a6bb1 45 break;
Reignbow 0:3e6ae21a6bb1 46 }
Reignbow 0:3e6ae21a6bb1 47 }
Reignbow 0:3e6ae21a6bb1 48 }
Reignbow 0:3e6ae21a6bb1 49
Reignbow 0:3e6ae21a6bb1 50 void limitIntegrator(){
Reignbow 0:3e6ae21a6bb1 51 if (integrator > 3.3) {
Reignbow 0:3e6ae21a6bb1 52 integrator = 3.3;
Reignbow 0:3e6ae21a6bb1 53 }
Reignbow 0:3e6ae21a6bb1 54 if (integrator < -3.3) {
Reignbow 0:3e6ae21a6bb1 55 integrator = -3.3;
Reignbow 0:3e6ae21a6bb1 56 }
Reignbow 0:3e6ae21a6bb1 57 }
Reignbow 0:3e6ae21a6bb1 58
Reignbow 0:3e6ae21a6bb1 59 int main() {
Reignbow 0:3e6ae21a6bb1 60 float err;
Reignbow 0:3e6ae21a6bb1 61 pc.printf("mbed restarted!\n");
Reignbow 0:3e6ae21a6bb1 62
Reignbow 0:3e6ae21a6bb1 63 commTick.attach(&serialComm,0.5); //check serial every half second
Reignbow 0:3e6ae21a6bb1 64 intTick.attach(&limitIntegrator,0.001); //check integrator overrun every ms
Reignbow 0:3e6ae21a6bb1 65
Reignbow 0:3e6ae21a6bb1 66 t.start();
Reignbow 0:3e6ae21a6bb1 67
Reignbow 0:3e6ae21a6bb1 68 while(1) {
Reignbow 0:3e6ae21a6bb1 69 err = ctl - pd;
Reignbow 0:3e6ae21a6bb1 70 integrator += err * iGain*1e-6 * t.read_us(); //iGain is in Hz!
Reignbow 0:3e6ae21a6bb1 71 t.reset(); //reset timer to get next integration time
Reignbow 0:3e6ae21a6bb1 72 vca = err * pGain + integrator;
Reignbow 0:3e6ae21a6bb1 73 }
Reignbow 0:3e6ae21a6bb1 74 }