Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

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?

UserRevisionLine numberNew 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 }