Diff: main.cpp

Revision:

2:43547160ab56

Parent:

1:f2d7bec926ce

Child:

3:c7e5419fa980

diff -r f2d7bec926ce -r 43547160ab56 main.cpp
--- a/main.cpp	Fri Jan 02 19:32:23 2015 +0000
+++ b/main.cpp	Sat Jan 03 14:18:50 2015 +0000
@@ -9,6 +9,15 @@
 ***********************************/
 #include "mbed.h"
 
+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;
+
 void move()
 {  
     DigitalOut MotorL_EN(p34);
@@ -18,13 +27,18 @@
     DigitalOut MotorR_EN(p36);
     DigitalOut MotorR_FORWARD(P1_3);
     //DigitalOut MotorR_REVERSE(P1_4);
+    PwmOut mg1(p34);             //PWM Ausgang zum Motor
+    PwmOut mg2(p36);
+    mg1=mg2=1.0;
 
     MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
 
     MotorR_FORWARD = MotorL_FORWARD = 1;   // Beide Motoren vorwärts EIN
     wait_ms(250);                           // warte 0,25 Sekunde
     MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
-    //wait_ms(250);            
+    MotorR_EN=MotorL_EN=0;
+    //mg1=mg2=0.0;
+    wait_ms(250);            
 }
 
 void moveSpeed(float speedy)
@@ -66,25 +80,58 @@
     DigitalOut MotorR_FORWARD(P1_3);
 //    DigitalOut MotorR_REVERSE(P1_4);
 //  MotorL_EN=1;
-
+    PwmOut mg2(p36);
+    mg2=1.0;
     MotorR_EN=1;      // rechten Motor ENABLE
 
     MotorR_FORWARD = 1;  // rechter Motor vorwärts EIN 
     wait_ms(750);        // warte (90°)
     MotorR_FORWARD = 0;  // Motoren AUS
+    MotorR_EN=0;
+    
+    wait_ms(250);        // warte (90°)
+}
+bool frontIsClear()
+{
+    I2C i2c(p28,p27);
+    const int addr = 0x40; // I2C-Adresse PCA9555
+    char cmd[3]; 
+    int16_t btns;
+    i2c.frequency(40000);  // I2C Frequenz 40kHz
+
+    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_FM )
+        return false;
+    else
+        return true;
 }
 
+/*bool IsAnyFrontButton()
+            { return btns & (BTN_FL|BTN_FM|BTN_FR); }
+*/
 // *************  Hauptprogramm ************
 int main()
 {
     //move();
     //turnLeft();
-    DigitalOut LED_D10(P1_8);    // LED D10 und D13 definieren
+/*    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()) {}
 
-    moveSpeed(0.25);
+    turnLeft();
+    move();
+    wait(1);
+/*    moveSpeed(0.25);
     LED_D10 = 1;
     moveSpeed(0.3);
     LED_D11 = 1;
@@ -102,9 +149,12 @@
     LED_D12 = 1;
     moveSpeed(1.0);
     LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
-    //wait(2);
-    //turnLeft();
-    //move();
+    PwmOut mg1(p34);             //PWM Ausgang zum Motor
+    PwmOut mg2(p36);
+    mg1=mg2=0.0;
+    wait(2);
+*/    turnLeft();
+    move();
     //turnLeft();
     //move(5);