Merged to branch
Dependencies: USBDevice mbed EquatorStrutController LightWeightSerialTransmit
Fork of EquatorStrutDigitalMonitor by
Diff: main.cpp
- Revision:
- 16:47d761226df6
- Parent:
- 14:67466da6663d
- Parent:
- 15:cd409a54ceec
- Child:
- 17:f54cdc9ae52f
--- a/main.cpp Wed Aug 13 08:36:07 2014 +0000 +++ b/main.cpp Wed Aug 13 08:47:02 2014 +0000 @@ -6,6 +6,8 @@ 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); PwmOut PhaseA(P0_9); @@ -33,10 +35,11 @@ char counter = 0; +volatile bool SinHigh = false; +volatile bool CosHigh = false; + void SmoothingAdd(int input) { - //arrayTotal -= intteruptPeriodArray[arrayPos]; - //arrayTotal += input; intteruptPeriodArray[arrayPos] = input * direction; if (arrayPos == arraySize-1) @@ -47,8 +50,6 @@ { arrayPos++; } - - //interruptPeriod = arrayTotal / 15; } int SmoothedInterruptPeriod() @@ -63,54 +64,41 @@ return arrayTotal / arraySize; } -void RGHSinHandler() -{ - if (PinState == 2) +void RGHSinRisingHandler() +{ + SinHigh = true; +} + +void RGHSinFallingHandler() +{ + SinHigh = false; +} + +void RGHCosRisingHandler() +{ + if (CosHigh) { 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 if (SinHigh) + { + direction = -1; } else { - PinState = 0 |(RGHSinState << 1) | RGHCosState; + direction = 1; } + + position += 0.04 * direction; + SmoothingAdd(RunningTime.read_us() - lastTime); + lastTime = RunningTime.read_us(); + + CosHigh = true; } -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 RGHCosFallingHandler() +{ + CosHigh = false; } void SetPower(double power) @@ -311,6 +299,8 @@ { RGHSinInterrupt.disable_irq(); RGHCosInterrupt.disable_irq(); + RGHSinFallingInterrupt.disable_irq(); + RGHCosFallingInterrupt.disable_irq(); if (direction < 0) { @@ -325,6 +315,8 @@ } RGHSinInterrupt.enable_irq(); RGHCosInterrupt.enable_irq(); + RGHSinFallingInterrupt.enable_irq(); + RGHCosFallingInterrupt.enable_irq(); } @@ -455,8 +447,10 @@ int main() { - RGHSinInterrupt.rise(&RGHSinHandler); - RGHCosInterrupt.rise(&RGHCosHandler); + RGHSinInterrupt.rise(&RGHSinRisingHandler); + RGHCosInterrupt.rise(&RGHCosRisingHandler); + RGHSinFallingInterrupt.fall(&RGHSinFallingHandler); + RGHCosFallingInterrupt.fall(&RGHCosFallingHandler); HallSensor.fall(&HallEffectFall); HallSensor.mode(PullUp); @@ -472,7 +466,7 @@ PreviousTime = RunningTime.read_us(); - while(Enabled) + /*while(Enabled) { //double pow = 0.0; while(PosKpGain < 1.0) @@ -501,7 +495,7 @@ } Disable(); - } + }*/ } /* Change this throttle value range to 1.0 and 0.0 for car speed