Alexander Doppler / Mbed 2 deprecated Bertl_Test

Bertl

Dependencies:   HCSR mbed

ur_Bertl.cpp@0:ae8f51bac163, 2015-04-23 (annotated)

Committer:
Alexander400
Date:
Thu Apr 23 12:04:34 2015 +0000
Revision:
0:ae8f51bac163
Bertl

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alexander400 0:ae8f51bac163 1 /***********************************
Alexander400 0:ae8f51bac163 2 name: ur_Bertl.cpp Version: 1.1
Alexander400 0:ae8f51bac163 3 author: PE HTL BULME
Alexander400 0:ae8f51bac163 4 email: pe@bulme.at
Alexander400 0:ae8f51bac163 5 description:
Alexander400 0:ae8f51bac163 6 Implementation portion of class ur_Bertl The Robot
Alexander400 0:ae8f51bac163 7 Blue LED and RGB LED test added
Alexander400 0:ae8f51bac163 8
Alexander400 0:ae8f51bac163 9 ***********************************/
Alexander400 0:ae8f51bac163 10 #include "mbed.h"
Alexander400 0:ae8f51bac163 11 #include "config.h"
Alexander400 0:ae8f51bac163 12 #include "ur_Bertl.h"
Alexander400 0:ae8f51bac163 13
Alexander400 0:ae8f51bac163 14 // Constructor
Alexander400 0:ae8f51bac163 15 ur_Bertl::ur_Bertl() : _interrupt(P1_12) // left sensor P1_13
Alexander400 0:ae8f51bac163 16 {
Alexander400 0:ae8f51bac163 17 i2c.frequency(40000); // I2C Frequenz 40kHz
Alexander400 0:ae8f51bac163 18 mg1 = mg2 = SPEED;
Alexander400 0:ae8f51bac163 19 _interrupt.rise(this, &ur_Bertl::increment); // attach increment function of this counter instance ie. motor sensor
Alexander400 0:ae8f51bac163 20 _count = 0;
Alexander400 0:ae8f51bac163 21 beepersInBag = 0;
Alexander400 0:ae8f51bac163 22 }
Alexander400 0:ae8f51bac163 23
Alexander400 0:ae8f51bac163 24 ur_Bertl::ur_Bertl(PinName pin) : _interrupt(pin) // create the InterruptIn on the pin specified to Counter
Alexander400 0:ae8f51bac163 25 {
Alexander400 0:ae8f51bac163 26 i2c.frequency(40000); // I2C Frequenz 40kHz
Alexander400 0:ae8f51bac163 27 mg1 = mg2 = SPEED;
Alexander400 0:ae8f51bac163 28 _interrupt.rise(this, &ur_Bertl::increment); // attach increment function of this counter instance ie. motor sensor
Alexander400 0:ae8f51bac163 29 _count = 0;
Alexander400 0:ae8f51bac163 30 beepersInBag = 0;
Alexander400 0:ae8f51bac163 31 }
Alexander400 0:ae8f51bac163 32
Alexander400 0:ae8f51bac163 33
Alexander400 0:ae8f51bac163 34 // Pulblic methodes
Alexander400 0:ae8f51bac163 35 void ur_Bertl::Move()
Alexander400 0:ae8f51bac163 36 {
Alexander400 0:ae8f51bac163 37 int count = _count;
Alexander400 0:ae8f51bac163 38 MotorR_EN=MotorL_EN=1; // both motor ENABLE
Alexander400 0:ae8f51bac163 39 MotorR_FORWARD = MotorL_FORWARD = 1; // both motor forward ON
Alexander400 0:ae8f51bac163 40
Alexander400 0:ae8f51bac163 41 while(_count < count+DISTANCE) {
Alexander400 0:ae8f51bac163 42 //if(!FrontIsClear()) // more convenient because there are no accidents :-)
Alexander400 0:ae8f51bac163 43 // break;
Alexander400 0:ae8f51bac163 44 #ifdef FRONTBUTTON
Alexander400 0:ae8f51bac163 45 if(frontButtonPressed())
Alexander400 0:ae8f51bac163 46 error();
Alexander400 0:ae8f51bac163 47 #endif
Alexander400 0:ae8f51bac163 48 DEBUG_PRINT("count: %d _count: %d", count, _count);
Alexander400 0:ae8f51bac163 49 }
Alexander400 0:ae8f51bac163 50 MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off
Alexander400 0:ae8f51bac163 51 MotorR_EN=MotorL_EN=0;
Alexander400 0:ae8f51bac163 52 wait_ms(250);
Alexander400 0:ae8f51bac163 53 }
Alexander400 0:ae8f51bac163 54
Alexander400 0:ae8f51bac163 55 void ur_Bertl::PutBeeper()
Alexander400 0:ae8f51bac163 56 {
Alexander400 0:ae8f51bac163 57 // wait_ms(500);
Alexander400 0:ae8f51bac163 58 if(beepersInBag > 0)
Alexander400 0:ae8f51bac163 59 beepersInBag--;
Alexander400 0:ae8f51bac163 60 else
Alexander400 0:ae8f51bac163 61 error();
Alexander400 0:ae8f51bac163 62 }
Alexander400 0:ae8f51bac163 63
Alexander400 0:ae8f51bac163 64 void ur_Bertl::PickBeeper()
Alexander400 0:ae8f51bac163 65 {
Alexander400 0:ae8f51bac163 66 // wait_ms(500);
Alexander400 0:ae8f51bac163 67 if (linesensor)
Alexander400 0:ae8f51bac163 68 beepersInBag++;
Alexander400 0:ae8f51bac163 69 else
Alexander400 0:ae8f51bac163 70 error();
Alexander400 0:ae8f51bac163 71 }
Alexander400 0:ae8f51bac163 72
Alexander400 0:ae8f51bac163 73 void ur_Bertl::TurnLeft()
Alexander400 0:ae8f51bac163 74 {
Alexander400 0:ae8f51bac163 75 int count = _count;
Alexander400 0:ae8f51bac163 76 MotorR_EN=MotorL_EN=1; // motor left and right ENABLE
Alexander400 0:ae8f51bac163 77
Alexander400 0:ae8f51bac163 78 MotorR_FORWARD = MotorL_REVERSE = 1;
Alexander400 0:ae8f51bac163 79 while(_count < count+ANGLE) {
Alexander400 0:ae8f51bac163 80 #ifdef FRONTBUTTON
Alexander400 0:ae8f51bac163 81 if(frontButtonPressed()) // get out if to much problems
Alexander400 0:ae8f51bac163 82 error();
Alexander400 0:ae8f51bac163 83 #endif
Alexander400 0:ae8f51bac163 84 DEBUG_PRINT("count: %d _count: %d", count, _count);
Alexander400 0:ae8f51bac163 85 }
Alexander400 0:ae8f51bac163 86 MotorR_FORWARD = MotorL_REVERSE = 0;
Alexander400 0:ae8f51bac163 87 MotorR_EN=MotorL_EN=0;
Alexander400 0:ae8f51bac163 88 wait_ms(250); // only to step the robot
Alexander400 0:ae8f51bac163 89 }
Alexander400 0:ae8f51bac163 90
Alexander400 0:ae8f51bac163 91 bool ur_Bertl::WaitUntilButtonPressed()
Alexander400 0:ae8f51bac163 92 {
Alexander400 0:ae8f51bac163 93 char cmd[3];
Alexander400 0:ae8f51bac163 94 int16_t btns;
Alexander400 0:ae8f51bac163 95 bool wert;
Alexander400 0:ae8f51bac163 96
Alexander400 0:ae8f51bac163 97 RGB_blue=RGB_red=RGB_green=0;
Alexander400 0:ae8f51bac163 98 cmd[0] = 0x06;
Alexander400 0:ae8f51bac163 99 cmd[1] = 0x00;
Alexander400 0:ae8f51bac163 100 i2c.write(addr, cmd, 2);
Alexander400 0:ae8f51bac163 101
Alexander400 0:ae8f51bac163 102 cmd[0]=0x01;
Alexander400 0:ae8f51bac163 103 i2c.write(addr, cmd, 1);
Alexander400 0:ae8f51bac163 104 i2c.read(addr|1, cmd, 1);
Alexander400 0:ae8f51bac163 105 btns = cmd[0];
Alexander400 0:ae8f51bac163 106 if( btns & (0xFF))
Alexander400 0:ae8f51bac163 107 wert = false;
Alexander400 0:ae8f51bac163 108 else
Alexander400 0:ae8f51bac163 109 wert = true;
Alexander400 0:ae8f51bac163 110 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Alexander400 0:ae8f51bac163 111 return wert;
Alexander400 0:ae8f51bac163 112 }
Alexander400 0:ae8f51bac163 113
Alexander400 0:ae8f51bac163 114 void ur_Bertl::ShutOff()
Alexander400 0:ae8f51bac163 115 {
Alexander400 0:ae8f51bac163 116 MotorR_FORWARD = MotorL_FORWARD = 0; // motor OFF
Alexander400 0:ae8f51bac163 117 MotorR_EN=MotorL_EN=0; // motor disable
Alexander400 0:ae8f51bac163 118 }
Alexander400 0:ae8f51bac163 119
Alexander400 0:ae8f51bac163 120 // LEDs methodes
Alexander400 0:ae8f51bac163 121
Alexander400 0:ae8f51bac163 122 void ur_Bertl::BlueLedsON()
Alexander400 0:ae8f51bac163 123 {
Alexander400 0:ae8f51bac163 124 LED_blue=0;
Alexander400 0:ae8f51bac163 125 }
Alexander400 0:ae8f51bac163 126
Alexander400 0:ae8f51bac163 127 void ur_Bertl::BlueLedsOFF()
Alexander400 0:ae8f51bac163 128 {
Alexander400 0:ae8f51bac163 129 LED_blue=1;
Alexander400 0:ae8f51bac163 130 }
Alexander400 0:ae8f51bac163 131
Alexander400 0:ae8f51bac163 132 void ur_Bertl::RGBLed(bool red, bool green, bool blue)
Alexander400 0:ae8f51bac163 133 {
Alexander400 0:ae8f51bac163 134 RGB_blue=!blue;
Alexander400 0:ae8f51bac163 135 RGB_red=!red;
Alexander400 0:ae8f51bac163 136 RGB_green=!green;
Alexander400 0:ae8f51bac163 137 }
Alexander400 0:ae8f51bac163 138
Alexander400 0:ae8f51bac163 139 void ur_Bertl::TurnLedOn(int16_t led)
Alexander400 0:ae8f51bac163 140 {
Alexander400 0:ae8f51bac163 141 char cmd[3];
Alexander400 0:ae8f51bac163 142
Alexander400 0:ae8f51bac163 143 cmd[0] = 0x02;
Alexander400 0:ae8f51bac163 144 cmd[1] = ~led;
Alexander400 0:ae8f51bac163 145 i2c.write(addr, cmd, 2);
Alexander400 0:ae8f51bac163 146 wait(0.5);
Alexander400 0:ae8f51bac163 147 }
Alexander400 0:ae8f51bac163 148
Alexander400 0:ae8f51bac163 149 void ur_Bertl::TurnLedOff(int16_t led)
Alexander400 0:ae8f51bac163 150 {
Alexander400 0:ae8f51bac163 151 char cmd[3];
Alexander400 0:ae8f51bac163 152
Alexander400 0:ae8f51bac163 153 cmd[0] = 0x02;
Alexander400 0:ae8f51bac163 154 cmd[1] = led;
Alexander400 0:ae8f51bac163 155 i2c.write(addr, cmd, 2);
Alexander400 0:ae8f51bac163 156 wait(0.5);
Alexander400 0:ae8f51bac163 157 }
Alexander400 0:ae8f51bac163 158
Alexander400 0:ae8f51bac163 159 void ur_Bertl::NibbleLeds(int value)
Alexander400 0:ae8f51bac163 160 {
Alexander400 0:ae8f51bac163 161 NibbleLEDs = value%16;
Alexander400 0:ae8f51bac163 162 }
Alexander400 0:ae8f51bac163 163
Alexander400 0:ae8f51bac163 164 //-----------------INTERNAL USE ONLY ----------------------------
Alexander400 0:ae8f51bac163 165 void ur_Bertl::error()
Alexander400 0:ae8f51bac163 166 {
Alexander400 0:ae8f51bac163 167 int wait = 500;
Alexander400 0:ae8f51bac163 168 MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off
Alexander400 0:ae8f51bac163 169 MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off
Alexander400 0:ae8f51bac163 170 MotorR_EN=MotorL_EN=0;
Alexander400 0:ae8f51bac163 171 while(1) {
Alexander400 0:ae8f51bac163 172 TurnLedOff(0xFF);
Alexander400 0:ae8f51bac163 173 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0;
Alexander400 0:ae8f51bac163 174 LED_blue=1;
Alexander400 0:ae8f51bac163 175 RGB_blue=RGB_green=RGB_red=1;
Alexander400 0:ae8f51bac163 176 wait_ms(wait);
Alexander400 0:ae8f51bac163 177 TurnLedOn(0xFF);
Alexander400 0:ae8f51bac163 178 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
Alexander400 0:ae8f51bac163 179 LED_blue=0;
Alexander400 0:ae8f51bac163 180 RGB_blue=RGB_green=1;RGB_red=0;
Alexander400 0:ae8f51bac163 181 wait_ms(wait);
Alexander400 0:ae8f51bac163 182 }
Alexander400 0:ae8f51bac163 183 }
Alexander400 0:ae8f51bac163 184
Alexander400 0:ae8f51bac163 185 bool ur_Bertl::frontButtonPressed()
Alexander400 0:ae8f51bac163 186 {
Alexander400 0:ae8f51bac163 187 char cmd[3]; // array for I2C
Alexander400 0:ae8f51bac163 188 int16_t btns;
Alexander400 0:ae8f51bac163 189 bool wert;
Alexander400 0:ae8f51bac163 190
Alexander400 0:ae8f51bac163 191 cmd[0] = 0x06;
Alexander400 0:ae8f51bac163 192 cmd[1] = 0x00;
Alexander400 0:ae8f51bac163 193 i2c.write(addr, cmd, 2);
Alexander400 0:ae8f51bac163 194
Alexander400 0:ae8f51bac163 195 cmd[0]=0x01;
Alexander400 0:ae8f51bac163 196 i2c.write(addr, cmd, 1);
Alexander400 0:ae8f51bac163 197 i2c.read(addr|1, cmd, 1);
Alexander400 0:ae8f51bac163 198 btns = cmd[0];
Alexander400 0:ae8f51bac163 199 if( btns & (BTN_FL|BTN_FM|BTN_FR|BTN_FRR|BTN_FLL))
Alexander400 0:ae8f51bac163 200 wert = true;
Alexander400 0:ae8f51bac163 201 else
Alexander400 0:ae8f51bac163 202 wert = false;
Alexander400 0:ae8f51bac163 203 DEBUG_PRINT("WERT: %d", wert);
Alexander400 0:ae8f51bac163 204 return wert;
Alexander400 0:ae8f51bac163 205 }
Alexander400 0:ae8f51bac163 206
Alexander400 0:ae8f51bac163 207 // ISR
Alexander400 0:ae8f51bac163 208
Alexander400 0:ae8f51bac163 209 void ur_Bertl::increment()
Alexander400 0:ae8f51bac163 210 {
Alexander400 0:ae8f51bac163 211 _count++;
Alexander400 0:ae8f51bac163 212 }
Alexander400 0:ae8f51bac163 213
Alexander400 0:ae8f51bac163 214 int ur_Bertl::Read()
Alexander400 0:ae8f51bac163 215 {
Alexander400 0:ae8f51bac163 216 return _count;
Alexander400 0:ae8f51bac163 217 }
Alexander400 0:ae8f51bac163 218
Alexander400 0:ae8f51bac163 219 //----------------------------------------------------------------------
Alexander400 0:ae8f51bac163 220 /*
Alexander400 0:ae8f51bac163 221 bool ur_Bertl::FrontIsClear()
Alexander400 0:ae8f51bac163 222 {
Alexander400 0:ae8f51bac163 223 int dist = 0;
Alexander400 0:ae8f51bac163 224 usensor.start();
Alexander400 0:ae8f51bac163 225 wait_ms(10);
Alexander400 0:ae8f51bac163 226 dist=usensor.get_dist_cm();
Alexander400 0:ae8f51bac163 227 if(dist < 5)
Alexander400 0:ae8f51bac163 228 return false;
Alexander400 0:ae8f51bac163 229 else
Alexander400 0:ae8f51bac163 230 return true;
Alexander400 0:ae8f51bac163 231 DEBUG_PRINT("Distance: %d", dist);
Alexander400 0:ae8f51bac163 232
Alexander400 0:ae8f51bac163 233 // if there is no ultra sonic sensor use this - with front buttons
Alexander400 0:ae8f51bac163 234 char cmd[3]; // array for I2C
Alexander400 0:ae8f51bac163 235 int16_t btns;
Alexander400 0:ae8f51bac163 236 bool wert;
Alexander400 0:ae8f51bac163 237
Alexander400 0:ae8f51bac163 238 cmd[0] = 0x06;
Alexander400 0:ae8f51bac163 239 cmd[1] = 0x00;
Alexander400 0:ae8f51bac163 240 i2c.write(addr, cmd, 2);
Alexander400 0:ae8f51bac163 241
Alexander400 0:ae8f51bac163 242 cmd[0]=0x01;
Alexander400 0:ae8f51bac163 243 i2c.write(addr, cmd, 1);
Alexander400 0:ae8f51bac163 244 i2c.read(addr|1, cmd, 1);
Alexander400 0:ae8f51bac163 245 btns = cmd[0];
Alexander400 0:ae8f51bac163 246 if( btns & (BTN_FL|BTN_FM|BTN_FR))
Alexander400 0:ae8f51bac163 247 wert = false;
Alexander400 0:ae8f51bac163 248 else
Alexander400 0:ae8f51bac163 249 wert = true;
Alexander400 0:ae8f51bac163 250 DEBUG_PRINT("WERT: %d", wert);
Alexander400 0:ae8f51bac163 251 return wert;
Alexander400 0:ae8f51bac163 252 }
Alexander400 0:ae8f51bac163 253
Alexander400 0:ae8f51bac163 254
Alexander400 0:ae8f51bac163 255 bool ur_Bertl::NextToABeeper()
Alexander400 0:ae8f51bac163 256 {
Alexander400 0:ae8f51bac163 257 if (BottomIsBlack())
Alexander400 0:ae8f51bac163 258 return true;
Alexander400 0:ae8f51bac163 259 else
Alexander400 0:ae8f51bac163 260 return false;
Alexander400 0:ae8f51bac163 261 }
Alexander400 0:ae8f51bac163 262
Alexander400 0:ae8f51bac163 263 int ur_Bertl::AnyBeeperInBag()
Alexander400 0:ae8f51bac163 264 {
Alexander400 0:ae8f51bac163 265 if(beepersInBag > 0)
Alexander400 0:ae8f51bac163 266 return beepersInBag;
Alexander400 0:ae8f51bac163 267 else
Alexander400 0:ae8f51bac163 268 return 0;
Alexander400 0:ae8f51bac163 269 }
Alexander400 0:ae8f51bac163 270
Alexander400 0:ae8f51bac163 271 bool ur_Bertl::backIsClear()
Alexander400 0:ae8f51bac163 272 {
Alexander400 0:ae8f51bac163 273 char cmd[3]; // array for I2C
Alexander400 0:ae8f51bac163 274 int16_t btns;
Alexander400 0:ae8f51bac163 275 bool wert;
Alexander400 0:ae8f51bac163 276
Alexander400 0:ae8f51bac163 277 cmd[0] = 0x06;
Alexander400 0:ae8f51bac163 278 cmd[1] = 0x00;
Alexander400 0:ae8f51bac163 279 i2c.write(addr, cmd, 2);
Alexander400 0:ae8f51bac163 280
Alexander400 0:ae8f51bac163 281 cmd[0]=0x01;
Alexander400 0:ae8f51bac163 282 i2c.write(addr, cmd, 1);
Alexander400 0:ae8f51bac163 283 i2c.read(addr|1, cmd, 1);
Alexander400 0:ae8f51bac163 284 btns = cmd[0];
Alexander400 0:ae8f51bac163 285 if( btns & (BTN_BL|BTN_BM|BTN_BR))
Alexander400 0:ae8f51bac163 286 wert = false;
Alexander400 0:ae8f51bac163 287 else
Alexander400 0:ae8f51bac163 288 wert = true;
Alexander400 0:ae8f51bac163 289 DEBUG_PRINT("WERT: %d", wert);
Alexander400 0:ae8f51bac163 290 return wert;
Alexander400 0:ae8f51bac163 291 }
Alexander400 0:ae8f51bac163 292
Alexander400 0:ae8f51bac163 293 int ur_Bertl::BottomIsBlack()
Alexander400 0:ae8f51bac163 294 {
Alexander400 0:ae8f51bac163 295 int detect;
Alexander400 0:ae8f51bac163 296
Alexander400 0:ae8f51bac163 297 detect = linesensor;
Alexander400 0:ae8f51bac163 298 return detect;
Alexander400 0:ae8f51bac163 299 }
Alexander400 0:ae8f51bac163 300
Alexander400 0:ae8f51bac163 301 void ur_Bertl::MoveBackwards()
Alexander400 0:ae8f51bac163 302 {
Alexander400 0:ae8f51bac163 303 int count = _count;
Alexander400 0:ae8f51bac163 304 //wait_ms(250); // waite until Bertl stops
Alexander400 0:ae8f51bac163 305 MotorR_EN=MotorL_EN=1; // both motor ENABLE
Alexander400 0:ae8f51bac163 306 MotorR_REVERSE = MotorL_REVERSE = 1; // both motor backwards ON
Alexander400 0:ae8f51bac163 307 while(_count < count+DISTANCE) {
Alexander400 0:ae8f51bac163 308 if(!backIsClear())
Alexander400 0:ae8f51bac163 309 break;
Alexander400 0:ae8f51bac163 310 DEBUG_PRINT("count: %d _count: %d", count, _count);
Alexander400 0:ae8f51bac163 311 }
Alexander400 0:ae8f51bac163 312 MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off
Alexander400 0:ae8f51bac163 313 MotorR_EN=MotorL_EN=0;
Alexander400 0:ae8f51bac163 314 wait_ms(250);
Alexander400 0:ae8f51bac163 315 }
Alexander400 0:ae8f51bac163 316 */