Revision 0:50d2b4671eac, committed 2015-02-05

Comitter:

Prettner67

Date:

Thu Feb 05 12:29:51 2015 +0000

Commit message:

Viele n?tzliche Funktionen

Changed in this revision

diff -r 000000000000 -r 50d2b4671eac Funktionen1.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Funktionen1.cpp	Thu Feb 05 12:29:51 2015 +0000
@@ -0,0 +1,546 @@
+#include "mbed.h"
+#include "config.h"
+
+void Move()
+{
+	MotorL_EN = MotorR_EN = 1;
+	MotorL_FORWARD = MotorR_FORWARD = 1;
+	wait_ms(300);
+	MotorL_FORWARD = MotorR_FORWARD = 0;
+	MotorL_EN = MotorR_EN = 0;
+	
+}
+
+void Back()
+{
+	MotorL_EN = MotorR_EN = 1;
+	MotorL_REVERSE = MotorR_REVERSE = 1;
+	wait_ms(300);
+	MotorL_REVERSE = MotorR_REVERSE = 0;
+	MotorL_EN = MotorR_EN = 0;
+	
+}
+
+void TurnRight()
+{
+	MotorL_EN = 1;
+	MotorL_FORWARD = 1;
+	wait_ms(100);
+	MotorL_FORWARD = 0;
+	MotorL_EN = 0;
+	
+}
+
+void TurnLeft()
+{
+	MotorR_EN = 1;
+	MotorR_FORWARD = 1;
+	wait_ms(100);
+	MotorR_FORWARD = 0;
+	MotorR_EN = 0;
+	
+}
+
+bool WaitUntilButtonPressed()
+{
+	char cmd[3];
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    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;
+	
+}
+
+bool Start()
+{
+    DEBUG_PRINT("Debug level: %d", (int) DEBUG);
+    i2c.frequency(40000);       
+
+    while(WaitUntilButtonPressed()) {    }
+    return true;
+	
+}
+
+bool FrontIsClear()
+{
+    char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_FLL|BTN_FL|BTN_FM|BTN_FR|BTN_FRR))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+	
+}
+
+void RotateRight()
+{
+	MotorL_EN = MotorR_EN = 1;
+	MotorL_FORWARD = MotorR_REVERSE = 1;
+	wait_ms(2000);
+	MotorL_FORWARD = MotorR_REVERSE = 0;
+	MotorL_EN = MotorR_EN = 0;
+	
+}
+
+void RotateLeft()
+{
+	MotorL_EN = MotorR_EN = 1;
+	MotorR_FORWARD = MotorL_REVERSE = 1;
+	wait_ms(2000);
+	MotorR_FORWARD = MotorL_REVERSE = 0;
+	MotorL_EN = MotorR_EN = 0;
+	
+}
+
+void MoveTime(int c)
+{
+	MotorL_EN = MotorR_EN = 1;
+	MotorL_FORWARD = MotorR_FORWARD = 1;
+	wait_ms(c);
+	MotorL_FORWARD = MotorR_FORWARD = 0;
+	MotorL_EN = MotorR_EN = 1;
+	
+}
+
+void BackTime(int c)
+{
+	MotorL_EN = MotorR_EN = 1;
+	MotorL_REVERSE = MotorR_REVERSE = 1;
+	wait_ms(c);
+	MotorL_EN = MotorR_EN = 0;
+	MotorL_REVERSE = MotorR_REVERSE = 0;
+	
+}
+
+void TurnRightTime(int c)
+{
+	MotorL_EN = 1;
+	MotorL_FORWARD = 1;
+	wait_ms(c);
+	MotorL_FORWARD = 0;
+	MotorL_EN = 0;
+	
+}
+
+void TrnLeftTime(int c)
+{
+	MotorR_EN = 1;
+	MotorR_FORWARD = 1; 
+	wait_ms(c);
+	MotorR_FORWARD = 0;
+	MotorR_EN = 0;
+	
+}
+
+void RotateRightTime(int c)
+{
+	MotorL_EN = MotorR_EN = 1;
+	MotorL_FORWARD = MotorR_REVERSE = 1;
+	wait_ms(c);
+	MotorL_FORWARD = MotorR_REVERSE = 0;
+	MotorL_EN = MotorR_EN = 0;
+	
+}
+
+void RotateLeftTime(int c)
+{
+	MotorR_EN = MotorL_EN = 1;
+	MotorR_FORWARD = MotorL_REVERSE = 1;
+	wait_ms(c);
+	MotorR_FORWARD = MotorL_REVERSE = 0; 
+	MotorL_EN = MotorR_EN = 0;
+	
+}
+
+bool BackIsClear()
+{
+    char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_BL|BTN_BM|BTN_BR))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+	
+}
+
+bool ButtonBackLeft()
+{
+    char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_BL))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+	
+}
+
+bool ButtonBackMid()
+{
+	char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_BM))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+}
+
+bool ButtonBackRight()
+{
+	char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_BR))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+}
+
+bool ButtonFrontLeftLeft()
+{
+	char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_FLL))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+}
+
+bool ButtonFrontLeft()
+{
+	char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_FL))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+}
+
+bool ButtonFrontMid()
+{
+	char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_FM))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+}
+
+bool ButtonFrontRight()
+{
+	char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_FR))
+        wert = false;
+    else
+        wert = true;
+    DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
+    return wert;
+}
+
+bool ButtonFrontRightRight()
+{
+	char cmd[3];    
+    int16_t btns;
+    bool wert;
+
+    cmd[0] = 0x06;
+    cmd[1] = 0x00;
+    i2c.write(addr, cmd, 2); 
+
+    cmd[0]=0x01;
+    i2c.write(addr, cmd, 1);
+    i2c.read(addr|1, cmd, 1);
+    btns = cmd[0];
+    if( btns & (BTN_FRR))
+        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);
+}
+
+bool MoveTicks(int c)
+{
+	int count = 0, left = 0, right = 0;
+	
+	MotorL_EN = MotorR_EN = 1;
+	
+	while(c--)
+	{
+		MotorL_FORWARD = MotorR_FORWARD = 1;
+		
+		if(SensorL == 1)
+			left++;
+		if(SensorR == 1)
+			right++;
+		
+		count++;
+		wait_ms(10);
+	}
+	
+	MotorL_FORWARD = MotorR_FORWARD = 0;
+	MotorL_EN = MotorR_EN = 0;
+	
+	return true;
+	
+}
+
+bool BackTicks(int c)
+{
+	int count = 0, left = 0, right = 0;
+	
+	MotorL_EN = MotorR_EN = 1;
+	
+	while(c--)
+	{
+		MotorL_REVERSE = MotorR_REVERSE = 1;
+		
+		if(SensorL == 1)
+			left++;
+		if(SensorR == 1)
+			right++;
+		
+		count++;
+		wait_ms(10);
+	}
+	
+	MotorL_REVERSE = MotorR_REVERSE = 0;
+	MotorL_EN = MotorR_EN = 0;
+	
+	return true;
+	
+}
+
+bool MotorRightMoveTicks(int c)
+{
+	int count = 0, right = 0;
+	
+	MotorR_EN = 1;
+	
+	while(c--)
+	{
+		MotorR_FORWARD = 1;
+		
+		if(SensorR == 1)
+			right++;
+		
+		count++;
+		wait_ms(10);
+	}
+	
+	MotorR_FORWARD = 0;
+	MotorR_EN = 0;
+	
+	return true;
+	
+}
+
+bool MotorRightBackTicks(int c)
+{
+	int count = 0, right = 0;
+	
+	MotorR_EN = 1;
+	
+	while(c--)
+	{
+		MotorR_REVERSE = 1;
+		
+		if(SensorR == 1)
+			right++;
+		
+		count++;
+		wait_ms(10);
+	}
+	
+	MotorR_REVERSE = 0;
+	MotorR_EN = 0;
+	
+	return true;
+	
+}
+
+bool MotorLeftMoveTicks(int c)
+{
+	int count = 0, right = 0;
+	
+	MotorL_EN = 1;
+	
+	while(c--)
+	{
+		MotorL_FORWARD = 1;
+		
+		if(SensorR == 1)
+			right++;
+		
+		count++;
+		wait_ms(10);
+	}
+	
+	MotorL_FORWARD = 0;
+	MotorL_EN = 0;
+	
+	return true;
+	
+}
+
+bool MotorLeftBackTicks(int c)
+{
+	int count = 0, right = 0;
+	
+	MotorL_EN = 1;
+	
+	while(c--)
+	{
+		MotorL_REVERSE = 1;
+		
+		if(SensorR == 1)
+			right++;
+		
+		count++;
+		wait_ms(10);
+	}
+	
+	MotorL_REVERSE = 0;
+	MotorL_EN = 0;
+	
+	return true;
+	
+}

