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@5:bf2ed3846087, 2013-03-04 (annotated)

Committer:
Reignbow
Date:
Mon Mar 04 16:21:35 2013 +0000
Revision:
5:bf2ed3846087
Parent:
4:c38f7e4e8fda 
Publish.

Who changed what in which revision?

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