Diff: main.cpp

Revision:

0:398432a37ca5

Child:

1:a33723b70582

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 05 11:23:39 2014 +0000
@@ -0,0 +1,367 @@
+#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;
+
+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;
+        }
+    }
+    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;
+        }
+    }
+    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.5);
+    
+    while (!HallTriggered)
+    {
+        wait(0.5);
+    }
+}
+
+void SerialTransmit()
+{        
+    double tempPos = position;
+    double tempTime = RunningTime.read();
+    double tempPow = currentPower;
+    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(',');
+    
+    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(',');
+    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(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();
+}
+
+int main()
+{
+    RGHSinInterrupt.rise(&RGHSinHandler);
+    RGHCosInterrupt.rise(&RGHCosHandler);
+    HallSensor.fall(&HallEffectFall);
+    HallSensor.mode(PullUp);
+    
+    RunningTime.start();
+    
+    pc.baud(115200);
+    
+    Home();
+    
+    while(1)
+    {        
+        SerialTransmit();
+        
+        switch(counter)
+        {
+        case 0:
+            if (position < 200)
+            {
+                SetPower(1.0);
+            }
+            else
+            {
+                counter++;
+            }
+            break;
+        case 1:
+            if (position > 10)
+            {
+                SetPower(-1.0);
+            }
+            else
+            {
+                counter = 0;
+            }
+            break;
+        }
+    }
+}
\ No newline at end of file