Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
main.cpp
- Committer:
- alpesh
- Date:
- 2014-08-11
- Revision:
- 8:7f6e81140f27
- Parent:
- 7:4cd7be306626
- Child:
- 9:ee84a9697de0
File content as of revision 8:7f6e81140f27:
#include "mbed.h" #include "RawSerial.h" DigitalIn RGHSinState(P0_11); DigitalIn RGHCosState(P0_12); InterruptIn RGHSinInterrupt(P0_11); InterruptIn RGHCosInterrupt(P0_12); InterruptIn HallSensor(P0_2); DigitalOut ResetLine(P1_29); PwmOut PhaseA(P0_9); PwmOut PhaseB(P0_8); Timer RunningTime; bool Enabled = false; bool Homing = false; bool HallTriggered = false; RawSerial pc(P1_27, P1_26); char PinState = 0; int direction = 0; double position = 0.0; double currentPower = 0.0; int interruptPeriod = 0; int lastTime = 0; char counter = 0; void RGHSinHandler() { if (PinState == 2) { return; } else if (PinState == 1) { PinState = 0 |(RGHSinState << 1) | RGHCosState; if(PinState == 3) { direction = 1; position += 0.04 * direction; interruptPeriod = RunningTime.read_us() - lastTime; lastTime = RunningTime.read_us(); } } else { PinState = 0 |(RGHSinState << 1) | RGHCosState; } } void RGHCosHandler() { if (PinState == 1) { return; } else if (PinState == 2) { PinState = 0 |(RGHSinState << 1) | RGHCosState; if (PinState == 3) { direction = -1; position += 0.04 * direction; interruptPeriod = RunningTime.read_us() - lastTime; lastTime = RunningTime.read_us(); } } else { PinState = 0 |(RGHSinState << 1) | RGHCosState; } } 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; } void Home() { if (!Enabled) { Enable(); } Homing = true; HallTriggered = false; SetPower(-1.0); while (!HallTriggered) { wait(0.5); } SetPower(1.0); while (position < 20.0) { } Homing = true; HallTriggered = false; SetPower(-0.7); while (!HallTriggered) { wait(0.5); } } double GetSpeed() { if ((RunningTime - lastTime) > 10000 || interruptPeriod == 0) { return 0.0; } return (direction * 0.04)/((double)interruptPeriod / 1000000.0); } double Error = 0; void SerialTransmit() { double tempPos = position; double tempTime = RunningTime.read(); double tempPow = currentPower; double tempSpeed = GetSpeed(); double tempError = Error; int outChar = 0; if (tempPos < 0.0) { pc.putc('-'); tempPos *= -1; } if (tempPos >= 100.0) { outChar = tempPos / 100; pc.putc(outChar + 48); tempPos -= outChar * 100.0; } if (tempPos >= 10.0) { outChar = tempPos / 10; pc.putc(outChar + 48); tempPos -= outChar * 10.0; } else if(outChar > 0) { pc.putc('0'); } if (tempPos >= 1.0) { outChar = tempPos; pc.putc(outChar + 48); tempPos -= outChar; } else { pc.putc('0'); } if (tempPos >= 0.1) { pc.putc('.'); outChar = tempPos * 10; pc.putc(outChar + 48); tempPos -= (double)outChar / 10.0; } else { pc.putc('.'); pc.putc('0'); } if (tempPos >= 0.01) { outChar = tempPos * 100; pc.putc(outChar + 48); } pc.putc('\t'); outChar = 0; if (tempTime >= 100.0) { outChar = tempTime / 100; pc.putc(outChar + 48); tempTime -= outChar * 100.0; } if (tempTime >= 10.0) { outChar = tempTime / 10; pc.putc(outChar + 48); tempTime -= outChar * 10.0; } else if(outChar > 0) { pc.putc('0'); } if (tempTime >= 1.0) { outChar = tempTime; pc.putc(outChar + 48); tempTime -= outChar; } else { pc.putc('0'); } if (tempTime >= 0.1) { pc.putc('.'); outChar = tempTime * 10; pc.putc(outChar + 48); tempTime -= (double)outChar / 10.0; } else { pc.putc('.'); pc.putc('0'); } if (tempTime >= 0.01) { outChar = tempTime * 100; pc.putc(outChar + 48); tempTime -= (double)outChar / 100.0; } else { pc.putc('0'); } if (tempTime >= 0.001) { outChar= tempTime * 1000; pc.putc(outChar + 48); } pc.putc('\t'); outChar = 0; if (tempPow < 0.0) { pc.putc('-'); tempPow *= -1; } if (tempPow >= 1.0) { outChar = tempPow; pc.putc(outChar + 48); tempPow -= outChar; } else { pc.putc('0'); } if (tempPow >= 0.1) { pc.putc('.'); outChar = tempPow * 10; pc.putc(outChar + 48); tempPow -= (double)outChar / 10.0; } else { pc.putc('.'); pc.putc('0'); } if (tempPow >= 0.01) { outChar = tempPow * 100; pc.putc(outChar + 48); tempPow -= (double)outChar / 100.0; } else { pc.putc('0'); } if (tempPow >= 0.001) { outChar= tempPow * 1000; pc.putc(outChar + 48); } pc.putc('\t'); outChar = 0; if (tempSpeed < 0.0) { pc.putc('-'); tempSpeed *= -1; } if (tempSpeed >= 100.0) { outChar = tempSpeed / 100; pc.putc(outChar + 48); tempSpeed -= outChar * 100.0; } if (tempSpeed >= 10.0) { outChar = tempSpeed / 10; pc.putc(outChar + 48); tempSpeed -= outChar * 10.0; } else if(outChar > 0) { pc.putc('0'); } if (tempSpeed >= 1.0) { outChar = tempSpeed; pc.putc(outChar + 48); tempSpeed -= outChar; } else { pc.putc('0'); } if (tempSpeed >= 0.1) { pc.putc('.'); outChar = tempSpeed * 10; pc.putc(outChar + 48); tempSpeed -= (double)outChar / 10.0; } else { pc.putc('.'); pc.putc('0'); } if (tempSpeed >= 0.01) { outChar = tempSpeed * 100; pc.putc(outChar + 48); } pc.putc('\t'); outChar = 0; if (tempError < 0.0) { pc.putc('-'); tempError *= -1; } if (tempError >= 100.0) { outChar = tempError / 100; pc.putc(outChar + 48); tempError -= outChar * 100.0; } if (tempError >= 10.0) { outChar = tempError / 10; pc.putc(outChar + 48); tempError -= outChar * 10.0; } else if(outChar > 0) { pc.putc('0'); } if (tempError >= 1.0) { outChar = tempError; pc.putc(outChar + 48); tempError -= outChar; } else { pc.putc('0'); } if (tempError >= 0.1) { pc.putc('.'); outChar = tempError * 10; pc.putc(outChar + 48); tempError -= (double)outChar / 10.0; } else { pc.putc('.'); pc.putc('0'); } if (tempError >= 0.01) { outChar = tempError * 100; pc.putc(outChar + 48); } pc.putc(10); pc.putc(13); } void HallEffectFall() { RGHSinInterrupt.disable_irq(); RGHCosInterrupt.disable_irq(); if (direction < 0) { SetPower(0.0); if (Homing) { HallTriggered = true; Homing = false; position = 0.0; } } RGHSinInterrupt.enable_irq(); RGHCosInterrupt.enable_irq(); } double SetPoint = 50.0; //Millimeter per second double KpTerm; double ErrorInt; double ErrorDer; double KpGain = 0.0; double KiGain = 0.0; double KdGain = 0.0; int errorcounter; double PreviousError [10]; double PwmChange=0; double pwm; double NewPwm; int previousTime = 0; void Controller () { if (position <= SetPoint || 1) { int timeStep = RunningTime.read_us() - previousTime; previousTime = RunningTime.read_us(); Error = SetPoint - position; KpTerm = Error * KpGain; ErrorDer = (Error - PreviousError[errorcounter]) / timeStep; ErrorInt = ErrorInt + Error * timeStep; NewPwm = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer); if (NewPwm > 1.0) { pwm = 1.0; } else if (NewPwm < -1.0) { pwm = -1.0; } else { pwm = NewPwm; } SetPower(pwm); errorcounter ++; if (errorcounter > 9) { errorcounter = 0; } PreviousError[errorcounter] = Error; } } int main() { RGHSinInterrupt.rise(&RGHSinHandler); RGHCosInterrupt.rise(&RGHCosHandler); HallSensor.fall(&HallEffectFall); HallSensor.mode(PullUp); RunningTime.start(); pc.baud(115200); Home(); //Enable(); errorcounter = 0; PreviousError[errorcounter]=0; previousTime = RunningTime.read_us(); while(Enabled) { double pow = 0.5; for (int i = 0; i < 10; i++) { //KpGain += 0.1; float iterationStart = RunningTime.read(); pow += 0.05; //while(RunningTime.read()-iterationStart < 10.0) //{ // SerialTransmit(); // Controller(); //} while(RunningTime.read() - iterationStart < 30.0 && position < 210.0) { SerialTransmit(); SetPower(pow); } iterationStart = RunningTime.read(); while(RunningTime.read() - iterationStart < 30.0 && position > 10.0) { SerialTransmit(); SetPower(-pow); } 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_ErrorDer + m_ErrorInt); 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; }*/