Diff: main.cpp

Revision:

4:a975caedface

Parent:

3:c7e5419fa980

Child:

5:7b091d085c70

--- a/main.cpp	Sat Jan 03 17:32:26 2015 +0000
+++ b/main.cpp	Sun Jan 11 08:42:08 2015 +0000
@@ -1,141 +1,223 @@
 /***********************************
-name:   Karel
-author: PE HTL BULME
-email:  pe@bulme.at
-description:
-    Funktionen von Karel The Robot 
-    
-      
-***********************************/
+* name:   ur_Karel    v 0.2
+* author: PE HTL BULME
+* email:  pe@bulme.at
+* description:
+*    functions for Karel The Robot
+*
+*************************************/
 #include "mbed.h"
+#include "config.h"
+
+bool FrontIsClear();
+
+void ShutOff()
+{
+    MotorR_FORWARD = MotorL_FORWARD = 0;    // motor OFF
+    MotorR_EN=MotorL_EN=0;                  // motor disable
+}
 
-const int BTN_FLL = 0x80;
-const int BTN_FL  = 0x04;
-const int BTN_FM  = 0x01;
-const int BTN_FR  = 0x08;
-const int BTN_FRR = 0x40;
-const int BTN_BL = 0x10;
-const int BTN_BM = 0x02;
-const int BTN_BR = 0x20;
-bool frontIsClear();
-void shutOff()
+void Move()
 {
- /*   DigitalOut MotorL_EN(p34);
-    DigitalOut MotorL_FORWARD(P1_1);
-    DigitalOut MotorR_EN(p36);
-    DigitalOut MotorR_FORWARD(P1_3);
-*/    PwmOut mg1(p34);             //PWM Ausgang zum Motor
-    PwmOut mg2(p36);
-    mg1=mg2=0.0;
+    int count=0, left=0, right=0;           // initialise variables
+    MotorR_EN=MotorL_EN=1;                  // both motor ENABLE
+
+    while(count<1000) {
+        MotorR_FORWARD = MotorL_FORWARD = 1;// both motor forward ON
+        LED_D10 = SensorL;                  //  LED D10 blinks
+        if(SensorL == 1)
+            left++;
+        if(SensorR == 1)
+            right++;
+        LED_D13 = SensorR;                  // LED D13 blinks
+        count++;
+        wait_ms(1);                         // wait for 1 ms
+    }
+    //DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);
+
+    MotorR_FORWARD = MotorL_FORWARD = 0;    // both motor off
+    MotorR_EN=MotorL_EN=0;
+    wait_ms(250);
+}
 
- //    MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
-//    MotorR_EN=MotorL_EN=0;   
+bool FrontIsClear()
+{
+    char cmd[3];    // array for I2C
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); // define Port0 = Out
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_FL|BTN_FM|BTN_FR))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
 }
