Übung
Fork of Bertl by
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?
User | Revision | Line number | New 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 | } |