Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
main.cpp
- Committer:
- pyrostew
- Date:
- 2014-08-29
- Revision:
- 28:18d85029a91b
- Parent:
- 27:1d55ebab6214
File content as of revision 28:18d85029a91b:
#include "mbed.h" #include "EquatorStrutController.h" #include "LightWeightSerialTransmit.h" Timer RunningTime; LWSerialTX SerialOutput(115200); 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; EquatorStrut strut; void LinearizePower(double power) { // Compute the corrected pwm value to linearise the velocity profile double correctedPwm; if (fabs(power) < 1.0) { if (fabs(power) < pLinStep) { correctedPwm = (fabs(power) * Glin) / G1; } else { correctedPwm = pStep + (fabs(power) - pLinStep) * Glin / G2; } } else { correctedPwm = 1.0; } // Make sure our corrected value has the correct sign. if(power < 0) correctedPwm *= -1.0; if (correctedPwm > 1.0) { correctedPwm = 1.0; } else if (correctedPwm < -1.0) { correctedPwm = -1.0; } strut.SetPower(correctedPwm); } double PosError = 0; //This has been defined here as it's being used in the serial transmit function double PosKpGain = 0.0; double PosKiGain = 0.0; double PosKdGain = 0.0; void SerialTransmit() { SerialOutput.Transmit(RunningTime.read()); SerialOutput.Delimiter(LWSerialTX::Tab); SerialOutput.Transmit(strut.GetPosition()); SerialOutput.Delimiter(LWSerialTX::Tab); SerialOutput.Transmit(strut.CurrentPower()); SerialOutput.Delimiter(LWSerialTX::Tab); SerialOutput.Transmit(strut.CurrentSpeed()); SerialOutput.Delimiter(LWSerialTX::Tab); SerialOutput.Transmit(PosError); SerialOutput.Delimiter(LWSerialTX::Tab); SerialOutput.Transmit(PosKpGain); SerialOutput.NewLine(); } double SetPoint = 200.0; //Target Position in Millimeters double PosProError; double PosIntError; double PosDifError; int errorcounter; double PosPreviousError [10]; double PwmChange=0; double pwm; double TargetVelocity; double PosiState; int PreviousTime = 0; void Controller () { /////////////////////////////////////////////////////////////////////////////////////////////// //Position PID /////////////////////////////////////////////////////////////////////////////////////////////// int timeStep = RunningTime.read_us() - PreviousTime; PreviousTime = RunningTime.read_us(); double integral_velmax = vMax/PosKiGain; double integral_velmin = -vMax/PosKiGain ; PosError = SetPoint - (strut.GetPosition()); PosProError = PosError * PosKpGain; PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep; PosiState += PosError; if (PosiState > integral_velmax) { PosiState = integral_velmax; } else if (PosiState < integral_velmin) { PosiState = integral_velmin; } PosIntError = PosKiGain * PosiState; TargetPwm = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError); if (TargetPwm > 1.0) { TargetPwm = 1.0; } else if (TargetPwm < -1.0) { TargetPwm = -1.0; } LinearizePower(TargetPwm); errorcounter++; if (errorcounter > 9) { errorcounter = 0; } PosPreviousError[errorcounter] = PosError; } int main() { RunningTime.start(); strut.Home(); errorcounter = 0; PosPreviousError[errorcounter]=0; PreviousTime = RunningTime.read_us(); while(strut.IsEnabled()) { int counter = 0; PosKpGain = 0.0; while(PosKpGain < 1.0) { counter++; PosKpGain += 0.01; SerialOutput.NewFile(); RunningTime.reset(); float iterationStart = RunningTime.read(); while(RunningTime.read()-iterationStart < 10.0) { SerialTransmit(); Controller(); } strut.Home(); if (counter == 10) { counter = 0; strut.Disable(); wait(60); strut.Enable(); } } strut.Disable(); } }