diff -r 000000000000 -r 50d2b4671eac Funktionenv1.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Funktionenv1.h	Thu Feb 05 12:29:51 2015 +0000
@@ -0,0 +1,34 @@
+#include "mbed.h"
+
+void Move();
+void Back();
+void TurnRight();
+void TurnLeft();
+void RotateRight();
+void RotateLeft();
+void MoveTime(int c);
+void BackTime(int c);
+void TurnRightTime(int c);
+void TrnLeftTime(int c);
+void RotateLeftTime(int c);
+void RotateRightTime(int c);
+void TurnLedOn(int16_t led);
+void TurnLedOff(int16_t led);
+bool BackIsClear();
+bool ButtonBackLeft();
+bool ButtonBackMid();
+bool ButtonBackRight();
+bool ButtonFrontLeftLeft();
+bool ButtonFrontLeft();
+bool ButtonFrontMid();
+bool ButtonFrontRight();
+bool ButtonFrontRightRight();
+bool Start();
+bool WaitUntilButtonPressed();
+bool FrontIsClear();
+bool MoveTicks(int c);
+bool BackTicks(int c);
+bool MotorRightMoveTicks(int c);
+bool MotorRightBackTicks(int c);
+bool MotorLeftMoveTicks(int c);
+bool MotorLeftBackTicks(int c);

