Diff: main.cpp

Revision:

0:486e8a67dbbc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 08 08:12:15 2015 +0000
@@ -0,0 +1,340 @@
+/***********************************
+* name:   func_Bertl    v 0.3
+* author: PE HTL BULME
+* email:  pe@bulme.at
+* description:
+*    functions for Bertl The Robot
+*
+*************************************/
+#include "mbed.h"
+#include "config.h"
+
+bool Start();
+void ShutOff();
+void Move();
+bool FrontIsClear();
+bool MoveMeasure(int time, int& left, int& right);
+bool MoveTicks(int ticks);
+void TurnBack();
+void TurnLeftTicks(int ticks);
+void TurnLeftTime(int ms);
+void TurnLeft();
+void TurnLedOn(int16_t led);
+void TurnLedOff(int16_t led);
+void MoveMore(int anzahl);
+void Back();
+bool WaitUntilButtonPressed();
+
+// Eigene Funktionsdefinitionen hier
+
+void TurnBackTime(int ms);
+void TurnRightTime(int ms);
+
+
+
+    
+// *************  Hauptprogramm ************
+int main()
+{
+    // while(!Start()) {}   // wait to start
+    while(FrontIsClear()) {
+        MoveTicks(10);
+    }
+
+    TurnBackTime(400);
+
+   // TurnRightTime(100);
+    MoveTicks(1000);
+
+    wait(1);
+    TurnLeftTicks(1500);    // Anzahl der MotorSensorwerte
+
+    ShutOff();
+    return 0;
+
+
+}
+
+
+bool Start()
+{
+    DEBUG_PRINT("Debug level: %d", (int) DEBUG);
+    i2c.frequency(40000);       // I2C Frequenz 40kHz
+
+    while(WaitUntilButtonPressed()) {    }
+    return true;
+}
+
+
+void ShutOff()
+{
+    MotorR_FORWARD = MotorL_FORWARD = 0;    // motor OFF
+    MotorR_EN=MotorL_EN=0;                  // motor disable
+}
+
+void Move()
+{
+    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);
+}
+
+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;
+}
+
+/*
+use in main():
+    int left=0, right=0;
+
+    MoveMeasure(1000, left, right);
+    pc.printf("\r\nleft= %d  right= %d\r\n", left, right);
+*/
+bool MoveMeasure(int time, int& left, int& right)
+{
+    int count=0; //, left=0, right=0;
+
+    MotorR_EN=MotorL_EN=1;
+    MotorR_FORWARD = MotorL_FORWARD = 1;
+
+    while(count<time) {
+        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;
+}
+
+bool MoveTicks(int ticks)
+{
+    int count=0, left=0, right=0;                   //Variable count auf 0 setzen
+
+    MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
+    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;
+    //wait_ms(250);
+    return true;
+}
+
+void TurnBack()
+{
+    MotorR_EN=1;
+
+    MotorR_REVERSE = 1;
+    wait_ms(750);
+    MotorR_REVERSE = 0;
+    MotorR_EN=0;
+
+    wait_ms(250);
+}
+
+void TurnBackTime(int ms)
+{
+    MotorR_EN=1;
+
+    MotorR_REVERSE = 1;
+    wait_ms(750);
+    MotorR_REVERSE = 0;
+    MotorR_EN=0;
+
+    wait_ms(250);
+}
+
+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);
+
+    MotorL_REVERSE = MotorR_FORWARD = 0;
+    MotorL_EN=MotorR_EN=0;
+
+    wait_ms(250);
+}
+
+void TurnLeftTime(int ms)
+{
+    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 TurnRightTime(int ms)
+{
+    MotorL_EN=1;            // motor left ENABLE
+
+    MotorL_FORWARD = 1;     // motor left forward ON
+    wait_ms(ms);            // wait for ms
+    MotorL_FORWARD = 0;     // motor left OFF
+    MotorL_EN=0;
+
+    wait_ms(250);           // only to step the robot
+}
+
+void TurnLeft()
+{
+    MotorR_EN=1;            // motor right 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
+}
+
+bool WaitUntilButtonPressed()
+{
+    char cmd[3];
+    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 & (0xFF))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+}
+
+void TurnLedOn(int16_t led)
+{
+    char cmd[3];
+
+    cmd[0] = 0x02;
+    cmd[1] = ~led;
+    i2c.write(addr, cmd, 2);
+    wait(0.5);
+}
+
+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);
+}
+
+void MoveMore(int anzahl)
+{
+    for(int i=0; i < anzahl; i++)
+        Move();
+}
+
+void Back()
+{
+    TurnLeftTicks(150);
+    TurnLeftTicks(150);
+}
+
+void MoveWhileFrontIsClear()
+{
+    while(FrontIsClear()) {
+        MoveTicks(30);
+    }
+}
+
+void LedTest()
+{
+    TurnLedOn(LED_FL1 |LED_FR1);
+    wait(1);
+    TurnAllLedOn();
+    wait(1);
+    TurnLedOff(LED_FL1 |LED_FR1);
+}
+*/
\ No newline at end of file