Bertl_Team_PE / Bertl

Class Bertl

Dependencies:   HCSR

Dependents:   LEDTestmitButton

Fork of ur_Bertl by BERTL_CHEL18

ur_Bertl.cpp@15:43d6a7e6e64a, 2015-05-08 (annotated)

Committer:
bulmecisco
Date:
Fri May 08 08:39:12 2015 +0000
Revision:
15:43d6a7e6e64a
Parent:
13:3ce84646fd74 
Version 3.0

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