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@2:6d472c554758, 2012-08-07 (annotated)

Committer:
Reignbow
Date:
Tue Aug 07 12:07:32 2012 +0000
Revision:
2:6d472c554758
Parent:
1:dc799997391e
Child:
3:1a4ad4c82046
Working PI servo with two SPI analog inputs and on-chip DAC. Cycle time about 26 us.

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 1:dc799997391e 11 DigitalOut adcCS(p8);
Reignbow 2:6d472c554758 12 DigitalOut cyc(p10);
Reignbow 0:3e6ae21a6bb1 13 AnalogOut vca(p18);
Reignbow 1:dc799997391e 14 SPI adcSPI(p5,p6,p7); //talk to ad7921
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 1:dc799997391e 22 float pGain = 0.5;
Reignbow 2:6d472c554758 23 float iGain = 1000.;
Reignbow 2:6d472c554758 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 2:6d472c554758 41 if (f==0)
Reignbow 2:6d472c554758 42 integrator = 0.;
Reignbow 0:3e6ae21a6bb1 43 break;
Reignbow 0:3e6ae21a6bb1 44 default:
Reignbow 0:3e6ae21a6bb1 45 pc.printf("Command not understood.\n",iGain);
Reignbow 1:dc799997391e 46 pc.printf("Read %c and %f.\n",c,f);
Reignbow 0:3e6ae21a6bb1 47 break;
Reignbow 0:3e6ae21a6bb1 48 }
Reignbow 0:3e6ae21a6bb1 49 }
Reignbow 0:3e6ae21a6bb1 50 }
Reignbow 0:3e6ae21a6bb1 51
Reignbow 0:3e6ae21a6bb1 52 void limitIntegrator(){
Reignbow 0:3e6ae21a6bb1 53 if (integrator > 3.3) {
Reignbow 0:3e6ae21a6bb1 54 integrator = 3.3;
Reignbow 0:3e6ae21a6bb1 55 }
Reignbow 2:6d472c554758 56 if (integrator < 0) {
Reignbow 2:6d472c554758 57 integrator = 0;
Reignbow 0:3e6ae21a6bb1 58 }
Reignbow 0:3e6ae21a6bb1 59 }
Reignbow 0:3e6ae21a6bb1 60
Reignbow 0:3e6ae21a6bb1 61 int main() {
Reignbow 2:6d472c554758 62 float err=0;
Reignbow 2:6d472c554758 63 float out=0.;
Reignbow 1:dc799997391e 64 int ctl=0;
Reignbow 1:dc799997391e 65 int pd=0;
Reignbow 0:3e6ae21a6bb1 66 pc.printf("mbed restarted!\n");
Reignbow 0:3e6ae21a6bb1 67
Reignbow 2:6d472c554758 68 adcSPI.format(16,2);
Reignbow 2:6d472c554758 69 adcSPI.frequency(5000000);
Reignbow 1:dc799997391e 70
Reignbow 2:6d472c554758 71 commTick.attach(&serialComm,0.5); //check serial every half second
Reignbow 2:6d472c554758 72 intTick.attach(&limitIntegrator,0.001); //check integrator overrun every ms
Reignbow 0:3e6ae21a6bb1 73
Reignbow 0:3e6ae21a6bb1 74 t.start();
Reignbow 2:6d472c554758 75 cyc = 0;
Reignbow 0:3e6ae21a6bb1 76
Reignbow 0:3e6ae21a6bb1 77 while(1) {
Reignbow 2:6d472c554758 78 cyc = cyc ^ 1; //toggle debug pin
Reignbow 2:6d472c554758 79 //read in from AD7921 over SPI
Reignbow 1:dc799997391e 80 adcCS = 0;
Reignbow 2:6d472c554758 81 pd = adcSPI.write(3<<13); //select ch 1
Reignbow 2:6d472c554758 82 pd = (pd) & 0xFFF;
Reignbow 1:dc799997391e 83 adcCS = 1;
Reignbow 2:6d472c554758 84
Reignbow 1:dc799997391e 85 adcCS = 0;
Reignbow 2:6d472c554758 86 ctl = adcSPI.write(1); // select ch 0
Reignbow 1:dc799997391e 87 //take the last 12 bits that carry data
Reignbow 2:6d472c554758 88 ctl = (ctl) & 0xFFF;
Reignbow 2:6d472c554758 89 adcCS = 1;
Reignbow 2:6d472c554758 90
Reignbow 2:6d472c554758 91 err = ctl*3.3/4095. - pd*3.3/4095.; //convert to volt
Reignbow 2:6d472c554758 92 integrator += err * iGain*1.e-6 * t.read_us();
Reignbow 1:dc799997391e 93 t.reset(); //reset timer to get next integration time
Reignbow 1:dc799997391e 94
Reignbow 2:6d472c554758 95 out = 1.0* err * pGain + integrator; //analog output
Reignbow 2:6d472c554758 96 vca.write(out);
Reignbow 1:dc799997391e 97
Reignbow 2:6d472c554758 98 // pc.printf("Measuring ctl=%i and pd=%i, err=%f, vca is %f\n",ctl,pd,err,out);
Reignbow 2:6d472c554758 99
Reignbow 0:3e6ae21a6bb1 100 }
Reignbow 0:3e6ae21a6bb1 101 }