Dr. Pucher / ur_Bertl

Class Bertl added

Dependencies:   HCSR

Dependents:   BertlDrive_V2 BertlDrive_V2

ur_Bertl.cpp@12:cedc088eaf05, 2015-04-26 (annotated)

Committer:
bulmecisco
Date:
Sun Apr 26 20:04:47 2015 +0000
Revision:
12:cedc088eaf05
Parent:
7:e7f74f072564 
Class Bertl with additional Methodes added

Who changed what in which revision?

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