Dr. Pucher / ur_Bertl

Class Bertl added

Dependencies:   HCSR

Dependents:   BertlDrive_V2 BertlDrive_V2

ur_Bertl.cpp@7:e7f74f072564, 2015-04-13 (annotated)

Committer:
bulmecisco
Date:
Mon Apr 13 15:28:10 2015 +0000
Revision:
7:e7f74f072564
Parent:
6:df6830254e8b
Child:
12:cedc088eaf05
int ReturnButtonPressed() added which returns the int value of button pressed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bulmecisco 0:66e9a0afcbd6 1 /***********************************
bulmecisco 7:e7f74f072564 2 name: ur_Bertl.cpp Version: 2.1
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 0:66e9a0afcbd6 43 void ur_Bertl::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 2:1cd559ff516b 49 wait_ms(MOVE);
bulmecisco 3:01b183fe8b41 50 #else
bulmecisco 0:66e9a0afcbd6 51
bulmecisco 0:66e9a0afcbd6 52 while(_count < count+DISTANCE) {
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 0:66e9a0afcbd6 64 wait_ms(250);
bulmecisco 0:66e9a0afcbd6 65 }
bulmecisco 0:66e9a0afcbd6 66
bulmecisco 0:66e9a0afcbd6 67 void ur_Bertl::PutBeeper()
bulmecisco 0:66e9a0afcbd6 68 {
bulmecisco 0:66e9a0afcbd6 69 // wait_ms(500);
bulmecisco 0:66e9a0afcbd6 70 if(beepersInBag > 0)
bulmecisco 0:66e9a0afcbd6 71 beepersInBag--;
bulmecisco 0:66e9a0afcbd6 72 else
bulmecisco 0:66e9a0afcbd6 73 error();
bulmecisco 0:66e9a0afcbd6 74 }
bulmecisco 0:66e9a0afcbd6 75
bulmecisco 0:66e9a0afcbd6 76 void ur_Bertl::PickBeeper()
bulmecisco 0:66e9a0afcbd6 77 {
bulmecisco 0:66e9a0afcbd6 78 // wait_ms(500);
bulmecisco 0:66e9a0afcbd6 79 if (linesensor)
bulmecisco 0:66e9a0afcbd6 80 beepersInBag++;
bulmecisco 0:66e9a0afcbd6 81 else
bulmecisco 0:66e9a0afcbd6 82 error();
bulmecisco 4:76acfddc26fb 83 if(beepersInBag > 16)
bulmecisco 4:76acfddc26fb 84 error();
bulmecisco 0:66e9a0afcbd6 85 }
bulmecisco 0:66e9a0afcbd6 86
bulmecisco 0:66e9a0afcbd6 87 void ur_Bertl::TurnLeft()
bulmecisco 0:66e9a0afcbd6 88 {
bulmecisco 0:66e9a0afcbd6 89 int count = _count;
bulmecisco 0:66e9a0afcbd6 90 MotorR_EN=MotorL_EN=1; // motor left and right ENABLE
bulmecisco 0:66e9a0afcbd6 91
bulmecisco 0:66e9a0afcbd6 92 MotorR_FORWARD = MotorL_REVERSE = 1;
bulmecisco 2:1cd559ff516b 93 #ifdef TIME
bulmecisco 2:1cd559ff516b 94 wait_ms(TURN);
bulmecisco 2:1cd559ff516b 95 #else
bulmecisco 2:1cd559ff516b 96
bulmecisco 0:66e9a0afcbd6 97 while(_count < count+ANGLE) {
bulmecisco 0:66e9a0afcbd6 98 #ifdef FRONTBUTTON
bulmecisco 0:66e9a0afcbd6 99 if(frontButtonPressed()) // get out if to much problems
bulmecisco 0:66e9a0afcbd6 100 error();
bulmecisco 0:66e9a0afcbd6 101 #endif
bulmecisco 0:66e9a0afcbd6 102 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 0:66e9a0afcbd6 103 }
bulmecisco 2:1cd559ff516b 104 #endif
bulmecisco 0:66e9a0afcbd6 105 MotorR_FORWARD = MotorL_REVERSE = 0;
bulmecisco 0:66e9a0afcbd6 106 MotorR_EN=MotorL_EN=0;
bulmecisco 0:66e9a0afcbd6 107 wait_ms(250); // only to step the robot
bulmecisco 0:66e9a0afcbd6 108 }
bulmecisco 0:66e9a0afcbd6 109
bulmecisco 0:66e9a0afcbd6 110 void ur_Bertl::ShutOff()
bulmecisco 0:66e9a0afcbd6 111 {
bulmecisco 0:66e9a0afcbd6 112 MotorR_FORWARD = MotorL_FORWARD = 0; // motor OFF
bulmecisco 0:66e9a0afcbd6 113 MotorR_EN=MotorL_EN=0; // motor disable
bulmecisco 0:66e9a0afcbd6 114 }
bulmecisco 0:66e9a0afcbd6 115
bulmecisco 4:76acfddc26fb 116 // Public LEDs methodes
bulmecisco 0:66e9a0afcbd6 117
bulmecisco 0:66e9a0afcbd6 118 void ur_Bertl::BlueLedsON()
bulmecisco 0:66e9a0afcbd6 119 {
bulmecisco 0:66e9a0afcbd6 120 LED_blue=0;
bulmecisco 0:66e9a0afcbd6 121 }
bulmecisco 0:66e9a0afcbd6 122
bulmecisco 0:66e9a0afcbd6 123 void ur_Bertl::BlueLedsOFF()
bulmecisco 0:66e9a0afcbd6 124 {
bulmecisco 0:66e9a0afcbd6 125 LED_blue=1;
bulmecisco 0:66e9a0afcbd6 126 }
bulmecisco 0:66e9a0afcbd6 127
bulmecisco 0:66e9a0afcbd6 128 void ur_Bertl::RGBLed(bool red, bool green, bool blue)
bulmecisco 0:66e9a0afcbd6 129 {
bulmecisco 0:66e9a0afcbd6 130 RGB_blue=!blue;
bulmecisco 0:66e9a0afcbd6 131 RGB_red=!red;
bulmecisco 0:66e9a0afcbd6 132 RGB_green=!green;
bulmecisco 0:66e9a0afcbd6 133 }
bulmecisco 0:66e9a0afcbd6 134
bulmecisco 0:66e9a0afcbd6 135 void ur_Bertl::TurnLedOn(int16_t led)
bulmecisco 0:66e9a0afcbd6 136 {
bulmecisco 0:66e9a0afcbd6 137 char cmd[3];
bulmecisco 0:66e9a0afcbd6 138
bulmecisco 1:fafbac0ba96d 139 if(led == 0xBB) {
bulmecisco 1:fafbac0ba96d 140 LED_blue=0;
bulmecisco 1:fafbac0ba96d 141 return;
bulmecisco 1:fafbac0ba96d 142 } else if (led == 0xFF) {
bulmecisco 1:fafbac0ba96d 143 RGBLed(1,1,1);
bulmecisco 1:fafbac0ba96d 144 LED_blue=0;
bulmecisco 1:fafbac0ba96d 145 }
bulmecisco 0:66e9a0afcbd6 146 cmd[0] = 0x02;
bulmecisco 0:66e9a0afcbd6 147 cmd[1] = ~led;
bulmecisco 0:66e9a0afcbd6 148 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 149 wait(0.5);
bulmecisco 0:66e9a0afcbd6 150 }
bulmecisco 0:66e9a0afcbd6 151
bulmecisco 0:66e9a0afcbd6 152 void ur_Bertl::TurnLedOff(int16_t led)
bulmecisco 0:66e9a0afcbd6 153 {
bulmecisco 0:66e9a0afcbd6 154 char cmd[3];
bulmecisco 1:fafbac0ba96d 155 if(led == 0xBB) {
bulmecisco 1:fafbac0ba96d 156 LED_blue=1;
bulmecisco 1:fafbac0ba96d 157 return;
bulmecisco 1:fafbac0ba96d 158 } else if (led == 0xFF) {
bulmecisco 1:fafbac0ba96d 159 RGBLed(0,0,0); //don't work?
bulmecisco 1:fafbac0ba96d 160 LED_blue=1;
bulmecisco 1:fafbac0ba96d 161 }
bulmecisco 0:66e9a0afcbd6 162 cmd[0] = 0x02;
bulmecisco 0:66e9a0afcbd6 163 cmd[1] = led;
bulmecisco 0:66e9a0afcbd6 164 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 165 wait(0.5);
bulmecisco 0:66e9a0afcbd6 166 }
bulmecisco 0:66e9a0afcbd6 167
bulmecisco 0:66e9a0afcbd6 168 void ur_Bertl::NibbleLeds(int value)
bulmecisco 0:66e9a0afcbd6 169 {
bulmecisco 0:66e9a0afcbd6 170 NibbleLEDs = value%16;
bulmecisco 0:66e9a0afcbd6 171 }
bulmecisco 0:66e9a0afcbd6 172
bulmecisco 0:66e9a0afcbd6 173 //----------------------------------------------------------------------
bulmecisco 1:fafbac0ba96d 174
bulmecisco 0:66e9a0afcbd6 175 bool ur_Bertl::FrontIsClear()
bulmecisco 0:66e9a0afcbd6 176 {
bulmecisco 1:fafbac0ba96d 177 #ifdef HCSR
bulmecisco 0:66e9a0afcbd6 178 int dist = 0;
bulmecisco 0:66e9a0afcbd6 179 usensor.start();
bulmecisco 0:66e9a0afcbd6 180 wait_ms(10);
bulmecisco 0:66e9a0afcbd6 181 dist=usensor.get_dist_cm();
bulmecisco 6:df6830254e8b 182 if(dist < ULTRASONIC_DISTANCE)
bulmecisco 0:66e9a0afcbd6 183 return false;
bulmecisco 0:66e9a0afcbd6 184 else
bulmecisco 0:66e9a0afcbd6 185 return true;
bulmecisco 1:fafbac0ba96d 186 #else
bulmecisco 0:66e9a0afcbd6 187 // if there is no ultra sonic sensor use this - with front buttons
bulmecisco 0:66e9a0afcbd6 188 char cmd[3]; // array for I2C
bulmecisco 0:66e9a0afcbd6 189 int16_t btns;
bulmecisco 0:66e9a0afcbd6 190 bool wert;
bulmecisco 0:66e9a0afcbd6 191
bulmecisco 0:66e9a0afcbd6 192 cmd[0] = 0x06;
bulmecisco 0:66e9a0afcbd6 193 cmd[1] = 0x00;
bulmecisco 0:66e9a0afcbd6 194 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 195
bulmecisco 0:66e9a0afcbd6 196 cmd[0]=0x01;
bulmecisco 0:66e9a0afcbd6 197 i2c.write(addr, cmd, 1);
bulmecisco 0:66e9a0afcbd6 198 i2c.read(addr|1, cmd, 1);
bulmecisco 0:66e9a0afcbd6 199 btns = cmd[0];
bulmecisco 0:66e9a0afcbd6 200 if( btns & (BTN_FL|BTN_FM|BTN_FR))
bulmecisco 0:66e9a0afcbd6 201 wert = false;
bulmecisco 0:66e9a0afcbd6 202 else
bulmecisco 0:66e9a0afcbd6 203 wert = true;
bulmecisco 0:66e9a0afcbd6 204 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 0:66e9a0afcbd6 205 return wert;
bulmecisco 1:fafbac0ba96d 206 #endif
bulmecisco 0:66e9a0afcbd6 207 }
bulmecisco 0:66e9a0afcbd6 208
bulmecisco 4:76acfddc26fb 209 bool ur_Bertl::WaitUntilButtonPressed()
bulmecisco 4:76acfddc26fb 210 {
bulmecisco 4:76acfddc26fb 211 char cmd[3];
bulmecisco 4:76acfddc26fb 212 int16_t btns;
bulmecisco 4:76acfddc26fb 213 bool wert;
bulmecisco 4:76acfddc26fb 214
bulmecisco 4:76acfddc26fb 215 RGB_blue=RGB_red=RGB_green=0;
bulmecisco 4:76acfddc26fb 216 cmd[0] = 0x06;
bulmecisco 4:76acfddc26fb 217 cmd[1] = 0x00;
bulmecisco 4:76acfddc26fb 218 i2c.write(addr, cmd, 2);
bulmecisco 4:76acfddc26fb 219
bulmecisco 4:76acfddc26fb 220 cmd[0]=0x01;
bulmecisco 4:76acfddc26fb 221 i2c.write(addr, cmd, 1);
bulmecisco 4:76acfddc26fb 222 i2c.read(addr|1, cmd, 1);
bulmecisco 4:76acfddc26fb 223 btns = cmd[0];
bulmecisco 4:76acfddc26fb 224 if( btns & (0xFF))
bulmecisco 4:76acfddc26fb 225 wert = false;
bulmecisco 4:76acfddc26fb 226 else
bulmecisco 4:76acfddc26fb 227 wert = true;
bulmecisco 4:76acfddc26fb 228 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
bulmecisco 4:76acfddc26fb 229 return wert;
bulmecisco 4:76acfddc26fb 230 }
bulmecisco 4:76acfddc26fb 231
bulmecisco 0:66e9a0afcbd6 232 bool ur_Bertl::NextToABeeper()
bulmecisco 0:66e9a0afcbd6 233 {
bulmecisco 4:76acfddc26fb 234 if (bottomIsBlack())
bulmecisco 0:66e9a0afcbd6 235 return true;
bulmecisco 0:66e9a0afcbd6 236 else
bulmecisco 0:66e9a0afcbd6 237 return false;
bulmecisco 0:66e9a0afcbd6 238 }
bulmecisco 0:66e9a0afcbd6 239
bulmecisco 0:66e9a0afcbd6 240 int ur_Bertl::AnyBeeperInBag()
bulmecisco 0:66e9a0afcbd6 241 {
bulmecisco 0:66e9a0afcbd6 242 if(beepersInBag > 0)
bulmecisco 0:66e9a0afcbd6 243 return beepersInBag;
bulmecisco 0:66e9a0afcbd6 244 else
bulmecisco 0:66e9a0afcbd6 245 return 0;
bulmecisco 0:66e9a0afcbd6 246 }
bulmecisco 0:66e9a0afcbd6 247
bulmecisco 4:76acfddc26fb 248 void ur_Bertl::MoveBackwards()
bulmecisco 4:76acfddc26fb 249 {
bulmecisco 4:76acfddc26fb 250 int count = _count;
bulmecisco 4:76acfddc26fb 251 //wait_ms(250); // waite until Bertl stops
bulmecisco 4:76acfddc26fb 252 MotorR_EN=MotorL_EN=1; // both motor ENABLE
bulmecisco 4:76acfddc26fb 253 MotorR_REVERSE = MotorL_REVERSE = 1; // both motor backwards ON
bulmecisco 4:76acfddc26fb 254 while(_count < count+DISTANCE) {
bulmecisco 4:76acfddc26fb 255 if(!backIsClear())
bulmecisco 4:76acfddc26fb 256 break;
bulmecisco 4:76acfddc26fb 257 DEBUG_PRINT("count: %d _count: %d", count, _count);
bulmecisco 4:76acfddc26fb 258 }
bulmecisco 4:76acfddc26fb 259 MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off
bulmecisco 4:76acfddc26fb 260 MotorR_EN=MotorL_EN=0;
bulmecisco 4:76acfddc26fb 261 wait_ms(250);
bulmecisco 4:76acfddc26fb 262 }
bulmecisco 4:76acfddc26fb 263
bulmecisco 5:6b667e2cb800 264 bool ur_Bertl::IsButtonPressed(const int btn)
bulmecisco 5:6b667e2cb800 265 {
bulmecisco 5:6b667e2cb800 266 char cmd[3]; // array for I2C
bulmecisco 5:6b667e2cb800 267 int16_t btns;
bulmecisco 5:6b667e2cb800 268 bool wert;
bulmecisco 5:6b667e2cb800 269
bulmecisco 5:6b667e2cb800 270 cmd[0] = 0x06;
bulmecisco 5:6b667e2cb800 271 cmd[1] = 0x00;
bulmecisco 5:6b667e2cb800 272 i2c.write(addr, cmd, 2);
bulmecisco 5:6b667e2cb800 273
bulmecisco 5:6b667e2cb800 274 cmd[0]=0x01;
bulmecisco 5:6b667e2cb800 275 i2c.write(addr, cmd, 1);
bulmecisco 5:6b667e2cb800 276 i2c.read(addr|1, cmd, 1);
bulmecisco 5:6b667e2cb800 277 btns = cmd[0];
bulmecisco 5:6b667e2cb800 278 if( btns & btn)
bulmecisco 5:6b667e2cb800 279 wert = true;
bulmecisco 5:6b667e2cb800 280 else
bulmecisco 5:6b667e2cb800 281 wert = false;
bulmecisco 5:6b667e2cb800 282 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 5:6b667e2cb800 283 return wert;
bulmecisco 5:6b667e2cb800 284 }
bulmecisco 5:6b667e2cb800 285
bulmecisco 7:e7f74f072564 286 int ur_Bertl::ReturnButtonPressed()
bulmecisco 7:e7f74f072564 287 {
bulmecisco 7:e7f74f072564 288 char cmd[3]; // array for I2C
bulmecisco 7:e7f74f072564 289 int16_t btns;
bulmecisco 7:e7f74f072564 290
bulmecisco 7:e7f74f072564 291 cmd[0] = 0x06;
bulmecisco 7:e7f74f072564 292 cmd[1] = 0x00;
bulmecisco 7:e7f74f072564 293 i2c.write(addr, cmd, 2);
bulmecisco 7:e7f74f072564 294
bulmecisco 7:e7f74f072564 295 cmd[0]=0x01;
bulmecisco 7:e7f74f072564 296 i2c.write(addr, cmd, 1);
bulmecisco 7:e7f74f072564 297 i2c.read(addr|1, cmd, 1);
bulmecisco 7:e7f74f072564 298 btns = cmd[0];
bulmecisco 7:e7f74f072564 299 DEBUG_PRINT("Button: %d", btns);
bulmecisco 7:e7f74f072564 300 return btns;
bulmecisco 7:e7f74f072564 301 }
bulmecisco 4:76acfddc26fb 302 // Protected methodes
bulmecisco 4:76acfddc26fb 303
bulmecisco 4:76acfddc26fb 304 int ur_Bertl::bottomIsBlack()
bulmecisco 4:76acfddc26fb 305 {
bulmecisco 4:76acfddc26fb 306 int detect;
bulmecisco 4:76acfddc26fb 307
bulmecisco 4:76acfddc26fb 308 detect = linesensor;
bulmecisco 4:76acfddc26fb 309 return detect;
bulmecisco 4:76acfddc26fb 310 }
bulmecisco 4:76acfddc26fb 311
bulmecisco 0:66e9a0afcbd6 312 bool ur_Bertl::backIsClear()
bulmecisco 0:66e9a0afcbd6 313 {
bulmecisco 0:66e9a0afcbd6 314 char cmd[3]; // array for I2C
bulmecisco 0:66e9a0afcbd6 315 int16_t btns;
bulmecisco 0:66e9a0afcbd6 316 bool wert;
bulmecisco 0:66e9a0afcbd6 317
bulmecisco 0:66e9a0afcbd6 318 cmd[0] = 0x06;
bulmecisco 0:66e9a0afcbd6 319 cmd[1] = 0x00;
bulmecisco 0:66e9a0afcbd6 320 i2c.write(addr, cmd, 2);
bulmecisco 0:66e9a0afcbd6 321
bulmecisco 0:66e9a0afcbd6 322 cmd[0]=0x01;
bulmecisco 0:66e9a0afcbd6 323 i2c.write(addr, cmd, 1);
bulmecisco 0:66e9a0afcbd6 324 i2c.read(addr|1, cmd, 1);
bulmecisco 0:66e9a0afcbd6 325 btns = cmd[0];
bulmecisco 0:66e9a0afcbd6 326 if( btns & (BTN_BL|BTN_BM|BTN_BR))
bulmecisco 0:66e9a0afcbd6 327 wert = false;
bulmecisco 0:66e9a0afcbd6 328 else
bulmecisco 0:66e9a0afcbd6 329 wert = true;
bulmecisco 0:66e9a0afcbd6 330 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 0:66e9a0afcbd6 331 return wert;
bulmecisco 0:66e9a0afcbd6 332 }
bulmecisco 0:66e9a0afcbd6 333
bulmecisco 4:76acfddc26fb 334 bool ur_Bertl::frontButtonPressed()
bulmecisco 0:66e9a0afcbd6 335 {
bulmecisco 4:76acfddc26fb 336 char cmd[3]; // array for I2C
bulmecisco 4:76acfddc26fb 337 int16_t btns;
bulmecisco 4:76acfddc26fb 338 bool wert;
bulmecisco 4:76acfddc26fb 339
bulmecisco 4:76acfddc26fb 340 cmd[0] = 0x06;
bulmecisco 4:76acfddc26fb 341 cmd[1] = 0x00;
bulmecisco 4:76acfddc26fb 342 i2c.write(addr, cmd, 2);
bulmecisco 4:76acfddc26fb 343
bulmecisco 4:76acfddc26fb 344 cmd[0]=0x01;
bulmecisco 4:76acfddc26fb 345 i2c.write(addr, cmd, 1);
bulmecisco 4:76acfddc26fb 346 i2c.read(addr|1, cmd, 1);
bulmecisco 4:76acfddc26fb 347 btns = cmd[0];
bulmecisco 4:76acfddc26fb 348 if( btns & (BTN_FL|BTN_FM|BTN_FR|BTN_FRR|BTN_FLL))
bulmecisco 4:76acfddc26fb 349 wert = true;
bulmecisco 4:76acfddc26fb 350 else
bulmecisco 4:76acfddc26fb 351 wert = false;
bulmecisco 4:76acfddc26fb 352 DEBUG_PRINT("WERT: %d", wert);
bulmecisco 4:76acfddc26fb 353 return wert;
bulmecisco 0:66e9a0afcbd6 354 }
bulmecisco 0:66e9a0afcbd6 355
bulmecisco 5:6b667e2cb800 356
bulmecisco 5:6b667e2cb800 357
bulmecisco 4:76acfddc26fb 358 //-----------------INTERNAL USE ONLY ----------------------------
bulmecisco 4:76acfddc26fb 359 void ur_Bertl::error()
bulmecisco 0:66e9a0afcbd6 360 {
bulmecisco 4:76acfddc26fb 361 int wait = 500;
bulmecisco 4:76acfddc26fb 362 MotorR_FORWARD = MotorL_FORWARD = 0; // both motor off
bulmecisco 0:66e9a0afcbd6 363 MotorR_REVERSE = MotorL_REVERSE = 0; // both motor off
bulmecisco 0:66e9a0afcbd6 364 MotorR_EN=MotorL_EN=0;
bulmecisco 4:76acfddc26fb 365 while(1) {
bulmecisco 4:76acfddc26fb 366 TurnLedOff(0xFF);
bulmecisco 4:76acfddc26fb 367 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 0;
bulmecisco 4:76acfddc26fb 368 LED_blue=1;
bulmecisco 4:76acfddc26fb 369 RGB_blue=RGB_green=RGB_red=1;
bulmecisco 4:76acfddc26fb 370 wait_ms(wait);
bulmecisco 4:76acfddc26fb 371 TurnLedOn(0xFF);
bulmecisco 4:76acfddc26fb 372 LED_D10 = LED_D11 = LED_D12 = LED_D13 = 1;
bulmecisco 4:76acfddc26fb 373 LED_blue=0;
bulmecisco 4:76acfddc26fb 374 RGB_blue=RGB_green=1;RGB_red=0;
bulmecisco 4:76acfddc26fb 375 wait_ms(wait);
bulmecisco 4:76acfddc26fb 376 }
bulmecisco 0:66e9a0afcbd6 377 }
bulmecisco 4:76acfddc26fb 378
bulmecisco 4:76acfddc26fb 379 // ISR
bulmecisco 4:76acfddc26fb 380
bulmecisco 4:76acfddc26fb 381 void ur_Bertl::increment()
bulmecisco 4:76acfddc26fb 382 {
bulmecisco 4:76acfddc26fb 383 _count++;
bulmecisco 4:76acfddc26fb 384 }
bulmecisco 4:76acfddc26fb 385
bulmecisco 4:76acfddc26fb 386 int ur_Bertl::Read()
bulmecisco 4:76acfddc26fb 387 {
bulmecisco 4:76acfddc26fb 388 return _count;
bulmecisco 4:76acfddc26fb 389 }
bulmecisco 4:76acfddc26fb 390