Florian Prettner / ur_Bertl

PickBeeperToWall

Dependencies:   HCSR

ur_Bertl.cpp@6:267f417dbce0, 2015-04-16 (annotated)

Committer:
Prettner67
Date:
Thu Apr 16 11:50:01 2015 +0000
Revision:
6:267f417dbce0
Parent:
5:6b667e2cb800 
Message required

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