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@4:c38f7e4e8fda, 2012-08-13 (annotated)

Committer:
Reignbow
Date:
Mon Aug 13 12:44:56 2012 +0000
Revision:
4:c38f7e4e8fda
Parent:
3:1a4ad4c82046
Child:
5:bf2ed3846087
* output to zero if CTL below 10

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 4:c38f7e4e8fda 12 DigitalOut dacCS(p14);
Reignbow 2:6d472c554758 13 DigitalOut cyc(p10);
Reignbow 0:3e6ae21a6bb1 14 AnalogOut vca(p18);
Reignbow 4:c38f7e4e8fda 15 SPI adc(p5,p6,p7); //talk to ad7921 on SSP1
Reignbow 4:c38f7e4e8fda 16 SPI dac(p11,p12,p13); //talk to ad5551 on SSP0
Reignbow 0:3e6ae21a6bb1 17 Serial pc(USBTX, USBRX);
Reignbow 0:3e6ae21a6bb1 18
Reignbow 0:3e6ae21a6bb1 19
Reignbow 0:3e6ae21a6bb1 20 Ticker commTick;
Reignbow 4:c38f7e4e8fda 21 Ticker limTick;
Reignbow 0:3e6ae21a6bb1 22
Reignbow 4:c38f7e4e8fda 23 // PI variables
Reignbow 4:c38f7e4e8fda 24 int pGain = 7000;
Reignbow 4:c38f7e4e8fda 25 int iGain = 12000;
Reignbow 4:c38f7e4e8fda 26 int ctl=0;
Reignbow 4:c38f7e4e8fda 27 int pd=0;
Reignbow 4:c38f7e4e8fda 28 int err=0;
Reignbow 4:c38f7e4e8fda 29 int out = 0;
Reignbow 3:1a4ad4c82046 30 int integrator = 0; //the building up integrator value
Reignbow 0:3e6ae21a6bb1 31
Reignbow 4:c38f7e4e8fda 32 //control variables
Reignbow 4:c38f7e4e8fda 33 int monitor = 0; //echo to serial?
Reignbow 4:c38f7e4e8fda 34 int testDAC = 0; //stops PI, just 0-5V jumps on output
Reignbow 4:c38f7e4e8fda 35
Reignbow 4:c38f7e4e8fda 36
Reignbow 4:c38f7e4e8fda 37 extern "C" void mbed_reset();
Reignbow 4:c38f7e4e8fda 38
Reignbow 4:c38f7e4e8fda 39
Reignbow 0:3e6ae21a6bb1 40 void serialComm(){
Reignbow 0:3e6ae21a6bb1 41 char c;
Reignbow 3:1a4ad4c82046 42 int i;
Reignbow 0:3e6ae21a6bb1 43
Reignbow 4:c38f7e4e8fda 44 if (monitor) {
Reignbow 4:c38f7e4e8fda 45 pc.printf("Measuring ctl=%i and pd=%i, err=%i, integrator= %i, out=%i\n",ctl,pd,err,integrator,out);
Reignbow 4:c38f7e4e8fda 46 }
Reignbow 4:c38f7e4e8fda 47
Reignbow 0:3e6ae21a6bb1 48 if (pc.readable()) {
Reignbow 0:3e6ae21a6bb1 49 c = pc.getc();
Reignbow 3:1a4ad4c82046 50 pc.scanf("%i", &i);
Reignbow 0:3e6ae21a6bb1 51 switch (c) {
Reignbow 0:3e6ae21a6bb1 52 case 'p':
Reignbow 3:1a4ad4c82046 53 pGain = i;
Reignbow 3:1a4ad4c82046 54 pc.printf("Changed p gain to %i.\n",pGain);
Reignbow 0:3e6ae21a6bb1 55 break;
Reignbow 0:3e6ae21a6bb1 56 case 'i':
Reignbow 3:1a4ad4c82046 57 iGain = i;
Reignbow 3:1a4ad4c82046 58 pc.printf("Changed i gain to %i.\n",iGain);
Reignbow 3:1a4ad4c82046 59 if (i==0)
Reignbow 3:1a4ad4c82046 60 integrator = 0;
Reignbow 0:3e6ae21a6bb1 61 break;
Reignbow 4:c38f7e4e8fda 62 case 'm':
Reignbow 4:c38f7e4e8fda 63 monitor = i;
Reignbow 4:c38f7e4e8fda 64 break;
Reignbow 4:c38f7e4e8fda 65 case 't':
Reignbow 4:c38f7e4e8fda 66 testDAC = i;
Reignbow 4:c38f7e4e8fda 67 pc.printf("Turning on test mode... outputting 0-5 V on DAC\n");
Reignbow 4:c38f7e4e8fda 68 break;
Reignbow 4:c38f7e4e8fda 69 case 'z':
Reignbow 4:c38f7e4e8fda 70 mbed_reset();
Reignbow 4:c38f7e4e8fda 71 break;
Reignbow 0:3e6ae21a6bb1 72 default:
Reignbow 0:3e6ae21a6bb1 73 pc.printf("Command not understood.\n",iGain);
Reignbow 3:1a4ad4c82046 74 pc.printf("Read %c and %i.\n",c,i);
Reignbow 0:3e6ae21a6bb1 75 break;
Reignbow 0:3e6ae21a6bb1 76 }
Reignbow 0:3e6ae21a6bb1 77 }
Reignbow 0:3e6ae21a6bb1 78 }
Reignbow 0:3e6ae21a6bb1 79
Reignbow 0:3e6ae21a6bb1 80
Reignbow 4:c38f7e4e8fda 81 void limitIntegrator() {
Reignbow 4:c38f7e4e8fda 82 if(integrator < -(1<<14)/iGain) {
Reignbow 4:c38f7e4e8fda 83 integrator = -(1<<14)/iGain;
Reignbow 4:c38f7e4e8fda 84 }
Reignbow 4:c38f7e4e8fda 85 if(integrator > (1<<14)/iGain) {
Reignbow 4:c38f7e4e8fda 86 integrator = (1<<14)/iGain;
Reignbow 4:c38f7e4e8fda 87 }
Reignbow 4:c38f7e4e8fda 88 }
Reignbow 4:c38f7e4e8fda 89
Reignbow 0:3e6ae21a6bb1 90 int main() {
Reignbow 0:3e6ae21a6bb1 91 pc.printf("mbed restarted!\n");
Reignbow 0:3e6ae21a6bb1 92
Reignbow 3:1a4ad4c82046 93 adc.format(16,2);
Reignbow 4:c38f7e4e8fda 94 adc.frequency(5e6);
Reignbow 4:c38f7e4e8fda 95 dac.format(14,2);
Reignbow 4:c38f7e4e8fda 96 dac.frequency(20e6);
Reignbow 1:dc799997391e 97
Reignbow 4:c38f7e4e8fda 98 commTick.attach(&serialComm,1); //check serial every second
Reignbow 4:c38f7e4e8fda 99 //limTick.attach_us(&limitIntegrator,500); //check integrator overflow every 500 us
Reignbow 2:6d472c554758 100 cyc = 0;
Reignbow 0:3e6ae21a6bb1 101
Reignbow 0:3e6ae21a6bb1 102 while(1) {
Reignbow 3:1a4ad4c82046 103 cyc = cyc^1; //toggle debug pin
Reignbow 2:6d472c554758 104
Reignbow 4:c38f7e4e8fda 105 while (testDAC) {
Reignbow 4:c38f7e4e8fda 106 dacCS = 1;
Reignbow 4:c38f7e4e8fda 107 dacCS = 0;
Reignbow 4:c38f7e4e8fda 108 LPC_SSP0->DR = 0x3FFF;
Reignbow 4:c38f7e4e8fda 109 wait_us(1000);
Reignbow 4:c38f7e4e8fda 110 dacCS = 1;
Reignbow 4:c38f7e4e8fda 111 dacCS = 0;
Reignbow 4:c38f7e4e8fda 112 LPC_SSP0->DR = 0;
Reignbow 4:c38f7e4e8fda 113 wait_us(1000);
Reignbow 4:c38f7e4e8fda 114 }
Reignbow 4:c38f7e4e8fda 115
Reignbow 3:1a4ad4c82046 116 //stagger read and output
Reignbow 3:1a4ad4c82046 117 //The SPI read takes a long time, so start it by writing to the data
Reignbow 4:c38f7e4e8fda 118 //register, do sth. else (write new output), then get the value
Reignbow 3:1a4ad4c82046 119 //and start the read from the other channel
Reignbow 3:1a4ad4c82046 120
Reignbow 3:1a4ad4c82046 121 //begin SPI transfer
Reignbow 1:dc799997391e 122 adcCS = 0;
Reignbow 4:c38f7e4e8fda 123 LPC_SSP1->DR = 3<<13; //select ch 1
Reignbow 3:1a4ad4c82046 124
Reignbow 4:c38f7e4e8fda 125 //write to DAC
Reignbow 4:c38f7e4e8fda 126 err = ctl - pd;
Reignbow 4:c38f7e4e8fda 127 integrator += err;
Reignbow 4:c38f7e4e8fda 128 dacCS = 0;
Reignbow 4:c38f7e4e8fda 129 // if control is very low (probably 0V), no output and no integrator
Reignbow 4:c38f7e4e8fda 130 // otherwise integrator overflows from stray light!
Reignbow 4:c38f7e4e8fda 131 if (ctl < 10) {
Reignbow 4:c38f7e4e8fda 132 err = 0;
Reignbow 4:c38f7e4e8fda 133 integrator = 0;
Reignbow 4:c38f7e4e8fda 134 }
Reignbow 4:c38f7e4e8fda 135 out = ( (err * pGain + integrator)>>12 ) & 0x3FFF;
Reignbow 4:c38f7e4e8fda 136 LPC_SSP0->DR = out;
Reignbow 4:c38f7e4e8fda 137
Reignbow 3:1a4ad4c82046 138 //read SPI transfer results
Reignbow 4:c38f7e4e8fda 139 while(LPC_SSP1->SR & 0x10) {} //check for busy p 10
Reignbow 4:c38f7e4e8fda 140 pd = LPC_SSP1->DR & 0xFFF; //last 12 bits are data
Reignbow 4:c38f7e4e8fda 141 wait_us(1); //delay necessary for unknown reason
Reignbow 4:c38f7e4e8fda 142 adcCS = 1;
Reignbow 4:c38f7e4e8fda 143 while(LPC_SSP0->SR & 0x10) {}
Reignbow 4:c38f7e4e8fda 144 dacCS = 1;
Reignbow 4:c38f7e4e8fda 145
Reignbow 4:c38f7e4e8fda 146
Reignbow 1:dc799997391e 147
Reignbow 3:1a4ad4c82046 148 //begin transfer from other channel
Reignbow 3:1a4ad4c82046 149 adcCS = 0;
Reignbow 4:c38f7e4e8fda 150 LPC_SSP1->DR = 1; //select ch 0
Reignbow 3:1a4ad4c82046 151
Reignbow 4:c38f7e4e8fda 152 //write to DAC
Reignbow 4:c38f7e4e8fda 153 err = ctl-pd;
Reignbow 4:c38f7e4e8fda 154 integrator += err*iGain;
Reignbow 4:c38f7e4e8fda 155 dacCS = 0;
Reignbow 4:c38f7e4e8fda 156 // if control is very low (probably 0V), no output and no integrator
Reignbow 4:c38f7e4e8fda 157 // otherwise integrator overflows from stray light!
Reignbow 4:c38f7e4e8fda 158 if (ctl < 10) {
Reignbow 4:c38f7e4e8fda 159 err = 0;
Reignbow 4:c38f7e4e8fda 160 integrator = 0;
Reignbow 4:c38f7e4e8fda 161 }
Reignbow 4:c38f7e4e8fda 162 out = ( (err * pGain + integrator)>>12 ) & 0x3FFF;
Reignbow 4:c38f7e4e8fda 163 LPC_SSP0->DR = out;
Reignbow 4:c38f7e4e8fda 164
Reignbow 3:1a4ad4c82046 165 //read SPI transfer results
Reignbow 4:c38f7e4e8fda 166 while(LPC_SSP1->SR & 0x10) {}
Reignbow 4:c38f7e4e8fda 167 ctl = LPC_SSP1->DR & 0xFFF; //last 12 bits are data
Reignbow 4:c38f7e4e8fda 168 wait_us(1); //delay necessary for unknown reason
Reignbow 4:c38f7e4e8fda 169 adcCS = 1;
Reignbow 4:c38f7e4e8fda 170 while(LPC_SSP0->SR & 0x10) {}
Reignbow 4:c38f7e4e8fda 171 dacCS = 1;
Reignbow 3:1a4ad4c82046 172
Reignbow 2:6d472c554758 173
Reignbow 4:c38f7e4e8fda 174
Reignbow 0:3e6ae21a6bb1 175 }
Reignbow 0:3e6ae21a6bb1 176 }