Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
main.cpp
- Committer:
- pyrostew
- Date:
- 2014-08-14
- Revision:
- 18:ab282713f4a7
- Parent:
- 17:f54cdc9ae52f
- Child:
- 19:a6369257c00f
File content as of revision 18:ab282713f4a7:
#include "mbed.h" #include "RawSerial.h" DigitalIn HallSensorState(P0_2); InterruptIn RGHSinInterrupt(P0_11); InterruptIn RGHCosInterrupt(P0_12); InterruptIn RGHSinFallingInterrupt(P0_13); InterruptIn RGHCosFallingInterrupt(P0_14); InterruptIn HallSensor(P0_2); DigitalOut ResetLine(P1_29); DigitalOut PulseOut(P1_22); PwmOut PhaseA(P0_9); PwmOut PhaseB(P0_8); Timer RunningTime; bool Enabled = false; volatile bool Homing = false; volatile bool HallTriggered = false; RawSerial pc(P1_27, P1_26); volatile int direction = 1; volatile int position = 0; double currentPower = 0.0; int lastTime = 0; double SpeedInterval = 0.0; int LastPosition = 0; char counter = 0; volatile bool SinHigh = false; volatile bool CosHigh = false; volatile bool LastSin = false; volatile bool LastHigh = false; void ActionEvent(bool CurrHigh, bool CurrSin) { // Same event again - DO NOTHING if (CurrHigh == LastHigh && CurrSin == LastSin) { return; } if (CurrSin != LastSin) // Otherwave { // Other wave if ((CurrSin && CurrHigh == LastHigh) || (!CurrSin && CurrHigh != LastHigh)) { //Forwards direction = 1; } else { //Backwards direction = -1; } } else { // Reversal direction = -direction; } position += direction; // Set the state for the wave that fired if(CurrSin) SinHigh = CurrHigh; else CosHigh = CurrHigh; // Set the last event values LastHigh = CurrHigh; LastSin = CurrSin; } void RGHSinRisingHandler() { PulseOut = 1; ActionEvent(true, true); PulseOut = 0; } void RGHSinFallingHandler() { ActionEvent(false, true); } void RGHCosRisingHandler() { ActionEvent(true, false); } void RGHCosFallingHandler() { ActionEvent(false, false); } void SetPower(double power) { currentPower = power; if(!Enabled) { return; } if (power > 1.0 || power < -1.0) { return; } PhaseA = (power + 1.0) / 2; PhaseB = 1.0 - ((power + 1.0) / 2); } void Enable() { SetPower(0.0); ResetLine = 1; Enabled = true; } void Disable() { ResetLine = 0; SetPower(0.0); Enabled = false; } double GetSpeed() { double interval = RunningTime.read() - SpeedInterval; int positionDiff = position - LastPosition; SpeedInterval = RunningTime.read(); LastPosition = position; return (positionDiff * 0.01)/interval; } double PosError = 0; //This has been defined here as it's being used in the serial transmit function double VelError = 0; 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; double VelKpGain = 0.01; double VelKiGain = 0.0; double VelKdGain = 0.0; void SerialTransmit() { SerialOut(RunningTime.read()); pc.putc('\t'); SerialOut((double)position*0.01); pc.putc('\t'); SerialOut(currentPower); pc.putc('\t'); SerialOut(GetSpeed()); pc.putc('\t'); SerialOut(PosError); pc.putc('\t'); SerialOut(PosKpGain); pc.putc('\t'); SerialOut(VelKpGain); //pc.putc('\t'); //SerialOut(PosKdGain); pc.putc(10); pc.putc(13); } void Home() { if (!Enabled) { Enable(); } if (HallSensorState == 1) { Homing = true; HallTriggered = false; SetPower(-0.6); while (!HallTriggered) { wait(0.1); } } SetPower(1.0); while (position < 2000) { //SerialTransmit(); } Homing = true; HallTriggered = false; SetPower(-0.4); while (!HallTriggered) { wait(0.1); } } void HallEffectFall() { RGHSinInterrupt.disable_irq(); RGHCosInterrupt.disable_irq(); RGHSinFallingInterrupt.disable_irq(); RGHCosFallingInterrupt.disable_irq(); if (direction < 0) { SetPower(0.0); if (Homing) { HallTriggered = true; Homing = false; position = 0.0; } } RGHSinInterrupt.enable_irq(); RGHCosInterrupt.enable_irq(); RGHSinFallingInterrupt.enable_irq(); RGHCosFallingInterrupt.enable_irq(); } double SetPoint = 70.0; //Target Position in Millimeter per second double PosProError; double PosIntError; double PosDifError; double VelProError; double VelIntError; double VelDifError; int errorcounter; double PosPreviousError [10]; double VelPreviousError [10]; double PwmChange=0; double pwm; double TargetPwm; double TargetVelocity; double PosiState; double VeliState; int PreviousTime = 0; double vMax = 20; void Controller () { /////////////////////////////////////////////////////////////////////////////////////////////// //Position PID /////////////////////////////////////////////////////////////////////////////////////////////// //if (position <= SetPoint || 1) //{ int timeStep = RunningTime.read_us() - PreviousTime; PreviousTime = RunningTime.read_us(); double integral_velmax = vMax/VelKiGain; double integral_velmin = -vMax/VelKiGain ; PosError = SetPoint - (position * 0.01); 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; TargetVelocity = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError); /////////////////////////////////////////////////////////////////////////////////////////////// //Velocity PID /////////////////////////////////////////////////////////////////////////////////////////////// double integral_pwmmax = 1.0/VelKiGain; double integral_pwmmin = -1.0/VelKiGain; VelError = TargetVelocity - GetSpeed(); VelProError = VelError * VelKpGain; VelDifError = (VelError - VelPreviousError[errorcounter]) / timeStep; //Calculate the integral state with appropriate limiting VeliState += VelError; if (VeliState > integral_pwmmax) { VeliState = integral_pwmmax; } else if (VeliState < integral_pwmmin) { VeliState = integral_pwmmin; } VelIntError = VelKiGain * VeliState; TargetPwm = (VelKpGain * VelError + VelKdGain * VelDifError + VelIntError); if (TargetPwm > 1.0) { pwm = 1.0; } else if (TargetPwm < -1.0) { pwm = -1.0; } else { pwm = TargetPwm; } SetPower(pwm); errorcounter ++; if (errorcounter > 9) { errorcounter = 0; } PosPreviousError[errorcounter] = PosError; VelPreviousError[errorcounter] = VelError; // } } int main() { RGHSinInterrupt.rise(&RGHSinRisingHandler); RGHCosInterrupt.rise(&RGHCosRisingHandler); RGHSinFallingInterrupt.fall(&RGHSinFallingHandler); RGHCosFallingInterrupt.fall(&RGHCosFallingHandler); HallSensor.fall(&HallEffectFall); HallSensor.mode(PullUp); RGHSinFallingInterrupt.mode(PullNone); RGHCosFallingInterrupt.mode(PullNone); RunningTime.start(); pc.baud(115200); Home(); //Enable(); errorcounter = 0; PosPreviousError[errorcounter]=0; PreviousTime = RunningTime.read_us(); while(Enabled) { //double pow = 0.4; //while(pow < 1.0) PosKpGain = 10.0; while(PosKpGain < 50.0) { //pow += 0.05; PosKpGain += 1.0; VelKpGain = 0.003; while (VelKpGain < 0.008) { VelKpGain += 0.001; float iterationStart = RunningTime.read(); while(RunningTime.read()-iterationStart < 10.0) { SerialTransmit(); Controller(); } Home(); } } Disable(); } } /* Change this throttle value range to 1.0 and 0.0 for car speed m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError); double throttle = m_throttle + m_throttlechange; if (new_throttle > 1.0) { m_throttle = 1.0; } else if (new_throttle < 0.0) { m_throttle = 0.0; } else { m_throttle = new_throttle; }*/