Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
main.cpp
- Committer:
- pyrostew
- Date:
- 2014-08-20
- Revision:
- 26:5e4b329defec
- Parent:
- 25:0e4bde9e1adc
- Child:
- 27:1d55ebab6214
File content as of revision 26:5e4b329defec:
#include "mbed.h" #include "RawSerial.h" #include "EquatorStrutController.h" Timer RunningTime; RawSerial pc(P1_27, P1_26); 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; 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 void SerialOut(double outputValue) { int outChar = 0; if (outputValue < 0.0) { pc.putc('-'); outputValue *= -1.0; } if (outputValue >= 1000.0) { outChar = outputValue / 1000; pc.putc(outChar + 48); outputValue -= outChar * 1000.0; } if (outputValue >= 100.0) { outChar = outputValue / 100; pc.putc(outChar + 48); outputValue -= outChar * 100.0; } else if(outChar > 0) { pc.putc('0'); } if (outputValue >= 10.0) { outChar = outputValue / 10; pc.putc(outChar + 48); outputValue -= outChar * 10.0; } else if(outChar > 0) { pc.putc('0'); } if (outputValue >= 1.0) { outChar = outputValue; pc.putc(outChar + 48); outputValue -= outChar; } else { pc.putc('0'); } if (outputValue >= 0.1) { pc.putc('.'); outChar = outputValue * 10; pc.putc(outChar + 48); outputValue -= (double)outChar / 10.0; } else { pc.putc('.'); pc.putc('0'); } if (outputValue >= 0.01) { outChar = outputValue * 100; pc.putc(outChar + 48); outputValue -= (double)outChar / 100.0; } else { pc.putc('0'); } if (outputValue >= 0.001) { outChar= outputValue * 1000; pc.putc(outChar + 48); } } double PosKpGain = 0.0; double PosKiGain = 0.0; double PosKdGain = 0.0; void SerialTransmit() { SerialOut(RunningTime.read()); pc.putc('\t'); SerialOut(strut.GetPosition()); pc.putc('\t'); SerialOut(strut.CurrentPower()); pc.putc('\t'); SerialOut(strut.CurrentSpeed()); pc.putc('\t'); SerialOut(PosError); pc.putc('\t'); SerialOut(PosKpGain); pc.putc('\t'); pc.putc(10); pc.putc(13); } void SerialNewFile() { pc.putc(28); pc.putc(10); pc.putc(13); } double SetPoint = 150.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; } strut.SetPower(TargetPwm); errorcounter++; if (errorcounter > 9) { errorcounter = 0; } PosPreviousError[errorcounter] = PosError; } int main() { RunningTime.start(); pc.baud(115200); strut.Home(); //Enable(); errorcounter = 0; PosPreviousError[errorcounter]=0; PreviousTime = RunningTime.read_us(); while(strut.IsEnabled()) { int counter = 0; //double pow = 0.0; //while(pow < 1.0) PosKpGain = 0.0; while(PosKpGain < 1.0) { counter++; //pow += 0.05; PosKpGain += 0.01; SerialNewFile(); RunningTime.reset(); float iterationStart = RunningTime.read(); while(RunningTime.read()-iterationStart < 10.0) { SerialTransmit(); Controller(); } //SetPower(pow); //while(position < 21000 && RunningTime.read()-iterationStart < 30.0) //{ // SerialTransmit(); //} //iterationStart = RunningTime.read(); //SerialNewFile(); //SetPower(-pow); //while(position > 1000 && RunningTime.read()-iterationStart < 30.0) //{ // SerialTransmit(); //} //SerialNewFile(); strut.Home(); if (counter == 10) { counter = 0; strut.Disable(); wait(60); strut.Enable(); } } strut.Disable(); } }