Dr. Pucher / ur_Bertl

Class Bertl added

Dependencies:   HCSR

Dependents:   BertlDrive_V2 BertlDrive_V2

ur_Bertl.cpp@6:df6830254e8b, 2015-04-10 (annotated)

Committer:
bulmecisco
Date:
Fri Apr 10 09:31:54 2015 +0000
Revision:
6:df6830254e8b
Parent:
5:6b667e2cb800
Child:
7:e7f74f072564
Constant ULTRASONIC_DISTANCE added.

Who changed what in which revision?

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