Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
Revision 23:e9e2cd9c1fd1, committed 2014-08-15
- Comitter:
- pyrostew
- Date:
- Fri Aug 15 13:29:14 2014 +0000
- Parent:
- 22:9f7dae024a81
- Child:
- 24:214f2d426484
- Commit message:
- Velocity Profile fixed
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Aug 15 10:26:49 2014 +0000 +++ b/main.cpp Fri Aug 15 13:29:14 2014 +0000 @@ -27,6 +27,15 @@ double SpeedInterval = 0.0; int LastPosition = 0; +double vMax = 300; +double vStep = 50; +double pStep = 0.5; +double G1 = vStep / pStep; +double G2 = (vMax - vStep)/(1.0-pStep); +double Glin = vMax; +double pLinStep = pStep * G1 / Glin; +double TargetPwm; + char counter = 0; volatile bool SinHigh = false; @@ -110,15 +119,37 @@ return; } + // Linear clamping - Linear clamping + // Compute the corrected pwm value to linearise the velocity profile + double correctedPwm; - + if (fabs(power) < pLinStep) + { + correctedPwm = (fabs(power) * Glin) / G1; + } + else + { + correctedPwm = pStep + (fabs(power) - pLinStep) * Glin / G2; + } - currentPower = power; + // Make sure our corrected value has the correct sign. + if(power < 0) correctedPwm *= -1.0; + + if (correctedPwm > 1.0) + { + correctedPwm = 1.0; + } - PhaseA = (power + 1.0) / 2; - PhaseB = 1.0 - ((power + 1.0) / 2); + else if (correctedPwm < -1.0) + { + correctedPwm = -1.0; + } + + currentPower = correctedPwm; + + PhaseA = (correctedPwm + 1.0) / 2; + PhaseB = 1.0 - ((correctedPwm + 1.0) / 2); } void Enable() @@ -271,6 +302,13 @@ pc.putc(13); } +void SerialNewFile() +{ + pc.putc(28); + pc.putc(10); + pc.putc(13); +} + void Home() { if (!Enabled) @@ -351,7 +389,8 @@ double PwmChange=0; double pwm; -double TargetPwm; + + double TargetVelocity; double PosiState; @@ -359,12 +398,7 @@ int PreviousTime = 0; -double vMax = 300; -double vStep = 50; -double pStep = 0.5; -double G1 = vStep / pStep; -double G2 = (vMax - vStep)/(1.0-pStep); -double Glin = vMax; + void Controller () { @@ -413,28 +447,7 @@ { TargetPwm = -1.0; } - - - // Compute the corrected pwm value to linearise the velocity profile - double correctedPwm; - if (fabs(TargetPwm) < pStep) - { - correctedPwm = (fabs(TargetPwm) * Glin) / G1; - } - - if (fabs(TargetPwm) > pStep) - { - correctedPwm = pStep + (fabs(TargetPwm) - pStep) * Glin / G2; - } - - // Make sure our corrected value has the correct sign. - if(TargetPwm < 0) correctedPwm *= -1.0; - - SetPower(correctedPwm); - - - errorcounter ++; if (errorcounter > 9) @@ -443,7 +456,6 @@ } PosPreviousError[errorcounter] = PosError; - // VelPreviousError[errorcounter] = VelError; // } } @@ -474,7 +486,7 @@ while(Enabled) { - double pow = 0.4; + double pow = 0.0; while(pow < 1.0) // PosKpGain = 10.0; // while(PosKpGain < 50.0) @@ -489,13 +501,25 @@ float iterationStart = RunningTime.read(); - while(RunningTime.read()-iterationStart < 10.0) + + SetPower(pow); + + while(position < 21000 && RunningTime.read()-iterationStart < 30.0) { SerialTransmit(); - - Controller(); } + iterationStart = RunningTime.read(); + SerialNewFile(); + + SetPower(-pow); + + while(position > 1000 && RunningTime.read()-iterationStart < 30.0) + { + SerialTransmit(); + } + + SerialNewFile(); Home(); // } }