Filip Putz / Bertl

Übung

Dependencies:   HCSR

Fork of Bertl by Franz Pucher

ur_Bertl.cpp@18:9480a699f9b7, 2015-12-21 (annotated)

Committer:
Putzi
Date:
Mon Dec 21 14:39:56 2015 +0000
Revision:
18:9480a699f9b7
Parent:
17:308802267a62 
?bung

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