Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
main.cpp@28:18d85029a91b, 2014-08-29 (annotated)
- Committer:
- pyrostew
- Date:
- Fri Aug 29 11:25:03 2014 +0000
- Revision:
- 28:18d85029a91b
- Parent:
- 27:1d55ebab6214
Publishing equator control system
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pyrostew | 0:398432a37ca5 | 1 | #include "mbed.h" |
pyrostew | 26:5e4b329defec | 2 | #include "EquatorStrutController.h" |
pyrostew | 27:1d55ebab6214 | 3 | #include "LightWeightSerialTransmit.h" |
pyrostew | 0:398432a37ca5 | 4 | |
pyrostew | 0:398432a37ca5 | 5 | Timer RunningTime; |
pyrostew | 0:398432a37ca5 | 6 | |
pyrostew | 27:1d55ebab6214 | 7 | LWSerialTX SerialOutput(115200); |
pyrostew | 0:398432a37ca5 | 8 | |
pyrostew | 23:e9e2cd9c1fd1 | 9 | double vMax = 300; |
pyrostew | 23:e9e2cd9c1fd1 | 10 | double vStep = 50; |
pyrostew | 23:e9e2cd9c1fd1 | 11 | double pStep = 0.5; |
pyrostew | 23:e9e2cd9c1fd1 | 12 | double G1 = vStep / pStep; |
pyrostew | 23:e9e2cd9c1fd1 | 13 | double G2 = (vMax - vStep)/(1.0-pStep); |
pyrostew | 23:e9e2cd9c1fd1 | 14 | double Glin = vMax; |
pyrostew | 23:e9e2cd9c1fd1 | 15 | double pLinStep = pStep * G1 / Glin; |
pyrostew | 23:e9e2cd9c1fd1 | 16 | double TargetPwm; |
pyrostew | 23:e9e2cd9c1fd1 | 17 | |
pyrostew | 26:5e4b329defec | 18 | EquatorStrut strut; |
pyrostew | 15:cd409a54ceec | 19 | |
pyrostew | 26:5e4b329defec | 20 | void LinearizePower(double power) |
pyrostew | 15:cd409a54ceec | 21 | { |
pyrostew | 23:e9e2cd9c1fd1 | 22 | // Compute the corrected pwm value to linearise the velocity profile |
pyrostew | 23:e9e2cd9c1fd1 | 23 | double correctedPwm; |
pyrostew | 22:9f7dae024a81 | 24 | |
pyrostew | 25:0e4bde9e1adc | 25 | if (fabs(power) < 1.0) |
pyrostew | 23:e9e2cd9c1fd1 | 26 | { |
pyrostew | 25:0e4bde9e1adc | 27 | if (fabs(power) < pLinStep) |
pyrostew | 25:0e4bde9e1adc | 28 | { |
pyrostew | 25:0e4bde9e1adc | 29 | correctedPwm = (fabs(power) * Glin) / G1; |
pyrostew | 25:0e4bde9e1adc | 30 | } |
pyrostew | 25:0e4bde9e1adc | 31 | else |
pyrostew | 25:0e4bde9e1adc | 32 | { |
pyrostew | 25:0e4bde9e1adc | 33 | correctedPwm = pStep + (fabs(power) - pLinStep) * Glin / G2; |
pyrostew | 25:0e4bde9e1adc | 34 | } |
pyrostew | 23:e9e2cd9c1fd1 | 35 | } |
pyrostew | 23:e9e2cd9c1fd1 | 36 | else |
pyrostew | 23:e9e2cd9c1fd1 | 37 | { |
pyrostew | 25:0e4bde9e1adc | 38 | correctedPwm = 1.0; |
pyrostew | 23:e9e2cd9c1fd1 | 39 | } |
pyrostew | 22:9f7dae024a81 | 40 | |
pyrostew | 23:e9e2cd9c1fd1 | 41 | // Make sure our corrected value has the correct sign. |
pyrostew | 23:e9e2cd9c1fd1 | 42 | if(power < 0) correctedPwm *= -1.0; |
pyrostew | 23:e9e2cd9c1fd1 | 43 | |
pyrostew | 26:5e4b329defec | 44 | if (correctedPwm > 1.0) |
pyrostew | 26:5e4b329defec | 45 | { |
pyrostew | 26:5e4b329defec | 46 | correctedPwm = 1.0; |
pyrostew | 26:5e4b329defec | 47 | } |
pyrostew | 0:398432a37ca5 | 48 | |
pyrostew | 26:5e4b329defec | 49 | else if (correctedPwm < -1.0) |
pyrostew | 26:5e4b329defec | 50 | { |
pyrostew | 26:5e4b329defec | 51 | correctedPwm = -1.0; |
pyrostew | 26:5e4b329defec | 52 | } |
pyrostew | 0:398432a37ca5 | 53 | |
pyrostew | 26:5e4b329defec | 54 | strut.SetPower(correctedPwm); |
pyrostew | 1:a33723b70582 | 55 | } |
pyrostew | 1:a33723b70582 | 56 | |
alpesh | 13:18c376e5dc9a | 57 | double PosError = 0; //This has been defined here as it's being used in the serial transmit function |
alpesh | 6:bfe745b152fa | 58 | |
alpesh | 13:18c376e5dc9a | 59 | double PosKpGain = 0.0; |
alpesh | 13:18c376e5dc9a | 60 | double PosKiGain = 0.0; |
alpesh | 13:18c376e5dc9a | 61 | double PosKdGain = 0.0; |
alpesh | 13:18c376e5dc9a | 62 | |
pyrostew | 12:814db1249a19 | 63 | void SerialTransmit() |
pyrostew | 12:814db1249a19 | 64 | { |
pyrostew | 27:1d55ebab6214 | 65 | SerialOutput.Transmit(RunningTime.read()); |
pyrostew | 27:1d55ebab6214 | 66 | SerialOutput.Delimiter(LWSerialTX::Tab); |
pyrostew | 0:398432a37ca5 | 67 | |
pyrostew | 27:1d55ebab6214 | 68 | SerialOutput.Transmit(strut.GetPosition()); |
pyrostew | 27:1d55ebab6214 | 69 | SerialOutput.Delimiter(LWSerialTX::Tab); |
pyrostew | 12:814db1249a19 | 70 | |
pyrostew | 27:1d55ebab6214 | 71 | SerialOutput.Transmit(strut.CurrentPower()); |
pyrostew | 27:1d55ebab6214 | 72 | SerialOutput.Delimiter(LWSerialTX::Tab); |
pyrostew | 12:814db1249a19 | 73 | |
pyrostew | 27:1d55ebab6214 | 74 | SerialOutput.Transmit(strut.CurrentSpeed()); |
pyrostew | 27:1d55ebab6214 | 75 | SerialOutput.Delimiter(LWSerialTX::Tab); |
pyrostew | 12:814db1249a19 | 76 | |
pyrostew | 27:1d55ebab6214 | 77 | SerialOutput.Transmit(PosError); |
pyrostew | 27:1d55ebab6214 | 78 | SerialOutput.Delimiter(LWSerialTX::Tab); |
alpesh | 6:bfe745b152fa | 79 | |
pyrostew | 27:1d55ebab6214 | 80 | SerialOutput.Transmit(PosKpGain); |
pyrostew | 27:1d55ebab6214 | 81 | SerialOutput.NewLine(); |
pyrostew | 0:398432a37ca5 | 82 | } |
pyrostew | 0:398432a37ca5 | 83 | |
pyrostew | 27:1d55ebab6214 | 84 | double SetPoint = 200.0; //Target Position in Millimeters |
alpesh | 6:bfe745b152fa | 85 | |
alpesh | 13:18c376e5dc9a | 86 | double PosProError; |
alpesh | 13:18c376e5dc9a | 87 | double PosIntError; |
alpesh | 13:18c376e5dc9a | 88 | double PosDifError; |
alpesh | 13:18c376e5dc9a | 89 | |
alpesh | 3:e693c65b04de | 90 | int errorcounter; |
alpesh | 13:18c376e5dc9a | 91 | double PosPreviousError [10]; |
alpesh | 2:d1805e7d46fb | 92 | |
alpesh | 3:e693c65b04de | 93 | double PwmChange=0; |
alpesh | 3:e693c65b04de | 94 | |
alpesh | 3:e693c65b04de | 95 | double pwm; |
pyrostew | 23:e9e2cd9c1fd1 | 96 | |
alpesh | 13:18c376e5dc9a | 97 | double TargetVelocity; |
alpesh | 3:e693c65b04de | 98 | |
alpesh | 13:18c376e5dc9a | 99 | double PosiState; |
alpesh | 13:18c376e5dc9a | 100 | |
alpesh | 13:18c376e5dc9a | 101 | int PreviousTime = 0; |
alpesh | 13:18c376e5dc9a | 102 | |
alpesh | 4:2ec05810bc47 | 103 | void Controller () |
pyrostew | 12:814db1249a19 | 104 | { |
alpesh | 13:18c376e5dc9a | 105 | /////////////////////////////////////////////////////////////////////////////////////////////// |
alpesh | 13:18c376e5dc9a | 106 | //Position PID |
alpesh | 13:18c376e5dc9a | 107 | /////////////////////////////////////////////////////////////////////////////////////////////// |
pyrostew | 26:5e4b329defec | 108 | int timeStep = RunningTime.read_us() - PreviousTime; |
pyrostew | 26:5e4b329defec | 109 | PreviousTime = RunningTime.read_us(); |
pyrostew | 12:814db1249a19 | 110 | |
pyrostew | 26:5e4b329defec | 111 | double integral_velmax = vMax/PosKiGain; |
pyrostew | 26:5e4b329defec | 112 | double integral_velmin = -vMax/PosKiGain ; |
pyrostew | 26:5e4b329defec | 113 | |
pyrostew | 26:5e4b329defec | 114 | PosError = SetPoint - (strut.GetPosition()); |
alpesh | 13:18c376e5dc9a | 115 | |
pyrostew | 26:5e4b329defec | 116 | PosProError = PosError * PosKpGain; |
pyrostew | 12:814db1249a19 | 117 | |
pyrostew | 26:5e4b329defec | 118 | PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep; |
pyrostew | 26:5e4b329defec | 119 | |
pyrostew | 26:5e4b329defec | 120 | PosiState += PosError; |
pyrostew | 26:5e4b329defec | 121 | |
pyrostew | 26:5e4b329defec | 122 | if (PosiState > integral_velmax) |
pyrostew | 26:5e4b329defec | 123 | { |
pyrostew | 26:5e4b329defec | 124 | PosiState = integral_velmax; |
pyrostew | 26:5e4b329defec | 125 | } |
pyrostew | 26:5e4b329defec | 126 | else if (PosiState < integral_velmin) |
pyrostew | 26:5e4b329defec | 127 | { |
pyrostew | 26:5e4b329defec | 128 | PosiState = integral_velmin; |
pyrostew | 26:5e4b329defec | 129 | } |
pyrostew | 26:5e4b329defec | 130 | PosIntError = PosKiGain * PosiState; |
pyrostew | 26:5e4b329defec | 131 | |
pyrostew | 26:5e4b329defec | 132 | TargetPwm = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError); |
pyrostew | 22:9f7dae024a81 | 133 | |
pyrostew | 26:5e4b329defec | 134 | |
pyrostew | 26:5e4b329defec | 135 | if (TargetPwm > 1.0) |
pyrostew | 26:5e4b329defec | 136 | { |
pyrostew | 26:5e4b329defec | 137 | TargetPwm = 1.0; |
pyrostew | 26:5e4b329defec | 138 | } |
pyrostew | 26:5e4b329defec | 139 | |
pyrostew | 26:5e4b329defec | 140 | else if (TargetPwm < -1.0) |
pyrostew | 26:5e4b329defec | 141 | { |
pyrostew | 26:5e4b329defec | 142 | TargetPwm = -1.0; |
pyrostew | 26:5e4b329defec | 143 | } |
pyrostew | 12:814db1249a19 | 144 | |
pyrostew | 27:1d55ebab6214 | 145 | LinearizePower(TargetPwm); |
pyrostew | 26:5e4b329defec | 146 | |
pyrostew | 26:5e4b329defec | 147 | errorcounter++; |
pyrostew | 12:814db1249a19 | 148 | |
pyrostew | 26:5e4b329defec | 149 | if (errorcounter > 9) |
pyrostew | 26:5e4b329defec | 150 | { |
pyrostew | 26:5e4b329defec | 151 | errorcounter = 0; |
pyrostew | 26:5e4b329defec | 152 | } |
pyrostew | 12:814db1249a19 | 153 | |
pyrostew | 26:5e4b329defec | 154 | PosPreviousError[errorcounter] = PosError; |
pyrostew | 12:814db1249a19 | 155 | } |
alpesh | 4:2ec05810bc47 | 156 | |
pyrostew | 0:398432a37ca5 | 157 | int main() |
pyrostew | 26:5e4b329defec | 158 | { |
pyrostew | 0:398432a37ca5 | 159 | RunningTime.start(); |
pyrostew | 0:398432a37ca5 | 160 | |
pyrostew | 26:5e4b329defec | 161 | strut.Home(); |
pyrostew | 0:398432a37ca5 | 162 | |
pyrostew | 26:5e4b329defec | 163 | errorcounter = 0; |
pyrostew | 22:9f7dae024a81 | 164 | PosPreviousError[errorcounter]=0; |
pyrostew | 22:9f7dae024a81 | 165 | PreviousTime = RunningTime.read_us(); |
pyrostew | 22:9f7dae024a81 | 166 | |
pyrostew | 26:5e4b329defec | 167 | while(strut.IsEnabled()) |
pyrostew | 22:9f7dae024a81 | 168 | { |
pyrostew | 26:5e4b329defec | 169 | int counter = 0; |
pyrostew | 26:5e4b329defec | 170 | PosKpGain = 0.0; |
pyrostew | 27:1d55ebab6214 | 171 | |
pyrostew | 26:5e4b329defec | 172 | while(PosKpGain < 1.0) |
pyrostew | 22:9f7dae024a81 | 173 | { |
pyrostew | 26:5e4b329defec | 174 | counter++; |
pyrostew | 22:9f7dae024a81 | 175 | |
pyrostew | 26:5e4b329defec | 176 | PosKpGain += 0.01; |
pyrostew | 26:5e4b329defec | 177 | |
pyrostew | 27:1d55ebab6214 | 178 | SerialOutput.NewFile(); |
pyrostew | 26:5e4b329defec | 179 | |
pyrostew | 26:5e4b329defec | 180 | RunningTime.reset(); |
pyrostew | 22:9f7dae024a81 | 181 | |
pyrostew | 26:5e4b329defec | 182 | float iterationStart = RunningTime.read(); |
pyrostew | 26:5e4b329defec | 183 | |
pyrostew | 26:5e4b329defec | 184 | while(RunningTime.read()-iterationStart < 10.0) |
pyrostew | 26:5e4b329defec | 185 | { |
pyrostew | 26:5e4b329defec | 186 | SerialTransmit(); |
pyrostew | 26:5e4b329defec | 187 | |
pyrostew | 26:5e4b329defec | 188 | Controller(); |
pyrostew | 26:5e4b329defec | 189 | } |
pyrostew | 26:5e4b329defec | 190 | |
pyrostew | 26:5e4b329defec | 191 | strut.Home(); |
pyrostew | 26:5e4b329defec | 192 | |
pyrostew | 26:5e4b329defec | 193 | if (counter == 10) |
pyrostew | 26:5e4b329defec | 194 | { |
pyrostew | 26:5e4b329defec | 195 | counter = 0; |
pyrostew | 26:5e4b329defec | 196 | strut.Disable(); |
pyrostew | 26:5e4b329defec | 197 | wait(60); |
pyrostew | 26:5e4b329defec | 198 | strut.Enable(); |
pyrostew | 26:5e4b329defec | 199 | } |
pyrostew | 22:9f7dae024a81 | 200 | } |
pyrostew | 22:9f7dae024a81 | 201 | |
pyrostew | 26:5e4b329defec | 202 | strut.Disable(); |
pyrostew | 22:9f7dae024a81 | 203 | } |
pyrostew | 26:5e4b329defec | 204 | } |