diff -r 000000000000 -r 50d2b4671eac config.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config.h	Thu Feb 05 12:29:51 2015 +0000
@@ -0,0 +1,69 @@
+/***********************************
+name:   config.h    v0.1
+author: PE HTL BULME
+email:  pe@bulme.at
+description:
+    Constants and wiring for ur_Karel
+   
+      
+***********************************/
+#ifndef _FOO_H
+#define _FOO_H
+
+#include "mbed.h"
+
+
+#define DEBUG 0
+
+#if defined(DEBUG) && DEBUG > 0
+ #define DEBUG_PRINT(fmt, args...) fprintf(stderr, "DEBUG: %s:%d:%s(): " fmt, \
+    __FILE__, __LINE__, __func__, ##args)
+#else
+ #define DEBUG_PRINT(fmt, args...) /* Don't do anything in release builds */
+#endif
+
+const int BTN_FLL = 0x80;   // button front left outer
+const int BTN_FL  = 0x04;   // button front left
+const int BTN_FM  = 0x01;   // button front middle
+const int BTN_FR  = 0x08;   // button front right 
+const int BTN_FRR = 0x40;   // button front right outer
+const int BTN_BL = 0x10;    // button back left
+const int BTN_BM = 0x02;    // button back middle
+const int BTN_BR = 0x20;    // button back right
+
+const int LED_FL1 = 0x01;   // front LED white 
+const int LED_FL2 = 0x02;   // front LED yellow
+const int LED_FR1 = 0x04;   // front LED white
+const int LED_FR2 = 0x08;   // front LED yellow
+const int LED_ALL_FRONT = 0x0F;
+
+const int LED_BL1 = 0x20;   // yellow LED back left outer
+const int LED_BL2 = 0x10;   // red LED back left inner
+const int LED_BR1 = 0x80;   // yellow LED back right outer
+const int LED_BR2 = 0x40;   // red LED back right inner
+const int LED_ALL_BACK = 0xF0;
+const int LED_ALL = 0xFF;
+
+const int addr = 0x40;      // I2C-address PCA9555
+
+Serial pc(USBTX, USBRX);    // tx, rx  
+
+DigitalOut LED_D10(P1_8);    // wiring LED D10 to D13 
+DigitalOut LED_D11(P1_9);    
+DigitalOut LED_D12(P1_10);   
+DigitalOut LED_D13(P1_11);   
+
+DigitalOut MotorL_EN(p34);  // wiring motor left
+DigitalOut MotorL_FORWARD(P1_1);
+DigitalOut MotorL_REVERSE(P1_0);
+
+DigitalOut MotorR_EN(p36);  // wiring motor right
+DigitalOut MotorR_FORWARD(P1_3);
+DigitalOut MotorR_REVERSE(P1_4);
+
+I2C i2c(p28,p27);   
+BusIn linesensor(p18, p16, p19, p17);  
+DigitalIn SensorL(P1_12);    // motor sensor left
+DigitalIn SensorR(P1_13);    // motor sensor right
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 50d2b4671eac main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Feb 05 12:29:51 2015 +0000
@@ -0,0 +1,24 @@
+#include "Funktionenv1.h"
+
+
+int main()
+{
+	/*while(!Start()) {}   
+    wait_ms(200);
+	*/
+	int count = 0;
+	
+	while(FrontIsClear())
+	{	
+		MoveTicks(1);
+		count ++;
+	}
+	
+	if (!FrontIsClear())
+	{
+		BackTicks(count);
+	}
+	
+	return 0;
+	
+}

diff -r 000000000000 -r 50d2b4671eac mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Feb 05 12:29:51 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b
\ No newline at end of file