123
Fork of LG by
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?
User | Revision | Line number | New 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 | } |