Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
main.cpp
- Committer:
- pyrostew
- Date:
- 2014-08-13
- Revision:
- 12:814db1249a19
- Parent:
- 11:b6958b3dbddf
- Child:
- 13:18c376e5dc9a
- Child:
- 15:cd409a54ceec
File content as of revision 12:814db1249a19:
#include "mbed.h" #include "RawSerial.h" DigitalIn RGHSinState(P0_11); DigitalIn RGHCosState(P0_12); DigitalIn HallSensorState(P0_2); 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; volatile bool Homing = false; volatile bool HallTriggered = false; RawSerial pc(P1_27, P1_26); volatile char PinState = 0; volatile int direction = 0; volatile double position = 0.0; double currentPower = 0.0; volatile int lastTime = 0; const int arraySize = 50; volatile int intteruptPeriodArray[arraySize]; volatile int arrayTotal = 0; volatile char arrayPos = 0; volatile int interruptPeriod = 0; char counter = 0; void SmoothingAdd(int input) { //arrayTotal -= intteruptPeriodArray[arrayPos]; //arrayTotal += input; intteruptPeriodArray[arrayPos] = input * direction; if (arrayPos == arraySize-1) { arrayPos = 0; } else { arrayPos++; } //interruptPeriod = arrayTotal / 15; } int SmoothedInterruptPeriod() { int arrayTotal = 0; for (char i = 0; i < arraySize; i++) { arrayTotal += intteruptPeriodArray[i]; } return arrayTotal / arraySize; } void RGHSinHandler() { if (PinState == 2) { return; } else if (PinState == 1) { PinState = 0 |(RGHSinState << 1) | RGHCosState; if(PinState == 3) { direction = 1; position += 0.04 * direction; SmoothingAdd(RunningTime.read_us() - lastTime); 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; SmoothingAdd(RunningTime.read_us() - lastTime); 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(); } if (HallSensorState == 1) { Homing = true; HallTriggered = false; SetPower(-0.6); while (!HallTriggered) { wait(0.5); } } SetPower(1.0); while (position < 20.0) { } Homing = true; HallTriggered = false; SetPower(-0.4); while (!HallTriggered) { wait(0.5); } } double GetSpeed() { //if ((RunningTime.read_us() - lastTime) > 1000 || SmoothedInterruptPeriod() == 0) //{ // return 0.0; //} return (0.04)/((double)SmoothedInterruptPeriod() / 1000000.0); //return SmoothedInterruptPeriod(); } double Error = 0; void SerialOut(double outputValue) { int outChar = 0; if (outputValue < 0.0) { pc.putc('-'); outputValue *= -1.0; } if (outputValue >= 100.0) { outChar = outputValue / 100; pc.putc(outChar + 48); outputValue -= outChar * 100.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 KpGain = 0.0; double KiGain = 0.0; double KdGain = 0.0; void SerialTransmit() { SerialOut(RunningTime.read()); pc.putc('\t'); SerialOut(position); pc.putc('\t'); SerialOut(currentPower); pc.putc('\t'); SerialOut(GetSpeed()); pc.putc('\t'); SerialOut(Error); pc.putc('\t'); SerialOut(KpGain); pc.putc('\t'); SerialOut(KiGain); pc.putc('\t'); SerialOut(KdGain); 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; 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.0; while(KpGain < 1.0) { KpGain += 0.01; while(KiGain < 1.0) { KiGain += 0.01; while(KdGain < 1.0) { KdGain += 0.01; 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_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; }*/