Richard Kraus / BertlUebung

BertlPractise

Dependencies:   HCSR

Fork of Bertl by Franz Pucher

ur_Bertl.cpp@13:3ce84646fd74, 2015-04-27 (annotated)

Committer:
bulmecisco
Date:
Mon Apr 27 13:13:45 2015 +0000
Revision:
13:3ce84646fd74
Parent:
12:cedc088eaf05
Child:
15:43d6a7e6e64a
Bertl version 3.0 updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bulmecisco 0:66e9a0afcbd6 1 /***********************************
bulmecisco 13:3ce84646fd74 2 name: ur_Bertl.cpp Version: 3.0
bulmecisco 13:3ce84646fd74 3 class Bertl included
bulmecisco 0:66e9a0afcbd6 4 author: PE HTL BULME
bulmecisco 0:66e9a0afcbd6 5 email: pe@bulme.at
bulmecisco 4:76acfddc26fb 6 WIKI: https://developer.mbed.org/teams/BERTL_CHEL_18/code/ur_Bertl/
bulmecisco 0:66e9a0afcbd6 7 description:
bulmecisco 4:76acfddc26fb 8 Implementation portion of class ur_Bertl The Robot
bulmecisco 7:e7f74f072564 9 boolean commands added for if/else, while, ...
bulmecisco 7:e7f74f072564 10 int ReturnButtonPressed() added which returns the int value of button pressed
bulmecisco 0:66e9a0afcbd6 11 ***********************************/
bulmecisco 0:66e9a0afcbd6 12 #include "mbed.h"
bulmecisco 0:66e9a0afcbd6 13 #include "config.h"
bulmecisco 0:66e9a0afcbd6 14 #include "ur_Bertl.h"
bulmecisco 0:66e9a0afcbd6 15
bulmecisco 0:66e9a0afcbd6 16 // Constructor
bulmecisco 13:3ce84646fd74 17 ur_Bertl::ur_Bertl() : _interrupt(MOTORENC)
bulmecisco 0:66e9a0afcbd6 18 {
bulmecisco 4:76acfddc26fb 19 i2c.frequency(40000); // I2C init
bulmecisco 3:01b183fe8b41 20 char init1[2] = {0x6, 0x00};
bulmecisco 3:01b183fe8b41 21 char init2[2] = {0x7, 0xff};
bulmecisco 3:01b183fe8b41 22 i2c.write(0x40, init1, 2);
bulmecisco 3:01b183fe8b41 23 i2c.write(0x40, init2, 2);
bulmecisco 0:66e9a0afcbd6 24 mg1 = mg2 = SPEED;
bulmecisco 0:66e9a0afcbd6 25 _interrupt.rise(this, &ur_Bertl::increment); // attach increment function of this counter instance ie. motor sensor
bulmecisco 0:66e9a0afcbd6 26 _count = 0;
bulmecisco 0:66e9a0afcbd6 27 beepersInBag = 0;
bulmecisco 0:66e9a0afcbd6 28 }
bulmecisco 0:66e9a0afcbd6 29
bulmecisco 0:66e9a0afcbd6 30 ur_Bertl::ur_Bertl(PinName pin) : _interrupt(pin) // create the InterruptIn on the pin specified to Counter
bulmecisco 0:66e9a0afcbd6 31 {
bulmecisco 4:76acfddc26fb 32 i2c.frequency(40000); // I2C init
bulmecisco 3:01b183fe8b41 33 char init1[2] = {0x6, 0x00};
bulmecisco 3:01b183fe8b41 34 char init2[2] = {0x7, 0xff};
bulmecisco 3:01b183fe8b41 35 i2c.write(0x40, init1, 2);
bulmecisco 3:01b183fe8b41 36 i2c.write(0x40, init2, 2);
bulmecisco 0:66e9a0afcbd6 37 mg1 = mg2 = SPEED;
bulmecisco 0:66e9a0afcbd6 38 _interrupt.rise(this, &ur_Bertl::increment); // attach increment function of this counter instance ie. motor sensor
bulmecisco 0:66e9a0afcbd6 39 _count = 0;
bulmecisco 0:66e9a0afcbd6 40 beepersInBag = 0;
bulmecisco 0:66e9a0afcbd6 41 }
bulmecisco 0:66e9a0afcbd6 42
bulmecisco 0:66e9a0afcbd6 43 // Pulblic methodes
bulmecisco 12:cedc088eaf05 44 void ur_Bertl::Move(int move)
bulmecisco 0:66e9a0afcbd6 45 {
bulmecisco 0:66e9a0afcbd6 46 int count = _count;
bulmecisco 0:66e9a0afcbd6 47 MotorR_EN=MotorL_EN=1; // both motor ENABLE
bulmecisco 0:66e9a0afcbd6 48 MotorR_FORWARD = MotorL_FORWARD = 1; // both motor forward ON
bulmecisco 2:1cd559ff516b 49 #ifdef TIME
bulmecisco 12:cedc088eaf05 50 wait_ms(move);
bulmecisco 3:01b183fe8b41 51 #else
bulmecisco 0:66e9a0afcbd6 52
bulmecisco 12:cedc088eaf05 53 while(_count < count+move) {
bulmecisco 0:66e9a0afcbd6 54 //if(!FrontIsClear()) // more convenient because there are no accidents :-)
bulmecisco 0:66e9a0afcbd6 55 // break;
bulmecisco 0:66e9a0afcbd6 56 #ifdef FRONTBUTTON
bulmecisco 0:66e9a0afcbd6 57 if(frontButtonPressed())
bulmecisco 0:66e9a0afcbd6 58 error();
bulmecisco 0:66e9a0afcbd6 59 #endif
bulmecisco 0:66e9a0afcbd6 60 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 0:66e9a0afcbd6 61 }
bulmecisco 2:1cd559ff516b 62 #endif
bulmecisco 0:66e9a0afcbd6 63 MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off
bulmecisco 0:66e9a0afcbd6 64 MotorR_EN=MotorL_EN=0;
bulmecisco 12:cedc088eaf05 65 if(move == MOVE)
bulmecisco 12:cedc088eaf05 66 wait_ms(250);
bulmecisco 0:66e9a0afcbd6 67 }
bulmecisco 0:66e9a0afcbd6 68
bulmecisco 0:66e9a0afcbd6 69 void ur_Bertl::PutBeeper()
bulmecisco 0:66e9a0afcbd6 70 {
bulmecisco 0:66e9a0afcbd6 71 // wait_ms(500);
bulmecisco 0:66e9a0afcbd6 72 if(beepersInBag > 0)
bulmecisco 0:66e9a0afcbd6 73 beepersInBag--;
bulmecisco 0:66e9a0afcbd6 74 else
bulmecisco 0:66e9a0afcbd6 75 error();
bulmecisco 0:66e9a0afcbd6 76 }
bulmecisco 0:66e9a0afcbd6 77
bulmecisco 0:66e9a0afcbd6 78 void ur_Bertl::PickBeeper()
bulmecisco 0:66e9a0afcbd6 79 {
bulmecisco 0:66e9a0afcbd6 80 // wait_ms(500);
bulmecisco 0:66e9a0afcbd6 81 if (linesensor)
bulmecisco 0:66e9a0afcbd6 82 beepersInBag++;
bulmecisco 0:66e9a0afcbd6 83 else
bulmecisco 0:66e9a0afcbd6 84 error();
bulmecisco 4:76acfddc26fb 85 if(beepersInBag > 16)
bulmecisco 4:76acfddc26fb 86 error();
bulmecisco 0:66e9a0afcbd6 87 }
bulmecisco 0:66e9a0afcbd6 88
bulmecisco 0:66e9a0afcbd6 89 void ur_Bertl::TurnLeft()
bulmecisco 0:66e9a0afcbd6 90 {
bulmecisco 0:66e9a0afcbd6 91 int count = _count;
bulmecisco 0:66e9a0afcbd6 92 MotorR_EN=MotorL_EN=1; // motor left and right ENABLE
bulmecisco 0:66e9a0afcbd6 93
bulmecisco 0:66e9a0afcbd6 94 MotorR_FORWARD = MotorL_REVERSE = 1;
bulmecisco 2:1cd559ff516b 95 #ifdef TIME
bulmecisco 2:1cd559ff516b 96 wait_ms(TURN);
bulmecisco 2:1cd559ff516b 97 #else
bulmecisco 2:1cd559ff516b 98
bulmecisco 0:66e9a0afcbd6 99 while(_count < count+ANGLE) {
bulmecisco 0:66e9a0afcbd6 100 #ifdef FRONTBUTTON
bulmecisco 0:66e9a0afcbd6 101 if(frontButtonPressed()) // get out if to much problems
bulmecisco 0:66e9a0afcbd6 102 error();
bulmecisco 0:66e9a0afcbd6 103 #endif
bulmecisco 0:66e9a0afcbd6 104 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 0:66e9a0afcbd6 105 }
bulmecisco 2:1cd559ff516b 106 #endif
bulmecisco 0:66e9a0afcbd6 107 MotorR_FORWARD = MotorL_REVERSE = 0;
bulmecisco 0:66e9a0afcbd6 108 MotorR_EN=MotorL_EN=0;
bulmecisco 0:66e9a0afcbd6 109 wait_ms(250); // only to step the robot
bulmecisco 0:66e9a0afcbd6 110 }
bulmecisco 0:66e9a0afcbd6 111
bulmecisco 0:66e9a0afcbd6 112 void ur_Bertl::ShutOff()
bulmecisco 0:66e9a0afcbd6 113 {
bulmecisco 0:66e9a0afcbd6 114 MotorR_FORWARD = MotorL_FORWARD = 0; // motor OFF
bulmecisco 0:66e9a0afcbd6 115 MotorR_EN=MotorL_EN=0; // motor disable
bulmecisco 0:66e9a0afcbd6 116 }
bulmecisco 0:66e9a0afcbd6 117
bulmecisco 4:76acfddc26fb 118 // Public LEDs methodes
bulmecisco 0:66e9a0afcbd6 119
bulmecisco 0:66e9a0afcbd6 120 void ur_Bertl::BlueLedsON()
bulmecisco 0:66e9a0afcbd6 121 {
bulmecisco 0:66e9a0afcbd6 122 LED_blue=0;
bulmecisco 0:66e9a0afcbd6 123 }
bulmecisco 0:66e9a0afcbd6 124
bulmecisco 0:66e9a0afcbd6 125 void ur_Bertl::BlueLedsOFF()
bulmecisco 0:66e9a0afcbd6 126 {
bulmecisco 0:66e9a0afcbd6 127 LED_blue=1;
bulmecisco 0:66e9a0afcbd6 128 }
bulmecisco 0:66e9a0afcbd6 129
bulmecisco 0:66e9a0afcbd6 130 void ur_Bertl::RGBLed(bool red, bool green, bool blue)
bulmecisco 0:66e9a0afcbd6 131 {
bulmecisco 0:66e9a0afcbd6 132 RGB_blue=!blue;
bulmecisco 0:66e9a0afcbd6 133 RGB_red=!red;
bulmecisco 0:66e9a0afcbd6 134 RGB_green=!green;
bulmecisco 0:66e9a0afcbd6 135 }
bulmecisco 0:66e9a0afcbd6 136
bulmecisco 0:66e9a0afcbd6 137 void ur_Bertl::TurnLedOn(int16_t led)
bulmecisco 0:66e9a0afcbd6 138 {
bulmecisco 0:66e9a0afcbd6 139 char cmd[3];
bulmecisco 0:66e9a0afcbd6 140
bulmecisco 1:fafbac0ba96d 141 if(led == 0xBB) {
bulmecisco 1:fafbac0ba96d 142 LED_blue=0;
bulmecisco 1:fafbac0ba96d 143 return;
bulmecisco 1:fafbac0ba96d 144 } else if (led == 0xFF) {
bulmecisco 1:fafbac0ba96d 145 RGBLed(1,1,1);
bulmecisco 1:fafbac0ba96d 146 LED_blue=0;
bulmecisco 1:fafbac0ba96d 147 }
bulmecisco 0:66e9a0afcbd6 148 cmd[0] = 0x02;
bulmecisco 0:66e9a0afcbd6 149 cmd[1] = ~led;
bulmecisco 0:66e9a0afcbd6 150 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 151 wait(0.5);
bulmecisco 0:66e9a0afcbd6 152 }
bulmecisco 0:66e9a0afcbd6 153
bulmecisco 0:66e9a0afcbd6 154 void ur_Bertl::TurnLedOff(int16_t led)
bulmecisco 0:66e9a0afcbd6 155 {
bulmecisco 0:66e9a0afcbd6 156 char cmd[3];
bulmecisco 1:fafbac0ba96d 157 if(led == 0xBB) {
bulmecisco 1:fafbac0ba96d 158 LED_blue=1;
bulmecisco 1:fafbac0ba96d 159 return;
bulmecisco 1:fafbac0ba96d 160 } else if (led == 0xFF) {
bulmecisco 1:fafbac0ba96d 161 RGBLed(0,0,0); //don't work?
bulmecisco 1:fafbac0ba96d 162 LED_blue=1;
bulmecisco 1:fafbac0ba96d 163 }
bulmecisco 0:66e9a0afcbd6 164 cmd[0] = 0x02;
bulmecisco 0:66e9a0afcbd6 165 cmd[1] = led;
bulmecisco 0:66e9a0afcbd6 166 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 167 wait(0.5);
bulmecisco 0:66e9a0afcbd6 168 }
bulmecisco 0:66e9a0afcbd6 169
bulmecisco 0:66e9a0afcbd6 170 void ur_Bertl::NibbleLeds(int value)
bulmecisco 0:66e9a0afcbd6 171 {
bulmecisco 0:66e9a0afcbd6 172 NibbleLEDs = value%16;
bulmecisco 0:66e9a0afcbd6 173 }
bulmecisco 0:66e9a0afcbd6 174
bulmecisco 0:66e9a0afcbd6 175 //----------------------------------------------------------------------
bulmecisco 1:fafbac0ba96d 176
bulmecisco 0:66e9a0afcbd6 177 bool ur_Bertl::FrontIsClear()
bulmecisco 0:66e9a0afcbd6 178 {
bulmecisco 1:fafbac0ba96d 179 #ifdef HCSR
bulmecisco 0:66e9a0afcbd6 180 int dist = 0;
bulmecisco 0:66e9a0afcbd6 181 usensor.start();
bulmecisco 0:66e9a0afcbd6 182 wait_ms(10);
bulmecisco 0:66e9a0afcbd6 183 dist=usensor.get_dist_cm();
bulmecisco 6:df6830254e8b 184 if(dist < ULTRASONIC_DISTANCE)
bulmecisco 0:66e9a0afcbd6 185 return false;
bulmecisco 0:66e9a0afcbd6 186 else
bulmecisco 0:66e9a0afcbd6 187 return true;
bulmecisco 1:fafbac0ba96d 188 #else
bulmecisco 0:66e9a0afcbd6 189 // if there is no ultra sonic sensor use this - with front buttons
bulmecisco 0:66e9a0afcbd6 190 char cmd[3]; // array for I2C
bulmecisco 0:66e9a0afcbd6 191 int16_t btns;
bulmecisco 0:66e9a0afcbd6 192 bool wert;
bulmecisco 0:66e9a0afcbd6 193
bulmecisco 0:66e9a0afcbd6 194 cmd[0] = 0x06;
bulmecisco 0:66e9a0afcbd6 195 cmd[1] = 0x00;
bulmecisco 0:66e9a0afcbd6 196 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 197
bulmecisco 0:66e9a0afcbd6 198 cmd[0]=0x01;
bulmecisco 0:66e9a0afcbd6 199 i2c.write(addr, cmd, 1);
bulmecisco 0:66e9a0afcbd6 200 i2c.read(addr|1, cmd, 1);
bulmecisco 0:66e9a0afcbd6 201 btns = cmd[0];
bulmecisco 0:66e9a0afcbd6 202 if( btns & (BTN_FL|BTN_FM|BTN_FR))
bulmecisco 0:66e9a0afcbd6 203 wert = false;
bulmecisco 0:66e9a0afcbd6 204 else
bulmecisco 0:66e9a0afcbd6 205 wert = true;
bulmecisco 0:66e9a0afcbd6 206 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 0:66e9a0afcbd6 207 return wert;
bulmecisco 1:fafbac0ba96d 208 #endif
bulmecisco 0:66e9a0afcbd6 209 }
bulmecisco 0:66e9a0afcbd6 210
bulmecisco 4:76acfddc26fb 211 bool ur_Bertl::WaitUntilButtonPressed()
bulmecisco 4:76acfddc26fb 212 {
bulmecisco 4:76acfddc26fb 213 char cmd[3];
bulmecisco 4:76acfddc26fb 214 int16_t btns;
bulmecisco 4:76acfddc26fb 215 bool wert;
bulmecisco 4:76acfddc26fb 216
bulmecisco 4:76acfddc26fb 217 RGB_blue=RGB_red=RGB_green=0;
bulmecisco 4:76acfddc26fb 218 cmd[0] = 0x06;
bulmecisco 4:76acfddc26fb 219 cmd[1] = 0x00;
bulmecisco 4:76acfddc26fb 220 i2c.write(addr, cmd, 2);
bulmecisco 4:76acfddc26fb 221
bulmecisco 4:76acfddc26fb 222 cmd[0]=0x01;
bulmecisco 4:76acfddc26fb 223 i2c.write(addr, cmd, 1);
bulmecisco 4:76acfddc26fb 224 i2c.read(addr|1, cmd, 1);
bulmecisco 4:76acfddc26fb 225 btns = cmd[0];
bulmecisco 4:76acfddc26fb 226 if( btns & (0xFF))
bulmecisco 4:76acfddc26fb 227 wert = false;
bulmecisco 4:76acfddc26fb 228 else
bulmecisco 4:76acfddc26fb 229 wert = true;
bulmecisco 4:76acfddc26fb 230 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
bulmecisco 4:76acfddc26fb 231 return wert;
bulmecisco 4:76acfddc26fb 232 }
bulmecisco 4:76acfddc26fb 233
bulmecisco 0:66e9a0afcbd6 234 bool ur_Bertl::NextToABeeper()
bulmecisco 0:66e9a0afcbd6 235 {
bulmecisco 4:76acfddc26fb 236 if (bottomIsBlack())
bulmecisco 0:66e9a0afcbd6 237 return true;
bulmecisco 0:66e9a0afcbd6 238 else
bulmecisco 0:66e9a0afcbd6 239 return false;
bulmecisco 0:66e9a0afcbd6 240 }
bulmecisco 0:66e9a0afcbd6 241
bulmecisco 0:66e9a0afcbd6 242 int ur_Bertl::AnyBeeperInBag()
bulmecisco 0:66e9a0afcbd6 243 {
bulmecisco 0:66e9a0afcbd6 244 if(beepersInBag > 0)
bulmecisco 0:66e9a0afcbd6 245 return beepersInBag;
bulmecisco 0:66e9a0afcbd6 246 else
bulmecisco 0:66e9a0afcbd6 247 return 0;
bulmecisco 0:66e9a0afcbd6 248 }
bulmecisco 0:66e9a0afcbd6 249
bulmecisco 5:6b667e2cb800 250 bool ur_Bertl::IsButtonPressed(const int btn)
bulmecisco 5:6b667e2cb800 251 {
bulmecisco 5:6b667e2cb800 252 char cmd[3]; // array for I2C
bulmecisco 5:6b667e2cb800 253 int16_t btns;
bulmecisco 5:6b667e2cb800 254 bool wert;
bulmecisco 5:6b667e2cb800 255
bulmecisco 5:6b667e2cb800 256 cmd[0] = 0x06;
bulmecisco 5:6b667e2cb800 257 cmd[1] = 0x00;
bulmecisco 5:6b667e2cb800 258 i2c.write(addr, cmd, 2);
bulmecisco 5:6b667e2cb800 259
bulmecisco 5:6b667e2cb800 260 cmd[0]=0x01;
bulmecisco 5:6b667e2cb800 261 i2c.write(addr, cmd, 1);
bulmecisco 5:6b667e2cb800 262 i2c.read(addr|1, cmd, 1);
bulmecisco 5:6b667e2cb800 263 btns = cmd[0];
bulmecisco 5:6b667e2cb800 264 if( btns & btn)
bulmecisco 5:6b667e2cb800 265 wert = true;
bulmecisco 5:6b667e2cb800 266 else
bulmecisco 5:6b667e2cb800 267 wert = false;
bulmecisco 5:6b667e2cb800 268 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 5:6b667e2cb800 269 return wert;
bulmecisco 5:6b667e2cb800 270 }
bulmecisco 5:6b667e2cb800 271
bulmecisco 7:e7f74f072564 272 int ur_Bertl::ReturnButtonPressed()
bulmecisco 7:e7f74f072564 273 {
bulmecisco 7:e7f74f072564 274 char cmd[3]; // array for I2C
bulmecisco 7:e7f74f072564 275 int16_t btns;
bulmecisco 7:e7f74f072564 276
bulmecisco 7:e7f74f072564 277 cmd[0] = 0x06;
bulmecisco 7:e7f74f072564 278 cmd[1] = 0x00;
bulmecisco 7:e7f74f072564 279 i2c.write(addr, cmd, 2);
bulmecisco 7:e7f74f072564 280
bulmecisco 7:e7f74f072564 281 cmd[0]=0x01;
bulmecisco 7:e7f74f072564 282 i2c.write(addr, cmd, 1);
bulmecisco 7:e7f74f072564 283 i2c.read(addr|1, cmd, 1);
bulmecisco 7:e7f74f072564 284 btns = cmd[0];
bulmecisco 7:e7f74f072564 285 DEBUG_PRINT("Button: %d", btns);
bulmecisco 7:e7f74f072564 286 return btns;
bulmecisco 7:e7f74f072564 287 }
bulmecisco 4:76acfddc26fb 288 // Protected methodes
bulmecisco 4:76acfddc26fb 289
bulmecisco 4:76acfddc26fb 290 int ur_Bertl::bottomIsBlack()
bulmecisco 4:76acfddc26fb 291 {
bulmecisco 4:76acfddc26fb 292 int detect;
bulmecisco 4:76acfddc26fb 293
bulmecisco 4:76acfddc26fb 294 detect = linesensor;
bulmecisco 4:76acfddc26fb 295 return detect;
bulmecisco 4:76acfddc26fb 296 }
bulmecisco 4:76acfddc26fb 297
bulmecisco 0:66e9a0afcbd6 298 bool ur_Bertl::backIsClear()
bulmecisco 0:66e9a0afcbd6 299 {
bulmecisco 0:66e9a0afcbd6 300 char cmd[3]; // array for I2C
bulmecisco 0:66e9a0afcbd6 301 int16_t btns;
bulmecisco 0:66e9a0afcbd6 302 bool wert;
bulmecisco 0:66e9a0afcbd6 303
bulmecisco 0:66e9a0afcbd6 304 cmd[0] = 0x06;
bulmecisco 0:66e9a0afcbd6 305 cmd[1] = 0x00;
bulmecisco 0:66e9a0afcbd6 306 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 307
bulmecisco 0:66e9a0afcbd6 308 cmd[0]=0x01;
bulmecisco 0:66e9a0afcbd6 309 i2c.write(addr, cmd, 1);
bulmecisco 0:66e9a0afcbd6 310 i2c.read(addr|1, cmd, 1);
bulmecisco 0:66e9a0afcbd6 311 btns = cmd[0];
bulmecisco 0:66e9a0afcbd6 312 if( btns & (BTN_BL|BTN_BM|BTN_BR))
bulmecisco 0:66e9a0afcbd6 313 wert = false;
bulmecisco 0:66e9a0afcbd6 314 else
bulmecisco 0:66e9a0afcbd6 315 wert = true;
bulmecisco 0:66e9a0afcbd6 316 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 0:66e9a0afcbd6 317 return wert;
bulmecisco 0:66e9a0afcbd6 318 }
bulmecisco 0:66e9a0afcbd6 319
bulmecisco 4:76acfddc26fb 320 bool ur_Bertl::frontButtonPressed()
bulmecisco 0:66e9a0afcbd6 321 {
bulmecisco 4:76acfddc26fb 322 char cmd[3]; // array for I2C
bulmecisco 4:76acfddc26fb 323 int16_t btns;
bulmecisco 4:76acfddc26fb 324 bool wert;
bulmecisco 4:76acfddc26fb 325
bulmecisco 4:76acfddc26fb 326 cmd[0] = 0x06;
bulmecisco 4:76acfddc26fb 327 cmd[1] = 0x00;
bulmecisco 4:76acfddc26fb 328 i2c.write(addr, cmd, 2);
bulmecisco 4:76acfddc26fb 329
bulmecisco 4:76acfddc26fb 330 cmd[0]=0x01;
bulmecisco 4:76acfddc26fb 331 i2c.write(addr, cmd, 1);
bulmecisco 4:76acfddc26fb 332 i2c.read(addr|1, cmd, 1);
bulmecisco 4:76acfddc26fb 333 btns = cmd[0];
bulmecisco 4:76acfddc26fb 334 if( btns & (BTN_FL|BTN_FM|BTN_FR|BTN_FRR|BTN_FLL))
bulmecisco 4:76acfddc26fb 335 wert = true;
bulmecisco 4:76acfddc26fb 336 else
bulmecisco 4:76acfddc26fb 337 wert = false;
bulmecisco 4:76acfddc26fb 338 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 4:76acfddc26fb 339 return wert;
bulmecisco 0:66e9a0afcbd6 340 }
bulmecisco 0:66e9a0afcbd6 341
bulmecisco 5:6b667e2cb800 342
bulmecisco 5:6b667e2cb800 343
bulmecisco 4:76acfddc26fb 344 //-----------------INTERNAL USE ONLY ----------------------------
bulmecisco 4:76acfddc26fb 345 void ur_Bertl::error()
bulmecisco 0:66e9a0afcbd6 346 {
bulmecisco 4:76acfddc26fb 347 int wait = 500;
bulmecisco 4:76acfddc26fb 348 MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off
bulmecisco 0:66e9a0afcbd6 349 MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off
bulmecisco 0:66e9a0afcbd6 350 MotorR_EN=MotorL_EN=0;
bulmecisco 4:76acfddc26fb 351 while(1) {
bulmecisco 4:76acfddc26fb 352 TurnLedOff(0xFF);
bulmecisco 4:76acfddc26fb 353 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0;
bulmecisco 4:76acfddc26fb 354 LED_blue=1;
bulmecisco 4:76acfddc26fb 355 RGB_blue=RGB_green=RGB_red=1;
bulmecisco 4:76acfddc26fb 356 wait_ms(wait);
bulmecisco 4:76acfddc26fb 357 TurnLedOn(0xFF);
bulmecisco 4:76acfddc26fb 358 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
bulmecisco 4:76acfddc26fb 359 LED_blue=0;
bulmecisco 4:76acfddc26fb 360 RGB_blue=RGB_green=1;RGB_red=0;
bulmecisco 4:76acfddc26fb 361 wait_ms(wait);
bulmecisco 4:76acfddc26fb 362 }
bulmecisco 0:66e9a0afcbd6 363 }
bulmecisco 4:76acfddc26fb 364
bulmecisco 4:76acfddc26fb 365 // ISR
bulmecisco 4:76acfddc26fb 366
bulmecisco 4:76acfddc26fb 367 void ur_Bertl::increment()
bulmecisco 4:76acfddc26fb 368 {
bulmecisco 4:76acfddc26fb 369 _count++;
bulmecisco 4:76acfddc26fb 370 }
bulmecisco 4:76acfddc26fb 371
bulmecisco 4:76acfddc26fb 372 int ur_Bertl::Read()
bulmecisco 4:76acfddc26fb 373 {
bulmecisco 4:76acfddc26fb 374 return _count;
bulmecisco 4:76acfddc26fb 375 }
bulmecisco 4:76acfddc26fb 376
bulmecisco 12:cedc088eaf05 377 // -------------------- BERTL CLASS -------------------------------------
bulmecisco 13:3ce84646fd74 378 int Bertl::MoveBackwards(int move)
bulmecisco 12:cedc088eaf05 379 {
bulmecisco 12:cedc088eaf05 380 int count = _count;
bulmecisco 12:cedc088eaf05 381 //wait_ms(250); // waite until Bertl stops
bulmecisco 12:cedc088eaf05 382 MotorR_EN=MotorL_EN=1; // both motor ENABLE
bulmecisco 12:cedc088eaf05 383 MotorR_REVERSE = MotorL_REVERSE = 1; // both motor backwards ON
bulmecisco 12:cedc088eaf05 384 #ifndef TIME
bulmecisco 12:cedc088eaf05 385 while(_count < count+move) {
bulmecisco 12:cedc088eaf05 386 if(!backIsClear())
bulmecisco 12:cedc088eaf05 387 break;
bulmecisco 12:cedc088eaf05 388 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 12:cedc088eaf05 389 }
bulmecisco 12:cedc088eaf05 390 #else
bulmecisco 12:cedc088eaf05 391 wait_ms(move);
bulmecisco 12:cedc088eaf05 392 #endif
bulmecisco 12:cedc088eaf05 393 MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off
bulmecisco 12:cedc088eaf05 394 MotorR_EN=MotorL_EN=0;
bulmecisco 12:cedc088eaf05 395 if(move == MOVE)
bulmecisco 12:cedc088eaf05 396 wait_ms(250);
bulmecisco 13:3ce84646fd74 397 return _count - count;
bulmecisco 12:cedc088eaf05 398 }
bulmecisco 12:cedc088eaf05 399 // ------------------------- BERT CLASS --------------------------------------
bulmecisco 13:3ce84646fd74 400 int Bertl::Move(int move)
bulmecisco 13:3ce84646fd74 401 {
bulmecisco 13:3ce84646fd74 402 int count = _count;
bulmecisco 13:3ce84646fd74 403 MotorR_EN=MotorL_EN=1; // both motor ENABLE
bulmecisco 13:3ce84646fd74 404 MotorR_FORWARD = MotorL_FORWARD = 1; // both motor forward ON
bulmecisco 13:3ce84646fd74 405 #ifdef TIME
bulmecisco 13:3ce84646fd74 406 wait_ms(move);
bulmecisco 13:3ce84646fd74 407 #else
bulmecisco 13:3ce84646fd74 408
bulmecisco 13:3ce84646fd74 409 while(_count < count+move) {
bulmecisco 13:3ce84646fd74 410 //if(!FrontIsClear()) // more convenient because there are no accidents :-)
bulmecisco 13:3ce84646fd74 411 // break;
bulmecisco 13:3ce84646fd74 412 #ifdef FRONTBUTTON
bulmecisco 13:3ce84646fd74 413 if(frontButtonPressed())
bulmecisco 13:3ce84646fd74 414 error();
bulmecisco 13:3ce84646fd74 415 #endif
bulmecisco 13:3ce84646fd74 416 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 13:3ce84646fd74 417 }
bulmecisco 13:3ce84646fd74 418 #endif
bulmecisco 13:3ce84646fd74 419 MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off
bulmecisco 13:3ce84646fd74 420 MotorR_EN=MotorL_EN=0;
bulmecisco 13:3ce84646fd74 421 if(move == MOVE)
bulmecisco 13:3ce84646fd74 422 wait_ms(250);
bulmecisco 13:3ce84646fd74 423 return _count - count;
bulmecisco 13:3ce84646fd74 424 }
bulmecisco 13:3ce84646fd74 425
bulmecisco 12:cedc088eaf05 426 void Bertl::TurnRigth()
bulmecisco 12:cedc088eaf05 427 {
bulmecisco 12:cedc088eaf05 428 int count = _count;
bulmecisco 12:cedc088eaf05 429 MotorR_EN=MotorL_EN=1; // motor left and right ENABLE
bulmecisco 12:cedc088eaf05 430
bulmecisco 12:cedc088eaf05 431 MotorR_FORWARD = MotorL_REVERSE = 0;
bulmecisco 12:cedc088eaf05 432 MotorL_FORWARD = MotorR_REVERSE = 1;
bulmecisco 12:cedc088eaf05 433 #ifdef TIME
bulmecisco 12:cedc088eaf05 434 wait_ms(TURN);
bulmecisco 12:cedc088eaf05 435 #else
bulmecisco 12:cedc088eaf05 436
bulmecisco 12:cedc088eaf05 437 while(_count < count+ANGLE) {
bulmecisco 12:cedc088eaf05 438 #ifdef FRONTBUTTON
bulmecisco 12:cedc088eaf05 439 if(frontButtonPressed()) // get out if to much problems
bulmecisco 12:cedc088eaf05 440 error();
bulmecisco 12:cedc088eaf05 441 #endif
bulmecisco 12:cedc088eaf05 442 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 12:cedc088eaf05 443 }
bulmecisco 12:cedc088eaf05 444 #endif
bulmecisco 12:cedc088eaf05 445 MotorL_FORWARD = MotorR_REVERSE = 0;
bulmecisco 12:cedc088eaf05 446 MotorR_FORWARD = MotorL_REVERSE = 0;
bulmecisco 12:cedc088eaf05 447 MotorR_EN=MotorL_EN=0;
bulmecisco 12:cedc088eaf05 448 wait_ms(250); // only to step the robot
bulmecisco 12:cedc088eaf05 449 }
bulmecisco 12:cedc088eaf05 450 // ------------------------- to adjust turns ---------------------------------
bulmecisco 12:cedc088eaf05 451 void Bertl::TurnRigthStep(int step)
bulmecisco 12:cedc088eaf05 452 {
bulmecisco 12:cedc088eaf05 453 int count = _count;
bulmecisco 12:cedc088eaf05 454 MotorR_EN=MotorL_EN=1; // motor left and right ENABLE
bulmecisco 12:cedc088eaf05 455
bulmecisco 12:cedc088eaf05 456 MotorR_FORWARD = MotorL_REVERSE = 0;
bulmecisco 12:cedc088eaf05 457 MotorL_FORWARD = MotorR_REVERSE = 1;
bulmecisco 12:cedc088eaf05 458 wait_ms(step);
bulmecisco 12:cedc088eaf05 459 #ifdef TIME
bulmecisco 12:cedc088eaf05 460 #else
bulmecisco 12:cedc088eaf05 461
bulmecisco 12:cedc088eaf05 462 while(_count < count+1) {
bulmecisco 12:cedc088eaf05 463 #ifdef FRONTBUTTON
bulmecisco 12:cedc088eaf05 464 if(frontButtonPressed()) // get out if to much problems
bulmecisco 12:cedc088eaf05 465 error();
bulmecisco 12:cedc088eaf05 466 #endif
bulmecisco 12:cedc088eaf05 467 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 12:cedc088eaf05 468 }
bulmecisco 12:cedc088eaf05 469 #endif
bulmecisco 12:cedc088eaf05 470 MotorL_FORWARD = MotorR_REVERSE = 0;
bulmecisco 12:cedc088eaf05 471 MotorR_FORWARD = MotorL_REVERSE = 0;
bulmecisco 12:cedc088eaf05 472 MotorR_EN=MotorL_EN=0;
bulmecisco 12:cedc088eaf05 473 // wait_ms(250); // only to step the robot
bulmecisco 12:cedc088eaf05 474 }
bulmecisco 12:cedc088eaf05 475
bulmecisco 12:cedc088eaf05 476 void Bertl::TurnLeftStep(int step)
bulmecisco 12:cedc088eaf05 477 {
bulmecisco 12:cedc088eaf05 478 int count = _count;
bulmecisco 12:cedc088eaf05 479 MotorR_EN=MotorL_EN=1; // motor left and right ENABLE
bulmecisco 12:cedc088eaf05 480
bulmecisco 12:cedc088eaf05 481 MotorR_FORWARD = MotorL_REVERSE = 1;
bulmecisco 12:cedc088eaf05 482 wait_ms(step);
bulmecisco 12:cedc088eaf05 483
bulmecisco 12:cedc088eaf05 484 #ifdef TIME
bulmecisco 12:cedc088eaf05 485 #else
bulmecisco 12:cedc088eaf05 486
bulmecisco 12:cedc088eaf05 487 while(_count < count+1) {
bulmecisco 12:cedc088eaf05 488 #ifdef FRONTBUTTON
bulmecisco 12:cedc088eaf05 489 if(frontButtonPressed()) // get out if to much problems
bulmecisco 12:cedc088eaf05 490 error();
bulmecisco 12:cedc088eaf05 491 #endif
bulmecisco 12:cedc088eaf05 492 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 12:cedc088eaf05 493 }
bulmecisco 12:cedc088eaf05 494 #endif
bulmecisco 12:cedc088eaf05 495 MotorR_FORWARD = MotorL_REVERSE = 0;
bulmecisco 12:cedc088eaf05 496 MotorR_EN=MotorL_EN=0;
bulmecisco 12:cedc088eaf05 497 //wait_ms(250); // only to step the robot
bulmecisco 12:cedc088eaf05 498 }
bulmecisco 12:cedc088eaf05 499
bulmecisco 12:cedc088eaf05 500 uint8_t Bertl::GetLineValues()
bulmecisco 12:cedc088eaf05 501 {
bulmecisco 12:cedc088eaf05 502 uint8_t detect;
bulmecisco 12:cedc088eaf05 503
bulmecisco 12:cedc088eaf05 504 detect = linesensor;
bulmecisco 12:cedc088eaf05 505 return detect;
bulmecisco 13:3ce84646fd74 506 }
bulmecisco 13:3ce84646fd74 507
bulmecisco 13:3ce84646fd74 508 void Bertl::RGBLed(bool red, bool green, bool blue)
bulmecisco 13:3ce84646fd74 509 {
bulmecisco 13:3ce84646fd74 510 RGB_blue=!blue;
bulmecisco 13:3ce84646fd74 511 RGB_red=!red;
bulmecisco 13:3ce84646fd74 512 RGB_green=!green;
bulmecisco 13:3ce84646fd74 513 }