BULME_BERTL14
Bertl Robot with fiunctions
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;
}