igor Apu / Mbed 2 deprecated LG Featured

123

Dependencies:   mbed

Fork of LG by igor Apu

DevicePLCS.c@166:c3c0b8a90d81, 2016-05-22 (annotated)

Committer:
Diletant
Date:
Sun May 22 18:59:20 2016 +0000
Revision:
166:c3c0b8a90d81
Parent:
156:e68ee0bcdcda
Child:
167:bedc0a9d559a
Device & ... update. Not final!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Diletant 149:abbf7663d27d 1 #include "Device.h"
Diletant 149:abbf7663d27d 2 extern Device device;
Diletant 149:abbf7663d27d 3
Diletant 149:abbf7663d27d 4 void InitPathLengthControlSystemWithDefaults(void){
Diletant 166:c3c0b8a90d81 5 //device.plcs.amplifier.settings.reference = 0x00020000;
Diletant 166:c3c0b8a90d81 6 //device.plcs.amplifier.settings.gain = 10;
Diletant 149:abbf7663d27d 7 }
Diletant 149:abbf7663d27d 8
Diletant 149:abbf7663d27d 9 void InitPathLengthControlSystem(void){
Diletant 166:c3c0b8a90d81 10 //Init P0.26: Sequencer GPIO output
Diletant 166:c3c0b8a90d81 11 LPC_PINCON->PINSEL1 &= ~(3<<20); //P0.26 is GPIO pin (write ( 00 ) in bits 21:20 of PINSEL1)
Diletant 166:c3c0b8a90d81 12 LPC_PINCON->PINMODE1 |= (3<<20); //P0.26 pull-down resistor on (write ( 11 ) in bits 21:20 of PINMODE0)
Diletant 166:c3c0b8a90d81 13 LPC_GPIO0->FIODIR |= (1<<26); //P0.26 is output (write ( 1 ) in bit 4 of FIODIR)
Diletant 166:c3c0b8a90d81 14 LPC_GPIO0->FIOSET |= (1<<26); //off
Diletant 149:abbf7663d27d 15 }
Diletant 149:abbf7663d27d 16
Diletant 149:abbf7663d27d 17 void StartPathLengthControlSystem(void){
Diletant 156:e68ee0bcdcda 18 }
Diletant 156:e68ee0bcdcda 19
Diletant 156:e68ee0bcdcda 20 /*
Diletant 156:e68ee0bcdcda 21 //Move to DevicePLCS regulator
Diletant 156:e68ee0bcdcda 22 uint32_t value;
Diletant 156:e68ee0bcdcda 23 switch(device.plcs.state.modulation) {
Diletant 156:e68ee0bcdcda 24 case 1://малое воздействие
Diletant 156:e68ee0bcdcda 25 value = device.SSP.DAC[1] + Gyro.StrayPLC_Pls;
Diletant 156:e68ee0bcdcda 26 break;
Diletant 156:e68ee0bcdcda 27
Diletant 156:e68ee0bcdcda 28 case 3://малое воздействие
Diletant 156:e68ee0bcdcda 29 value = device.SSP.DAC[1] + Gyro.StrayPLC_Mns;
Diletant 156:e68ee0bcdcda 30 break;
Diletant 156:e68ee0bcdcda 31
Diletant 156:e68ee0bcdcda 32 case 2://большое воздействие
Diletant 156:e68ee0bcdcda 33 value = device.SSP.DAC[1] + Gyro.StrayPLC_2Mode;
Diletant 156:e68ee0bcdcda 34 break;
Diletant 156:e68ee0bcdcda 35
Diletant 156:e68ee0bcdcda 36 default://режим без воздействия
Diletant 156:e68ee0bcdcda 37 value = device.SSP.DAC[1];
Diletant 156:e68ee0bcdcda 38 break;
Diletant 156:e68ee0bcdcda 39 }
Diletant 156:e68ee0bcdcda 40 LPC_SSP0->DR = device.SSP.DAC[1];
Diletant 156:e68ee0bcdcda 41 */
Diletant 156:e68ee0bcdcda 42
Diletant 166:c3c0b8a90d81 43 int32_t plcsInterpolate(int32_t a,int32_t a1,int32_t a2,int32_t b1,int32_t b2) {
Diletant 166:c3c0b8a90d81 44 int32_t ma, mb;
Diletant 166:c3c0b8a90d81 45 while (1) {
Diletant 166:c3c0b8a90d81 46 if (a1 == a) return b1;
Diletant 166:c3c0b8a90d81 47 if (a2 == a) return b2;
Diletant 166:c3c0b8a90d81 48
Diletant 166:c3c0b8a90d81 49 ma = (a1 + a2) >> 1;
Diletant 166:c3c0b8a90d81 50 mb = (b1 + b2) >> 1;
Diletant 166:c3c0b8a90d81 51
Diletant 166:c3c0b8a90d81 52 if (a < ma) {
Diletant 166:c3c0b8a90d81 53 if (a2 == ma) return mb;
Diletant 166:c3c0b8a90d81 54 if (b1 == mb) return mb;
Diletant 166:c3c0b8a90d81 55 a2 = ma; b2 = mb;
Diletant 166:c3c0b8a90d81 56 } else if (a > ma) {
Diletant 166:c3c0b8a90d81 57 if (a1 == ma) return mb;
Diletant 166:c3c0b8a90d81 58 if (b2 == mb) return mb;
Diletant 166:c3c0b8a90d81 59 a1 = ma; b1 = mb;
Diletant 166:c3c0b8a90d81 60 } else return mb;
Diletant 166:c3c0b8a90d81 61 }
Diletant 166:c3c0b8a90d81 62 }
Diletant 166:c3c0b8a90d81 63
Diletant 166:c3c0b8a90d81 64 uint32_t plcsVoltageToCode(int32_t voltage) {
Diletant 166:c3c0b8a90d81 65 int32_t v1, v2;
Diletant 166:c3c0b8a90d81 66 for (uint8_t i = 1; i < device.plcs.output.interpolator.settings.points; i++) {
Diletant 166:c3c0b8a90d81 67 v1 = device.plcs.output.interpolator.settings.voltage[i - 1];
Diletant 166:c3c0b8a90d81 68 if (voltage < v1) {
Diletant 166:c3c0b8a90d81 69 return device.plcs.output.interpolator.settings.code[i - 1];
Diletant 166:c3c0b8a90d81 70 }
Diletant 166:c3c0b8a90d81 71
Diletant 166:c3c0b8a90d81 72 v2 = device.plcs.output.interpolator.settings.voltage[i];
Diletant 166:c3c0b8a90d81 73 if (voltage < v2) {
Diletant 166:c3c0b8a90d81 74 int32_t c1 = device.plcs.output.interpolator.settings.code[i - 1];
Diletant 166:c3c0b8a90d81 75 int32_t c2 = device.plcs.output.interpolator.settings.code[i];
Diletant 166:c3c0b8a90d81 76 return plcsInterpolate(voltage, v1, v2, c1, c2);
Diletant 166:c3c0b8a90d81 77 }
Diletant 166:c3c0b8a90d81 78 }
Diletant 166:c3c0b8a90d81 79 return device.isacs.output.interpolator.settings.code[device.isacs.output.interpolator.settings.points - 1];
Diletant 166:c3c0b8a90d81 80 }
Diletant 166:c3c0b8a90d81 81
Diletant 166:c3c0b8a90d81 82 /*
Diletant 166:c3c0b8a90d81 83 void plcsSetOutputVoltage(int32_t voltage) {
Diletant 166:c3c0b8a90d81 84 int32_t v1, v2;
Diletant 166:c3c0b8a90d81 85 for (uint8_t i = 1; i < device.plcs.output.settings.interpolator.points; i++) {
Diletant 166:c3c0b8a90d81 86 v1 = device.plcs.output.settings.interpolator.voltage[i - 1];
Diletant 166:c3c0b8a90d81 87 if (voltage < v1) {
Diletant 166:c3c0b8a90d81 88 uint32_t code = device.plcs.output.settings.interpolator.code[i - 1];
Diletant 166:c3c0b8a90d81 89 device.plcs.output.state.voltage = v1;
Diletant 166:c3c0b8a90d81 90 device.controller.SSP.out[1] = code;
Diletant 166:c3c0b8a90d81 91 return;
Diletant 166:c3c0b8a90d81 92 }
Diletant 166:c3c0b8a90d81 93
Diletant 166:c3c0b8a90d81 94 v2 = device.plcs.output.settings.interpolator.voltage[i];
Diletant 166:c3c0b8a90d81 95 if (voltage < v2) {
Diletant 166:c3c0b8a90d81 96 int32_t c1 = device.plcs.output.settings.interpolator.code[i - 1];
Diletant 166:c3c0b8a90d81 97 int32_t c2 = device.plcs.output.settings.interpolator.code[i];
Diletant 166:c3c0b8a90d81 98 uint32_t code = plcsInterpolate(voltage, v1, v2, c1, c2);
Diletant 166:c3c0b8a90d81 99 device.plcs.output.state.voltage = voltage;
Diletant 166:c3c0b8a90d81 100 device.controller.SSP.out[1] = code;
Diletant 166:c3c0b8a90d81 101 return;
Diletant 166:c3c0b8a90d81 102 }
Diletant 166:c3c0b8a90d81 103 }
Diletant 166:c3c0b8a90d81 104 uint32_t code = device.plcs.output.settings.interpolator.code[device.plcs.output.settings.interpolator.points - 1];
Diletant 166:c3c0b8a90d81 105 device.plcs.output.state.voltage = device.plcs.output.settings.interpolator.voltage[device.plcs.output.settings.interpolator.points - 1];
Diletant 166:c3c0b8a90d81 106 device.controller.SSP.out[1] = code;
Diletant 166:c3c0b8a90d81 107 }
Diletant 166:c3c0b8a90d81 108
Diletant 166:c3c0b8a90d81 109 void plcsSetOutputCode(int32_t code) {
Diletant 166:c3c0b8a90d81 110 int32_t c1, c2;
Diletant 166:c3c0b8a90d81 111 for (uint8_t i = 1; i < device.plcs.output.settings.interpolator.points; i++) {
Diletant 166:c3c0b8a90d81 112 c1 = device.plcs.output.settings.interpolator.code[i - 1];
Diletant 166:c3c0b8a90d81 113 if (code < c1) {
Diletant 166:c3c0b8a90d81 114 device.plcs.output.state.voltage = device.plcs.output.settings.interpolator.voltage[i - 1];
Diletant 166:c3c0b8a90d81 115 device.controller.SSP.out[1] = c1;
Diletant 166:c3c0b8a90d81 116 return;
Diletant 166:c3c0b8a90d81 117 }
Diletant 166:c3c0b8a90d81 118
Diletant 166:c3c0b8a90d81 119 c2 = device.plcs.output.settings.interpolator.code[i];
Diletant 166:c3c0b8a90d81 120 if (code < c2) {
Diletant 166:c3c0b8a90d81 121 int32_t v1 = device.plcs.output.settings.interpolator.voltage[i - 1];
Diletant 166:c3c0b8a90d81 122 int32_t v2 = device.plcs.output.settings.interpolator.voltage[i];
Diletant 166:c3c0b8a90d81 123 device.plcs.output.state.voltage = plcsInterpolate(code, c1, c2, v1, v2);
Diletant 166:c3c0b8a90d81 124 device.controller.SSP.out[1] = code;
Diletant 166:c3c0b8a90d81 125 return;
Diletant 166:c3c0b8a90d81 126 }
Diletant 166:c3c0b8a90d81 127 }
Diletant 166:c3c0b8a90d81 128 device.plcs.output.state.voltage = device.plcs.output.settings.interpolator.voltage[device.plcs.output.settings.interpolator.points - 1];
Diletant 166:c3c0b8a90d81 129 device.controller.SSP.out[1] = device.plcs.output.settings.interpolator.code[device.plcs.output.settings.interpolator.points - 1];
Diletant 166:c3c0b8a90d81 130 }
Diletant 166:c3c0b8a90d81 131 */
Diletant 166:c3c0b8a90d81 132
Diletant 166:c3c0b8a90d81 133 /*
Diletant 156:e68ee0bcdcda 134 //"Amplifier output voltage" = ("Amplifier reference" - "DAC output voltage") * "Amplifier gain"
Diletant 156:e68ee0bcdcda 135 void SetPathLengthDriveVoltage(int32_t Vdrive){
Diletant 166:c3c0b8a90d81 136
Diletant 156:e68ee0bcdcda 137 //Calculate amplifier output voltage
Diletant 156:e68ee0bcdcda 138 int32_t Vamp = Vdrive; //TODO: take control unit (transistor or high voltage amplifier) into account!!!
Diletant 156:e68ee0bcdcda 139 //Calculate DAC output voltage
Diletant 156:e68ee0bcdcda 140 int32_t Vdac_int32 = device.plcs.amplifier.settings.reference - Vamp / device.plcs.amplifier.settings.gain;
Diletant 156:e68ee0bcdcda 141 if (Vdac_int32 < 0) Vdac_int32 = 0;
Diletant 156:e68ee0bcdcda 142 uint32_t Vdac_uint32 = (uint32_t)Vdac_int32;
Diletant 156:e68ee0bcdcda 143 if (Vdac_uint32 > device.dac.settings.reference) Vdac_uint32 = device.dac.settings.reference;
Diletant 156:e68ee0bcdcda 144 //Set DAC output 1 voltage
Diletant 166:c3c0b8a90d81 145 DeviceDACSetVoltage(1, Vdac_uint32);
Diletant 166:c3c0b8a90d81 146 }
Diletant 166:c3c0b8a90d81 147 */
Diletant 166:c3c0b8a90d81 148
Diletant 166:c3c0b8a90d81 149 void plcsOutput(void) {
Diletant 166:c3c0b8a90d81 150 if (device.plcs.output.state.enabled) {
Diletant 166:c3c0b8a90d81 151 if (device.plcs.regulator.state.enabled) {
Diletant 166:c3c0b8a90d81 152 device.plcs.regulator.state.error = (device.plcs.regulator.state.reference - device.plcs.detector.state.phase) * device.plcs.regulator.state.scale;
Diletant 166:c3c0b8a90d81 153 device.plcs.output.state.voltage += device.plcs.regulator.state.error;
Diletant 166:c3c0b8a90d81 154 }
Diletant 166:c3c0b8a90d81 155 device.controller.SSP.out[1] = plcsVoltageToCode(device.plcs.output.state.voltage);
Diletant 166:c3c0b8a90d81 156 }
Diletant 166:c3c0b8a90d81 157 }
Diletant 166:c3c0b8a90d81 158
Diletant 166:c3c0b8a90d81 159 void plcsProcessDelta(void) {
Diletant 166:c3c0b8a90d81 160 //Process delta mode
Diletant 166:c3c0b8a90d81 161 //TODO
Diletant 166:c3c0b8a90d81 162
Diletant 166:c3c0b8a90d81 163 //Output voltage
Diletant 166:c3c0b8a90d81 164 plcsOutput();
Diletant 166:c3c0b8a90d81 165 }
Diletant 166:c3c0b8a90d81 166
Diletant 166:c3c0b8a90d81 167 void plcsProcessSequencer(void) {
Diletant 166:c3c0b8a90d81 168 //Process sequencer mode
Diletant 166:c3c0b8a90d81 169 //TODO
Diletant 166:c3c0b8a90d81 170
Diletant 166:c3c0b8a90d81 171 if (device.plcs.sequencer.state.enabled) {
Diletant 166:c3c0b8a90d81 172 if (device.plcs.sequencer.state.voltage) {
Diletant 166:c3c0b8a90d81 173 device.plcs.output.state.voltage += device.plcs.sequencer.settings.sequence[device.measurement.counter + device.measurement.length * device.plcs.sequencer.state.counter];
Diletant 166:c3c0b8a90d81 174 }
Diletant 166:c3c0b8a90d81 175
Diletant 166:c3c0b8a90d81 176 //Output io
Diletant 166:c3c0b8a90d81 177 if (device.plcs.sequencer.state.io) {
Diletant 166:c3c0b8a90d81 178 if (device.plcs.sequencer.settings.sequence[device.measurement.counter + device.measurement.length * device.plcs.sequencer.state.counter] > 0) {
Diletant 166:c3c0b8a90d81 179 LPC_GPIO0->FIOSET |= (1<<26); //1
Diletant 166:c3c0b8a90d81 180 } else {
Diletant 166:c3c0b8a90d81 181 LPC_GPIO0->FIOCLR |= (1<<26); //0
Diletant 166:c3c0b8a90d81 182 }
Diletant 166:c3c0b8a90d81 183 }
Diletant 166:c3c0b8a90d81 184 }
Diletant 166:c3c0b8a90d81 185 //Output voltage
Diletant 166:c3c0b8a90d81 186 plcsOutput();
Diletant 166:c3c0b8a90d81 187 }
Diletant 166:c3c0b8a90d81 188
Diletant 166:c3c0b8a90d81 189 void plcsProcess(void) {
Diletant 166:c3c0b8a90d81 190 if (device.plcs.detector.state.delta) {
Diletant 166:c3c0b8a90d81 191
Diletant 166:c3c0b8a90d81 192 if (device.plcs.detector.state.input) {
Diletant 166:c3c0b8a90d81 193 device.plcs.input.voltage = device.isacs.input.average;
Diletant 166:c3c0b8a90d81 194 } else if (device.plcs.detector.state.output) {
Diletant 166:c3c0b8a90d81 195 device.plcs.input.voltage = device.isacs.output.voltage;
Diletant 166:c3c0b8a90d81 196 }
Diletant 166:c3c0b8a90d81 197
Diletant 166:c3c0b8a90d81 198 device.plcs.input.sum += device.plcs.input.voltage;
Diletant 166:c3c0b8a90d81 199 if (device.measurement.counter == 31) {
Diletant 166:c3c0b8a90d81 200 if (device.plcs.input.counter == 255) {
Diletant 166:c3c0b8a90d81 201 device.plcs.input.average = device.plcs.input.sum >> 8;
Diletant 166:c3c0b8a90d81 202 device.plcs.input.counter = 0;
Diletant 166:c3c0b8a90d81 203 plcsProcessDelta();
Diletant 166:c3c0b8a90d81 204 } else {
Diletant 166:c3c0b8a90d81 205 device.plcs.input.counter++;
Diletant 166:c3c0b8a90d81 206 }
Diletant 166:c3c0b8a90d81 207 }
Diletant 166:c3c0b8a90d81 208
Diletant 166:c3c0b8a90d81 209 } else if (device.plcs.detector.state.sequencer) {
Diletant 166:c3c0b8a90d81 210
Diletant 166:c3c0b8a90d81 211 if (device.plcs.detector.state.input) {
Diletant 166:c3c0b8a90d81 212 device.plcs.input.voltage = device.isacs.input.voltage;
Diletant 166:c3c0b8a90d81 213 } else if (device.plcs.detector.state.output) {
Diletant 166:c3c0b8a90d81 214 device.plcs.input.voltage = device.isacs.output.voltage;
Diletant 166:c3c0b8a90d81 215 }
Diletant 166:c3c0b8a90d81 216 plcsProcessSequencer();
Diletant 166:c3c0b8a90d81 217
Diletant 166:c3c0b8a90d81 218 }
Diletant 149:abbf7663d27d 219 }