Diff: main.cpp

Revision:

2:1115b3fa7fde

Parent:

1:ca7fdf2e285d

Child:

4:ae6da6b26fc9

--- a/main.cpp	Thu Apr 16 11:44:31 2015 +0000
+++ b/main.cpp	Sun Apr 26 20:05:33 2015 +0000
@@ -2,43 +2,122 @@
 #include "const.h"
 #include "ur_Bertl.h"
 
+Serial bt(p9, p10);
+
+const int blinkTime = 200; // ms
+
+class BlinkBertl : public Bertl
+{
+public:
+    void BlinkLeftLed();
+    void BlinkRigthLed();
+};
+
+void BlinkBertl::BlinkLeftLed()
+{
+    for(int i = 0; i < 3; i++) {
+        TurnLedOn(LED_FL2 | LED_BL1);
+        wait_ms(blinkTime);
+        TurnLedOff(LED_ALL);
+        wait_ms(blinkTime);
+    }
+}
+
+void BlinkBertl::BlinkRigthLed()
+{
+    for(int i = 0; i < 3; i++) {
+        TurnLedOn(LED_FR2 | LED_BR1);
+        wait_ms(blinkTime);
+        TurnLedOff(LED_ALL);
+        wait_ms(blinkTime);
+    }
+}
+
 int main()
 {
-     
-     ur_Bertl karel;
-     
-     while(karel.AnyBeeperInBag())
-     {
-     }
-     karel.TurnLedOff(LED_ALL);
-     while(1)
-     {
-        if( karel.IsButtonPressed(BTN_FLL))
-        karel.NibbleLeds(0x01);
-        
-     if( karel.IsButtonPressed(BTN_FL))
-        karel.NibbleLeds(0x02);
-        
-    if( karel.IsButtonPressed(BTN_FM))
-        karel.NibbleLeds(0x0F);
-        
-     if( karel.IsButtonPressed(BTN_FR))
-        karel.NibbleLeds(0x04);
-        
-    if( karel.IsButtonPressed(BTN_FRR))
-        karel.NibbleLeds(0x08);
-        
-    if( karel.IsButtonPressed(BTN_BL))
-        karel.NibbleLeds(0x0E);
-        
-     if( karel.IsButtonPressed(BTN_BM))
-        karel.NibbleLeds(0x09);
-        
-    if( karel.IsButtonPressed(BTN_BR))
-        karel.NibbleLeds(0x07);
-        
-    else
-        karel.NibbleLeds(0x00);
-    
+    char cmd, b1, b3;
+    bool turn = false;
+
+    BlinkBertl karel;
+
+    bt.printf("Kommands 1:Move, 3:TurnLeft, 5:TurnRight, 7:MoveBack, 6 and 8 stepwise turns \n");
+    /*    karel.Move();
+        karel.TurnLeft();
+        karel.TurnRigth();
+        karel.MoveBackwards();
+    */
+    karel.TurnLedOn(LED_ALL);
+    wait_ms(200);
+    karel.TurnLedOff(LED_ALL);
+    wait_ms(200);
+    while(karel.WaitUntilButtonPressed()) {}
+    wait(1);
+    while(karel.WaitUntilButtonPressed())
+    {
+        karel.NibbleLeds(karel.GetLineValues());
+        switch(karel.GetLineValues())
+        {
+        case 0x00:
+            karel.TurnLeftStep(50);
+            break;
+        case 0x03: case 0x01: case 0x07:
+            karel.TurnLeftStep(20);
+            break;
+        case 0x0E: case 0x0A: case 0x08:
+            karel.TurnRigthStep(20);
+            break;
+        default:
+            karel.Move(20);
+            break;
+        }
+        if(!karel.FrontIsClear())
+            karel.TurnLeftStep(1000);
+    }
+    while(true) {
+        if (bt.readable()) {
+            b1 = bt.getc();
+            cmd = bt.getc();
+            b3 = bt.getc();
+            //bt.printf("3 Getchar: %x %x %x\n", b1, cmd ,b3);
+            karel.NibbleLeds(karel.GetLineValues());
+            switch (cmd) {
+                case '1':
+                    karel.Move();
+                    break;
+                case '3':
+                    karel.TurnLeft();
+                    break;
+                case '5':
+                    karel.TurnRigth();
+                    break;
+                case '7':
+                    karel.MoveBackwards();
+                    break;
+                case '6':
+                    karel.TurnLeftStep();
+                    break;
+                case '8':
+                    karel.TurnRigthStep();
+                    break;
+                case '0':
+                    karel.BlinkLeftLed();
+                    break;
+                case '2':
+                    karel.BlinkRigthLed();
+                    break;
+                default:
+                    break;
+            }
+            if(cmd == '9' && turn)
+            {
+                turn = !turn;
+                karel.TurnLedOff(LED_ALL);
+            } else if(cmd == '9' && !turn)
+            {
+                turn = !turn;
+                karel.TurnLedOn(LED_ALL);
+            }
+            
+        }
     }
 }
\ No newline at end of file