-bool move()
-{
-    DigitalOut MotorL_EN(p34);
-    DigitalOut MotorL_FORWARD(P1_0);
-    //DigitalOut MotorL_REVERSE(P1_1);
 
-    DigitalOut MotorR_EN(p36);
-    DigitalOut MotorR_FORWARD(P1_4);
-    //DigitalOut MotorR_REVERSE(P1_3);
-    PwmOut mg1(p34);             //PWM Ausgang zum Motor
-    PwmOut mg2(p36);
-    if(!frontIsClear()) {
-        mg1=mg2=0.0;
-        return false;
-    } else {
-        mg1=mg2=1.0;
-        MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
+bool MoveMeasure(int& left, int& right)
+{
+    int count=0; //, left=0, right=0;
+
+    MotorR_EN=MotorL_EN=1;
+    MotorR_FORWARD = MotorL_FORWARD = 1;
 
-        MotorR_FORWARD = MotorL_FORWARD = 1;   // Beide Motoren vorwärts EIN
-        wait_ms(250);                           // warte 0,25 Sekunde
-        MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
-        MotorR_EN=MotorL_EN=0;
-        //mg1=mg2=0.0;
-        wait_ms(250);
-        return true;
+    while(count<1000) {
+        MotorR_FORWARD = MotorL_FORWARD = 1;
+        LED_D10 = SensorL;
+        if(SensorL == 1)
+            left++;
+        if(SensorR == 1)
+            right++;
+        LED_D13 = SensorR;
+        count++;
+        wait_ms(1);
     }
+    DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);
+
+    MotorR_FORWARD = MotorL_FORWARD = 0;
+    MotorR_EN=MotorL_EN=0;
+    wait_ms(500);
+    return true;
 }
 
-void moveSpeed(float speedy)
-{  
-    DigitalOut MotorL_EN(p34);
-    DigitalOut MotorL_FORWARD(P1_0);
-    //DigitalOut MotorL_REVERSE(P1_1);
+bool MoveTicks(int ticks)
+{
+    int count=0, left=0, right=0;                   //Variable count auf 0 setzen
 
-    DigitalOut MotorR_EN(p36);
-    DigitalOut MotorR_FORWARD(P1_4);
-    //DigitalOut MotorR_REVERSE(P1_3);
-    PwmOut mg1(p34);             //PWM Ausgang zum Motor
-    PwmOut mg2(p36);
-    mg1=mg2=speedy;
-    
     MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
-    MotorR_FORWARD = MotorL_FORWARD = 1;   // Beide Motoren vorwärts EIN
-    wait_ms(2000);                           // warte 0,25 Sekunde
+    while(ticks--) {             // mache solang ticks
+        MotorR_FORWARD = MotorL_FORWARD = 1;  // Beide Motoren vorwärts EIN
+        LED_D10 = SensorL;   //  LED D10 blinkt
+        if(SensorL == 1)
+            left++;
+        if(SensorR == 1)
+            right++;
+        LED_D13 = SensorR;   //  LED D13 blinkt
+        count++;
+        wait_ms(1);          //  warte 1 mSekunde
+    }
+    DEBUG_PRINT("SensorL: %d SensorR: %d anzahl: %d\right\n", left, right, count);
+
     MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
-    MotorR_EN=MotorL_EN=0;                 // Beide Motoren ENABLE
-    wait_ms(500);            
+    MotorR_EN=MotorL_EN=0;
+    //wait_ms(250);
+    return true;
 }
-void move(int anzahl)
+
+void TurnBack()
 {
-  
-    for(int i=0; i < anzahl; i++)
-        move();
-    
+    MotorR_EN=1;
+
+    MotorR_REVERSE = 1;
+    wait_ms(750);
+    MotorR_REVERSE = 0;
+    MotorR_EN=0;
+
+    wait_ms(250);
 }
-void turnBack()
-{
-//    DigitalOut MotorL_EN(p34);
-//    DigitalOut MotorL_FORWARD(P1_1);
-//    DigitalOut MotorL_REVERSE(P1_0);
 
-    DigitalOut MotorR_EN(p36);
-//    DigitalOut MotorR_FORWARD(P1_4);
-    DigitalOut MotorR_REVERSE(P1_3);
-//  MotorL_EN=1;
-    PwmOut mg2(p36);
-    mg2=1.0;
-    MotorR_EN=1;      // rechten Motor ENABLE
+void TurnLeftTicks(int ticks)
+{
+    MotorR_EN=1;
+    MotorL_EN=1;
+    int left=0, right=0;
+    while(ticks--) {
+        MotorR_FORWARD = MotorL_REVERSE = 1;
+        LED_D10 = SensorL;
+        if(SensorL == 1)
+            left++;
+        if(SensorR == 1)
+            right++;
+        LED_D13 = SensorR;
+        wait_ms(1);
+    }
+    DEBUG_PRINT("\right\nTurnLeft: SensorL: %d SensorR: %d \right\n", left, right);
 
-    MotorR_REVERSE = 1;  // rechter Motor vorwärts EIN 
-    wait_ms(750);        // warte (90°)
-    MotorR_REVERSE = 0;  // Motoren AUS
-    MotorR_EN=0;
-    
-    wait_ms(250);        // warte (90°)
+    MotorL_REVERSE = MotorR_FORWARD = 0;
+    MotorL_EN=MotorR_EN=0;
+
+    wait_ms(250);
 }
 
-void turnLeft()
+void TurnLeftTime(int ms)
 {
-//    DigitalOut MotorL_EN(p34);
-//    DigitalOut MotorL_FORWARD(P1_1);
-//    DigitalOut MotorL_REVERSE(P1_0);
+    MotorR_EN=1;            // motor right ENABLE
+
+    MotorR_FORWARD = 1;     // motor right forward ON
+    wait_ms(ms);            // wait for ms
+    MotorR_FORWARD = 0;     // motor right OFF
+    MotorR_EN=0;
+
+    wait_ms(250);           // only to step the robot
+}
+
+void TurnLeft()
+{
+    MotorR_EN=1;            // motor right ENABLE
 
-    DigitalOut MotorR_EN(p36);
-    DigitalOut MotorR_FORWARD(P1_4);
-//    DigitalOut MotorR_REVERSE(P1_3);
-//  MotorL_EN=1;
-    PwmOut mg2(p36);
-    mg2=1.0;
-    MotorR_EN=1;      // rechten Motor ENABLE
+    MotorR_FORWARD = 1;     // motor right forward ON
+    wait_ms(500);           // wait 500 ms (90°)
+    MotorR_FORWARD = 0;     // motor right forward OFF
+    MotorR_EN=0;
+
+    wait_ms(250);           // only to step the robot
+}
+
+void TurnLedOn(int16_t led)
+{
+    char cmd[3];
+
+    cmd[0] = 0x02;
+    cmd[1] = ~led;
+    i2c.write(addr, cmd, 2);
+    wait(0.5);
+}
 
-    MotorR_FORWARD = 1;  // rechter Motor vorwärts EIN 
-    wait_ms(500);        // warte (90°)
-    MotorR_FORWARD = 0;  // Motoren AUS
-    MotorR_EN=0;
-    
-    wait_ms(250);        // warte (90°)
+void TurnLedOff(int16_t led)
+{
+    char cmd[3];
+
+    cmd[0] = 0x02;
+    cmd[1] = led;
+    i2c.write(addr, cmd, 2);
+    wait(0.5);
+}
+
+void TurnAllLedOff()
+{
+    TurnLedOff(LED_ALL);
+}
+
+void TurnAllLedOn()
+{
+    TurnLedOn(LED_ALL);
 }
-bool frontIsClear()
+
+void MoveMore(int anzahl)
+{
+    for(int i=0; i < anzahl; i++)
+        Move();
+}
+
+void Back()
 {
-    I2C i2c(p28,p27);
-    const int addr = 0x40; // I2C-Adresse PCA9555
-    char cmd[3]; 
+    TurnLeftTicks(150);
+    TurnLeftTicks(150);
+}
+
+bool WaitUntilButtonPressed()
+{
+    char cmd[3];
     int16_t btns;
-    i2c.frequency(40000);  // I2C Frequenz 40kHz
+    bool wert;
 
     cmd[0] = 0x06;
     cmd[1] = 0x00;
@@ -145,67 +227,62 @@
     i2c.write(addr, cmd, 1);
     i2c.read(addr|1, cmd, 1);
     btns = cmd[0];
-    if( btns & (BTN_FL|BTN_FM|BTN_FR))
-        return false;
+    if( btns & (0xFF))
+        wert = false;
     else
-        return true;
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
 }
 
-/*bool IsAnyFrontButton()
-            { return btns & (BTN_FL|BTN_FM|BTN_FR); }
-*/
+bool Start()
+{
+    while(WaitUntilButtonPressed()) {
+        LED_D12 = !LED_D12;
+        wait_ms(250);
+    }
+    return true;
+}
+
+void MoveWhileFrontIsClear()
+{
+    while(FrontIsClear()) {
+        MoveTicks(30);
+    }
+}
+
 // *************  Hauptprogramm ************
 int main()
 {
-    //move();
-    //turnLeft();
-/*    DigitalOut LED_D10(P1_8);    // LED D10 und D13 definieren
-    DigitalOut LED_D11(P1_9);    // LED D10 und D13 definieren
-    DigitalOut LED_D12(P1_10);    // LED D10 und D13 definieren
-    DigitalOut LED_D13(P1_11);   //
-*/
-    while(frontIsClear()) {}
+    DEBUG_PRINT("Debug level: %d", (int) DEBUG);
+    i2c.frequency(40000);       // I2C Frequenz 40kHz
 
-    move();
-    turnLeft();
-    if(move())
-        turnBack();
-    if(move())
-        turnBack();
-    if(move())
-        turnBack();
-    wait(1);
-/*    moveSpeed(0.25);
-    LED_D10 = 1;
-    moveSpeed(0.3);
-    LED_D11 = 1;
-    moveSpeed(0.4);
+    // LED test
     LED_D12 = 1;
-    moveSpeed(0.5);
-    LED_D13 = 1;
-    moveSpeed(0.6);
-    LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0;
-    moveSpeed(0.7);
-    LED_D10 = 1;
-    moveSpeed(0.8);
-    LED_D11 = 1;
-    moveSpeed(0.9);
-    LED_D12 = 1;
-    moveSpeed(1.0);
-    LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
-    PwmOut mg1(p34);             //PWM Ausgang zum Motor
-    PwmOut mg2(p36);
-    mg1=mg2=0.0;
-    wait(2);
-*/    
-    move(5);
-    turnLeft();
-    move();
-    turnLeft();
-    shutOff();
-    //turnLeft();
-    //move(5);
+    TurnLedOn(LED_FL1 |LED_FR1);
+    wait(1);
+    TurnAllLedOn();
+    wait(1);
+    TurnLedOff(LED_FL1 |LED_FR1);
+
+    while(!Start()) {}   // wait to start
+    wait_ms(200);
 
-    //wait(2);
+    MoveWhileFrontIsClear();
+    
+    TurnAllLedOff();
+    wait(1);
+
+    while(FrontIsClear()) {
+        Move();
+    }
+    Back();
+    wait(1);
+
+    while(FrontIsClear()) {
+        MoveTicks(50);
+    }
+    TurnLedOn(LED_ALL_FRONT);
+    ShutOff();
     return 0;
 }