BERTL_CHEL18 / Mbed 2 deprecated Func_Bertl_Ultra

ultra sonic sensor asdded

Dependencies:   HCSR mbed

Fork of func_Bertl by BULME_BERTL14

main.cpp@3:c7e5419fa980, 2015-01-03 (annotated)

Committer:
bulmecisco
Date:
Sat Jan 03 17:32:26 2015 +0000
Revision:
3:c7e5419fa980
Parent:
2:43547160ab56
Child:
4:a975caedface
Karel frontIsClear OK

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 3:c7e5419fa980 20 bool frontIsClear();
bulmecisco 3:c7e5419fa980 21 void shutOff()
bulmecisco 3:c7e5419fa980 22 {
bulmecisco 3:c7e5419fa980 23 /* DigitalOut MotorL_EN(p34);
bulmecisco 1:f2d7bec926ce 24 DigitalOut MotorL_FORWARD(P1_1);
bulmecisco 1:f2d7bec926ce 25 DigitalOut MotorR_EN(p36);
bulmecisco 1:f2d7bec926ce 26 DigitalOut MotorR_FORWARD(P1_3);
bulmecisco 3:c7e5419fa980 27 */ PwmOut mg1(p34); //PWM Ausgang zum Motor
bulmecisco 3:c7e5419fa980 28 PwmOut mg2(p36);
bulmecisco 3:c7e5419fa980 29 mg1=mg2=0.0;
bulmecisco 3:c7e5419fa980 30
bulmecisco 3:c7e5419fa980 31 // MotorR_FORWARD = MotorL_FORWARD = 0; // Motoren AUS
bulmecisco 3:c7e5419fa980 32 // MotorR_EN=MotorL_EN=0;
bulmecisco 3:c7e5419fa980 33 }
bulmecisco 3:c7e5419fa980 34 bool move()
bulmecisco 3:c7e5419fa980 35 {
bulmecisco 3:c7e5419fa980 36 DigitalOut MotorL_EN(p34);
bulmecisco 3:c7e5419fa980 37 DigitalOut MotorL_FORWARD(P1_0);
bulmecisco 3:c7e5419fa980 38 //DigitalOut MotorL_REVERSE(P1_1);
bulmecisco 3:c7e5419fa980 39
bulmecisco 3:c7e5419fa980 40 DigitalOut MotorR_EN(p36);
bulmecisco 3:c7e5419fa980 41 DigitalOut MotorR_FORWARD(P1_4);
bulmecisco 3:c7e5419fa980 42 //DigitalOut MotorR_REVERSE(P1_3);
bulmecisco 2:43547160ab56 43 PwmOut mg1(p34); //PWM Ausgang zum Motor
bulmecisco 2:43547160ab56 44 PwmOut mg2(p36);
bulmecisco 3:c7e5419fa980 45 if(!frontIsClear()) {
bulmecisco 3:c7e5419fa980 46 mg1=mg2=0.0;
bulmecisco 3:c7e5419fa980 47 return false;
bulmecisco 3:c7e5419fa980 48 } else {
bulmecisco 3:c7e5419fa980 49 mg1=mg2=1.0;
bulmecisco 3:c7e5419fa980 50 MotorR_EN=MotorL_EN=1; // Beide Motoren ENABLE
bulmecisco 1:f2d7bec926ce 51
bulmecisco 3:c7e5419fa980 52 MotorR_FORWARD = MotorL_FORWARD = 1; // Beide Motoren vorwärts EIN
bulmecisco 3:c7e5419fa980 53 wait_ms(250); // warte 0,25 Sekunde
bulmecisco 3:c7e5419fa980 54 MotorR_FORWARD = MotorL_FORWARD = 0; // Motoren AUS
bulmecisco 3:c7e5419fa980 55 MotorR_EN=MotorL_EN=0;
bulmecisco 3:c7e5419fa980 56 //mg1=mg2=0.0;
bulmecisco 3:c7e5419fa980 57 wait_ms(250);
bulmecisco 3:c7e5419fa980 58 return true;
bulmecisco 3:c7e5419fa980 59 }
bulmecisco 1:f2d7bec926ce 60 }
bulmecisco 1:f2d7bec926ce 61
bulmecisco 1:f2d7bec926ce 62 void moveSpeed(float speedy)
bulmecisco 1:f2d7bec926ce 63 {
bulmecisco 1:f2d7bec926ce 64 DigitalOut MotorL_EN(p34);
bulmecisco 3:c7e5419fa980 65 DigitalOut MotorL_FORWARD(P1_0);
bulmecisco 3:c7e5419fa980 66 //DigitalOut MotorL_REVERSE(P1_1);
Enenkel 0:0b7c22955b8c 67
bulmecisco 1:f2d7bec926ce 68 DigitalOut MotorR_EN(p36);
bulmecisco 3:c7e5419fa980 69 DigitalOut MotorR_FORWARD(P1_4);
bulmecisco 3:c7e5419fa980 70 //DigitalOut MotorR_REVERSE(P1_3);
bulmecisco 1:f2d7bec926ce 71 PwmOut mg1(p34); //PWM Ausgang zum Motor
bulmecisco 1:f2d7bec926ce 72 PwmOut mg2(p36);
bulmecisco 1:f2d7bec926ce 73 mg1=mg2=speedy;
bulmecisco 1:f2d7bec926ce 74
bulmecisco 1:f2d7bec926ce 75 MotorR_EN=MotorL_EN=1; // Beide Motoren ENABLE
bulmecisco 1:f2d7bec926ce 76 MotorR_FORWARD = MotorL_FORWARD = 1; // Beide Motoren vorwärts EIN
bulmecisco 1:f2d7bec926ce 77 wait_ms(2000); // warte 0,25 Sekunde
bulmecisco 1:f2d7bec926ce 78 MotorR_FORWARD = MotorL_FORWARD = 0; // Motoren AUS
bulmecisco 1:f2d7bec926ce 79 MotorR_EN=MotorL_EN=0; // Beide Motoren ENABLE
bulmecisco 1:f2d7bec926ce 80 wait_ms(500);
bulmecisco 1:f2d7bec926ce 81 }
bulmecisco 1:f2d7bec926ce 82 void move(int anzahl)
bulmecisco 1:f2d7bec926ce 83 {
bulmecisco 1:f2d7bec926ce 84
bulmecisco 1:f2d7bec926ce 85 for(int i=0; i < anzahl; i++)
bulmecisco 1:f2d7bec926ce 86 move();
bulmecisco 1:f2d7bec926ce 87
bulmecisco 1:f2d7bec926ce 88 }
bulmecisco 3:c7e5419fa980 89 void turnBack()
bulmecisco 3:c7e5419fa980 90 {
bulmecisco 3:c7e5419fa980 91 // DigitalOut MotorL_EN(p34);
bulmecisco 3:c7e5419fa980 92 // DigitalOut MotorL_FORWARD(P1_1);
bulmecisco 3:c7e5419fa980 93 // DigitalOut MotorL_REVERSE(P1_0);
bulmecisco 3:c7e5419fa980 94
bulmecisco 3:c7e5419fa980 95 DigitalOut MotorR_EN(p36);
bulmecisco 3:c7e5419fa980 96 // DigitalOut MotorR_FORWARD(P1_4);
bulmecisco 3:c7e5419fa980 97 DigitalOut MotorR_REVERSE(P1_3);
bulmecisco 3:c7e5419fa980 98 // MotorL_EN=1;
bulmecisco 3:c7e5419fa980 99 PwmOut mg2(p36);
bulmecisco 3:c7e5419fa980 100 mg2=1.0;
bulmecisco 3:c7e5419fa980 101 MotorR_EN=1; // rechten Motor ENABLE
bulmecisco 3:c7e5419fa980 102
bulmecisco 3:c7e5419fa980 103 MotorR_REVERSE = 1; // rechter Motor vorwärts EIN
bulmecisco 3:c7e5419fa980 104 wait_ms(750); // warte (90°)
bulmecisco 3:c7e5419fa980 105 MotorR_REVERSE = 0; // Motoren AUS
bulmecisco 3:c7e5419fa980 106 MotorR_EN=0;
bulmecisco 3:c7e5419fa980 107
bulmecisco 3:c7e5419fa980 108 wait_ms(250); // warte (90°)
bulmecisco 3:c7e5419fa980 109 }
bulmecisco 1:f2d7bec926ce 110
bulmecisco 1:f2d7bec926ce 111 void turnLeft()
bulmecisco 1:f2d7bec926ce 112 {
bulmecisco 1:f2d7bec926ce 113 // DigitalOut MotorL_EN(p34);
bulmecisco 1:f2d7bec926ce 114 // DigitalOut MotorL_FORWARD(P1_1);
bulmecisco 1:f2d7bec926ce 115 // DigitalOut MotorL_REVERSE(P1_0);
bulmecisco 1:f2d7bec926ce 116
bulmecisco 1:f2d7bec926ce 117 DigitalOut MotorR_EN(p36);
bulmecisco 3:c7e5419fa980 118 DigitalOut MotorR_FORWARD(P1_4);
bulmecisco 3:c7e5419fa980 119 // DigitalOut MotorR_REVERSE(P1_3);
bulmecisco 1:f2d7bec926ce 120 // MotorL_EN=1;
bulmecisco 2:43547160ab56 121 PwmOut mg2(p36);
bulmecisco 2:43547160ab56 122 mg2=1.0;
bulmecisco 1:f2d7bec926ce 123 MotorR_EN=1; // rechten Motor ENABLE
bulmecisco 1:f2d7bec926ce 124
bulmecisco 1:f2d7bec926ce 125 MotorR_FORWARD = 1; // rechter Motor vorwärts EIN
bulmecisco 3:c7e5419fa980 126 wait_ms(500); // warte (90°)
bulmecisco 1:f2d7bec926ce 127 MotorR_FORWARD = 0; // Motoren AUS
bulmecisco 2:43547160ab56 128 MotorR_EN=0;
bulmecisco 2:43547160ab56 129
bulmecisco 2:43547160ab56 130 wait_ms(250); // warte (90°)
bulmecisco 2:43547160ab56 131 }
bulmecisco 2:43547160ab56 132 bool frontIsClear()
bulmecisco 2:43547160ab56 133 {
bulmecisco 2:43547160ab56 134 I2C i2c(p28,p27);
bulmecisco 2:43547160ab56 135 const int addr = 0x40; // I2C-Adresse PCA9555
bulmecisco 2:43547160ab56 136 char cmd[3];
bulmecisco 2:43547160ab56 137 int16_t btns;
bulmecisco 2:43547160ab56 138 i2c.frequency(40000); // I2C Frequenz 40kHz
bulmecisco 2:43547160ab56 139
bulmecisco 2:43547160ab56 140 cmd[0] = 0x06;
bulmecisco 2:43547160ab56 141 cmd[1] = 0x00;
bulmecisco 2:43547160ab56 142 i2c.write(addr, cmd, 2); // Define Port0 = Out
bulmecisco 2:43547160ab56 143
bulmecisco 2:43547160ab56 144 cmd[0]=0x01;
bulmecisco 2:43547160ab56 145 i2c.write(addr, cmd, 1);
bulmecisco 2:43547160ab56 146 i2c.read(addr|1, cmd, 1);
bulmecisco 2:43547160ab56 147 btns = cmd[0];
bulmecisco 3:c7e5419fa980 148 if( btns & (BTN_FL|BTN_FM|BTN_FR))
bulmecisco 2:43547160ab56 149 return false;
bulmecisco 2:43547160ab56 150 else
bulmecisco 2:43547160ab56 151 return true;
bulmecisco 1:f2d7bec926ce 152 }
Enenkel 0:0b7c22955b8c 153
bulmecisco 2:43547160ab56 154 /*bool IsAnyFrontButton()
bulmecisco 2:43547160ab56 155 { return btns & (BTN_FL|BTN_FM|BTN_FR); }
bulmecisco 2:43547160ab56 156 */
Enenkel 0:0b7c22955b8c 157 // ************* Hauptprogramm ************
bulmecisco 1:f2d7bec926ce 158 int main()
bulmecisco 1:f2d7bec926ce 159 {
bulmecisco 1:f2d7bec926ce 160 //move();
bulmecisco 1:f2d7bec926ce 161 //turnLeft();
bulmecisco 2:43547160ab56 162 /* DigitalOut LED_D10(P1_8); // LED D10 und D13 definieren
bulmecisco 1:f2d7bec926ce 163 DigitalOut LED_D11(P1_9); // LED D10 und D13 definieren
bulmecisco 1:f2d7bec926ce 164 DigitalOut LED_D12(P1_10); // LED D10 und D13 definieren
bulmecisco 1:f2d7bec926ce 165 DigitalOut LED_D13(P1_11); //
bulmecisco 2:43547160ab56 166 */
bulmecisco 3:c7e5419fa980 167 while(frontIsClear()) {}
Enenkel 0:0b7c22955b8c 168
bulmecisco 3:c7e5419fa980 169 move();
bulmecisco 2:43547160ab56 170 turnLeft();
bulmecisco 3:c7e5419fa980 171 if(move())
bulmecisco 3:c7e5419fa980 172 turnBack();
bulmecisco 3:c7e5419fa980 173 if(move())
bulmecisco 3:c7e5419fa980 174 turnBack();
bulmecisco 3:c7e5419fa980 175 if(move())
bulmecisco 3:c7e5419fa980 176 turnBack();
bulmecisco 2:43547160ab56 177 wait(1);
bulmecisco 2:43547160ab56 178 /* moveSpeed(0.25);
bulmecisco 1:f2d7bec926ce 179 LED_D10 = 1;
bulmecisco 1:f2d7bec926ce 180 moveSpeed(0.3);
bulmecisco 1:f2d7bec926ce 181 LED_D11 = 1;
bulmecisco 1:f2d7bec926ce 182 moveSpeed(0.4);
bulmecisco 1:f2d7bec926ce 183 LED_D12 = 1;
bulmecisco 1:f2d7bec926ce 184 moveSpeed(0.5);
bulmecisco 1:f2d7bec926ce 185 LED_D13 = 1;
bulmecisco 1:f2d7bec926ce 186 moveSpeed(0.6);
bulmecisco 1:f2d7bec926ce 187 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0;
bulmecisco 1:f2d7bec926ce 188 moveSpeed(0.7);
bulmecisco 1:f2d7bec926ce 189 LED_D10 = 1;
bulmecisco 1:f2d7bec926ce 190 moveSpeed(0.8);
bulmecisco 1:f2d7bec926ce 191 LED_D11 = 1;
bulmecisco 1:f2d7bec926ce 192 moveSpeed(0.9);
bulmecisco 1:f2d7bec926ce 193 LED_D12 = 1;
bulmecisco 1:f2d7bec926ce 194 moveSpeed(1.0);
bulmecisco 1:f2d7bec926ce 195 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
bulmecisco 2:43547160ab56 196 PwmOut mg1(p34); //PWM Ausgang zum Motor
bulmecisco 2:43547160ab56 197 PwmOut mg2(p36);
bulmecisco 2:43547160ab56 198 mg1=mg2=0.0;
bulmecisco 2:43547160ab56 199 wait(2);
bulmecisco 3:c7e5419fa980 200 */
bulmecisco 3:c7e5419fa980 201 move(5);
bulmecisco 3:c7e5419fa980 202 turnLeft();
bulmecisco 2:43547160ab56 203 move();
bulmecisco 3:c7e5419fa980 204 turnLeft();
bulmecisco 3:c7e5419fa980 205 shutOff();
bulmecisco 1:f2d7bec926ce 206 //turnLeft();
bulmecisco 1:f2d7bec926ce 207 //move(5);
bulmecisco 1:f2d7bec926ce 208
bulmecisco 1:f2d7bec926ce 209 //wait(2);
bulmecisco 1:f2d7bec926ce 210 return 0;
bulmecisco 1:f2d7bec926ce 211 }