Franz Pucher / Bertl

Leds work simultanously. Version 3.1

Dependencies:   HCSR

Dependents:   bertl_led bertl_led bertl_led bertl_led ... more

Fork of Bertl by Bertl_Team_PE

BULME HTL Graz-Gösting

FSST - Hardwarenahe Programmierung

Created by Prof.Dr. Franz Pucher

Inhalt

Inhalt

Start mit folgendem Code in main.cpp eines neuen Projektes mbed_blinky:

#include "mbed.h"
#include "const.h"
#include "Robot.h"

Robot bertl;
    
int main()
{
    bertl.NibbleLeds(0x0F);
    wait(1);
    bertl.NibbleLeds(0x00);
    
    while(1)
    {
        if(bertl.IsButtonPressed(BTN_BL))
        {
            bertl.TurnLedOn(LED_BL1);
        }
        if(bertl.IsButtonPressed(BTN_BR))
        {
            bertl.TurnLedOff(LED_BL1);
        }
    }
}

ur_Bertl.cpp@18:e9bb4513ae3a, 2016-02-19 (annotated)

Committer:
fpucher
Date:
Fri Feb 19 15:28:42 2016 +0000
Revision:
18:e9bb4513ae3a
Parent:
17:308802267a62 
Version 3.2

Who changed what in which revision?

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