BERTL_CHEL18 / Mbed 2 deprecated Func_Bertl_Ultra

ultra sonic sensor asdded

Dependencies:   HCSR mbed

Fork of func_Bertl by BULME_BERTL14

main.cpp@2:43547160ab56, 2015-01-03 (annotated)

Committer:
bulmecisco
Date:
Sat Jan 03 14:18:50 2015 +0000
Revision:
2:43547160ab56
Parent:
1:f2d7bec926ce
Child:
3:c7e5419fa980
NOK

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Enenkel 0:0b7c22955b8c 1 /***********************************
bulmecisco 1:f2d7bec926ce 2 name: Karel
bulmecisco 1:f2d7bec926ce 3 author: PE HTL BULME
bulmecisco 1:f2d7bec926ce 4 email: pe@bulme.at
Enenkel 0:0b7c22955b8c 5 description:
bulmecisco 1:f2d7bec926ce 6 Funktionen von Karel The Robot
Enenkel 0:0b7c22955b8c 7
bulmecisco 1:f2d7bec926ce 8
Enenkel 0:0b7c22955b8c 9 ***********************************/
Enenkel 0:0b7c22955b8c 10 #include "mbed.h"
Enenkel 0:0b7c22955b8c 11
bulmecisco 2:43547160ab56 12 const int BTN_FLL = 0x80;
bulmecisco 2:43547160ab56 13 const int BTN_FL = 0x04;
bulmecisco 2:43547160ab56 14 const int BTN_FM = 0x01;
bulmecisco 2:43547160ab56 15 const int BTN_FR = 0x08;
bulmecisco 2:43547160ab56 16 const int BTN_FRR = 0x40;
bulmecisco 2:43547160ab56 17 const int BTN_BL = 0x10;
bulmecisco 2:43547160ab56 18 const int BTN_BM = 0x02;
bulmecisco 2:43547160ab56 19 const int BTN_BR = 0x20;
bulmecisco 2:43547160ab56 20
bulmecisco 1:f2d7bec926ce 21 void move()
bulmecisco 1:f2d7bec926ce 22 {
bulmecisco 1:f2d7bec926ce 23 DigitalOut MotorL_EN(p34);
bulmecisco 1:f2d7bec926ce 24 DigitalOut MotorL_FORWARD(P1_1);
bulmecisco 1:f2d7bec926ce 25 //DigitalOut MotorL_REVERSE(P1_0);
bulmecisco 1:f2d7bec926ce 26
bulmecisco 1:f2d7bec926ce 27 DigitalOut MotorR_EN(p36);
bulmecisco 1:f2d7bec926ce 28 DigitalOut MotorR_FORWARD(P1_3);
bulmecisco 1:f2d7bec926ce 29 //DigitalOut MotorR_REVERSE(P1_4);
bulmecisco 2:43547160ab56 30 PwmOut mg1(p34); //PWM Ausgang zum Motor
bulmecisco 2:43547160ab56 31 PwmOut mg2(p36);
bulmecisco 2:43547160ab56 32 mg1=mg2=1.0;
bulmecisco 1:f2d7bec926ce 33
bulmecisco 1:f2d7bec926ce 34 MotorR_EN=MotorL_EN=1; // Beide Motoren ENABLE
bulmecisco 1:f2d7bec926ce 35
bulmecisco 1:f2d7bec926ce 36 MotorR_FORWARD = MotorL_FORWARD = 1; // Beide Motoren vorwärts EIN
bulmecisco 1:f2d7bec926ce 37 wait_ms(250); // warte 0,25 Sekunde
bulmecisco 1:f2d7bec926ce 38 MotorR_FORWARD = MotorL_FORWARD = 0; // Motoren AUS
bulmecisco 2:43547160ab56 39 MotorR_EN=MotorL_EN=0;
bulmecisco 2:43547160ab56 40 //mg1=mg2=0.0;
bulmecisco 2:43547160ab56 41 wait_ms(250);
bulmecisco 1:f2d7bec926ce 42 }
bulmecisco 1:f2d7bec926ce 43
bulmecisco 1:f2d7bec926ce 44 void moveSpeed(float speedy)
bulmecisco 1:f2d7bec926ce 45 {
bulmecisco 1:f2d7bec926ce 46 DigitalOut MotorL_EN(p34);
bulmecisco 1:f2d7bec926ce 47 DigitalOut MotorL_FORWARD(P1_1);
bulmecisco 1:f2d7bec926ce 48 //DigitalOut MotorL_REVERSE(P1_0);
Enenkel 0:0b7c22955b8c 49
bulmecisco 1:f2d7bec926ce 50 DigitalOut MotorR_EN(p36);
bulmecisco 1:f2d7bec926ce 51 DigitalOut MotorR_FORWARD(P1_3);
bulmecisco 1:f2d7bec926ce 52 //DigitalOut MotorR_REVERSE(P1_4);
bulmecisco 1:f2d7bec926ce 53 PwmOut mg1(p34); //PWM Ausgang zum Motor
bulmecisco 1:f2d7bec926ce 54 PwmOut mg2(p36);
bulmecisco 1:f2d7bec926ce 55 mg1=mg2=speedy;
bulmecisco 1:f2d7bec926ce 56
bulmecisco 1:f2d7bec926ce 57 MotorR_EN=MotorL_EN=1; // Beide Motoren ENABLE
bulmecisco 1:f2d7bec926ce 58
bulmecisco 1:f2d7bec926ce 59 MotorR_FORWARD = MotorL_FORWARD = 1; // Beide Motoren vorwärts EIN
bulmecisco 1:f2d7bec926ce 60 wait_ms(2000); // warte 0,25 Sekunde
bulmecisco 1:f2d7bec926ce 61 MotorR_FORWARD = MotorL_FORWARD = 0; // Motoren AUS
bulmecisco 1:f2d7bec926ce 62 MotorR_EN=MotorL_EN=0; // Beide Motoren ENABLE
bulmecisco 1:f2d7bec926ce 63 wait_ms(500);
bulmecisco 1:f2d7bec926ce 64 }
bulmecisco 1:f2d7bec926ce 65 void move(int anzahl)
bulmecisco 1:f2d7bec926ce 66 {
bulmecisco 1:f2d7bec926ce 67
bulmecisco 1:f2d7bec926ce 68 for(int i=0; i < anzahl; i++)
bulmecisco 1:f2d7bec926ce 69 move();
bulmecisco 1:f2d7bec926ce 70
bulmecisco 1:f2d7bec926ce 71 }
bulmecisco 1:f2d7bec926ce 72
bulmecisco 1:f2d7bec926ce 73 void turnLeft()
bulmecisco 1:f2d7bec926ce 74 {
bulmecisco 1:f2d7bec926ce 75 // DigitalOut MotorL_EN(p34);
bulmecisco 1:f2d7bec926ce 76 // DigitalOut MotorL_FORWARD(P1_1);
bulmecisco 1:f2d7bec926ce 77 // DigitalOut MotorL_REVERSE(P1_0);
bulmecisco 1:f2d7bec926ce 78
bulmecisco 1:f2d7bec926ce 79 DigitalOut MotorR_EN(p36);
bulmecisco 1:f2d7bec926ce 80 DigitalOut MotorR_FORWARD(P1_3);
bulmecisco 1:f2d7bec926ce 81 // DigitalOut MotorR_REVERSE(P1_4);
bulmecisco 1:f2d7bec926ce 82 // MotorL_EN=1;
bulmecisco 2:43547160ab56 83 PwmOut mg2(p36);
bulmecisco 2:43547160ab56 84 mg2=1.0;
bulmecisco 1:f2d7bec926ce 85 MotorR_EN=1; // rechten Motor ENABLE
bulmecisco 1:f2d7bec926ce 86
bulmecisco 1:f2d7bec926ce 87 MotorR_FORWARD = 1; // rechter Motor vorwärts EIN
bulmecisco 1:f2d7bec926ce 88 wait_ms(750); // warte (90°)
bulmecisco 1:f2d7bec926ce 89 MotorR_FORWARD = 0; // Motoren AUS
bulmecisco 2:43547160ab56 90 MotorR_EN=0;
bulmecisco 2:43547160ab56 91
bulmecisco 2:43547160ab56 92 wait_ms(250); // warte (90°)
bulmecisco 2:43547160ab56 93 }
bulmecisco 2:43547160ab56 94 bool frontIsClear()
bulmecisco 2:43547160ab56 95 {
bulmecisco 2:43547160ab56 96 I2C i2c(p28,p27);
bulmecisco 2:43547160ab56 97 const int addr = 0x40; // I2C-Adresse PCA9555
bulmecisco 2:43547160ab56 98 char cmd[3];
bulmecisco 2:43547160ab56 99 int16_t btns;
bulmecisco 2:43547160ab56 100 i2c.frequency(40000); // I2C Frequenz 40kHz
bulmecisco 2:43547160ab56 101
bulmecisco 2:43547160ab56 102 cmd[0] = 0x06;
bulmecisco 2:43547160ab56 103 cmd[1] = 0x00;
bulmecisco 2:43547160ab56 104 i2c.write(addr, cmd, 2); // Define Port0 = Out
bulmecisco 2:43547160ab56 105
bulmecisco 2:43547160ab56 106 cmd[0]=0x01;
bulmecisco 2:43547160ab56 107 i2c.write(addr, cmd, 1);
bulmecisco 2:43547160ab56 108 i2c.read(addr|1, cmd, 1);
bulmecisco 2:43547160ab56 109 btns = cmd[0];
bulmecisco 2:43547160ab56 110 if( btns == BTN_FM )
bulmecisco 2:43547160ab56 111 return false;
bulmecisco 2:43547160ab56 112 else
bulmecisco 2:43547160ab56 113 return true;
bulmecisco 1:f2d7bec926ce 114 }
Enenkel 0:0b7c22955b8c 115
bulmecisco 2:43547160ab56 116 /*bool IsAnyFrontButton()
bulmecisco 2:43547160ab56 117 { return btns & (BTN_FL|BTN_FM|BTN_FR); }
bulmecisco 2:43547160ab56 118 */
Enenkel 0:0b7c22955b8c 119 // ************* Hauptprogramm ************
bulmecisco 1:f2d7bec926ce 120 int main()
bulmecisco 1:f2d7bec926ce 121 {
bulmecisco 1:f2d7bec926ce 122 //move();
bulmecisco 1:f2d7bec926ce 123 //turnLeft();
bulmecisco 2:43547160ab56 124 /* DigitalOut LED_D10(P1_8); // LED D10 und D13 definieren
bulmecisco 1:f2d7bec926ce 125 DigitalOut LED_D11(P1_9); // LED D10 und D13 definieren
bulmecisco 1:f2d7bec926ce 126 DigitalOut LED_D12(P1_10); // LED D10 und D13 definieren
bulmecisco 1:f2d7bec926ce 127 DigitalOut LED_D13(P1_11); //
bulmecisco 2:43547160ab56 128 */
bulmecisco 2:43547160ab56 129 //while(frontIsClear()) {}
Enenkel 0:0b7c22955b8c 130
bulmecisco 2:43547160ab56 131 turnLeft();
bulmecisco 2:43547160ab56 132 move();
bulmecisco 2:43547160ab56 133 wait(1);
bulmecisco 2:43547160ab56 134 /* moveSpeed(0.25);
bulmecisco 1:f2d7bec926ce 135 LED_D10 = 1;
bulmecisco 1:f2d7bec926ce 136 moveSpeed(0.3);
bulmecisco 1:f2d7bec926ce 137 LED_D11 = 1;
bulmecisco 1:f2d7bec926ce 138 moveSpeed(0.4);
bulmecisco 1:f2d7bec926ce 139 LED_D12 = 1;
bulmecisco 1:f2d7bec926ce 140 moveSpeed(0.5);
bulmecisco 1:f2d7bec926ce 141 LED_D13 = 1;
bulmecisco 1:f2d7bec926ce 142 moveSpeed(0.6);
bulmecisco 1:f2d7bec926ce 143 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0;
bulmecisco 1:f2d7bec926ce 144 moveSpeed(0.7);
bulmecisco 1:f2d7bec926ce 145 LED_D10 = 1;
bulmecisco 1:f2d7bec926ce 146 moveSpeed(0.8);
bulmecisco 1:f2d7bec926ce 147 LED_D11 = 1;
bulmecisco 1:f2d7bec926ce 148 moveSpeed(0.9);
bulmecisco 1:f2d7bec926ce 149 LED_D12 = 1;
bulmecisco 1:f2d7bec926ce 150 moveSpeed(1.0);
bulmecisco 1:f2d7bec926ce 151 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
bulmecisco 2:43547160ab56 152 PwmOut mg1(p34); //PWM Ausgang zum Motor
bulmecisco 2:43547160ab56 153 PwmOut mg2(p36);
bulmecisco 2:43547160ab56 154 mg1=mg2=0.0;
bulmecisco 2:43547160ab56 155 wait(2);
bulmecisco 2:43547160ab56 156 */ turnLeft();
bulmecisco 2:43547160ab56 157 move();
bulmecisco 1:f2d7bec926ce 158 //turnLeft();
bulmecisco 1:f2d7bec926ce 159 //move(5);
bulmecisco 1:f2d7bec926ce 160
bulmecisco 1:f2d7bec926ce 161 //wait(2);
bulmecisco 1:f2d7bec926ce 162 return 0;
bulmecisco 1:f2d7bec926ce 163 }