BERTL_CHEL18 / Mbed 2 deprecated Bertlv1

Viele Funktionen

Dependencies:   mbed

Funktionen1.cpp@0:50d2b4671eac, 2015-02-05 (annotated)

Committer:
Prettner67
Date:
Thu Feb 05 12:29:51 2015 +0000
Revision:
0:50d2b4671eac
Viele n?tzliche Funktionen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Prettner67 0:50d2b4671eac 1 #include "mbed.h"
Prettner67 0:50d2b4671eac 2 #include "config.h"
Prettner67 0:50d2b4671eac 3
Prettner67 0:50d2b4671eac 4 void Move()
Prettner67 0:50d2b4671eac 5 {
Prettner67 0:50d2b4671eac 6 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 7 MotorL_FORWARD = MotorR_FORWARD = 1;
Prettner67 0:50d2b4671eac 8 wait_ms(300);
Prettner67 0:50d2b4671eac 9 MotorL_FORWARD = MotorR_FORWARD = 0;
Prettner67 0:50d2b4671eac 10 MotorL_EN = MotorR_EN = 0;
Prettner67 0:50d2b4671eac 11
Prettner67 0:50d2b4671eac 12 }
Prettner67 0:50d2b4671eac 13
Prettner67 0:50d2b4671eac 14 void Back()
Prettner67 0:50d2b4671eac 15 {
Prettner67 0:50d2b4671eac 16 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 17 MotorL_REVERSE = MotorR_REVERSE = 1;
Prettner67 0:50d2b4671eac 18 wait_ms(300);
Prettner67 0:50d2b4671eac 19 MotorL_REVERSE = MotorR_REVERSE = 0;
Prettner67 0:50d2b4671eac 20 MotorL_EN = MotorR_EN = 0;
Prettner67 0:50d2b4671eac 21
Prettner67 0:50d2b4671eac 22 }
Prettner67 0:50d2b4671eac 23
Prettner67 0:50d2b4671eac 24 void TurnRight()
Prettner67 0:50d2b4671eac 25 {
Prettner67 0:50d2b4671eac 26 MotorL_EN = 1;
Prettner67 0:50d2b4671eac 27 MotorL_FORWARD = 1;
Prettner67 0:50d2b4671eac 28 wait_ms(100);
Prettner67 0:50d2b4671eac 29 MotorL_FORWARD = 0;
Prettner67 0:50d2b4671eac 30 MotorL_EN = 0;
Prettner67 0:50d2b4671eac 31
Prettner67 0:50d2b4671eac 32 }
Prettner67 0:50d2b4671eac 33
Prettner67 0:50d2b4671eac 34 void TurnLeft()
Prettner67 0:50d2b4671eac 35 {
Prettner67 0:50d2b4671eac 36 MotorR_EN = 1;
Prettner67 0:50d2b4671eac 37 MotorR_FORWARD = 1;
Prettner67 0:50d2b4671eac 38 wait_ms(100);
Prettner67 0:50d2b4671eac 39 MotorR_FORWARD = 0;
Prettner67 0:50d2b4671eac 40 MotorR_EN = 0;
Prettner67 0:50d2b4671eac 41
Prettner67 0:50d2b4671eac 42 }
Prettner67 0:50d2b4671eac 43
Prettner67 0:50d2b4671eac 44 bool WaitUntilButtonPressed()
Prettner67 0:50d2b4671eac 45 {
Prettner67 0:50d2b4671eac 46 char cmd[3];
Prettner67 0:50d2b4671eac 47 int16_t btns;
Prettner67 0:50d2b4671eac 48 bool wert;
Prettner67 0:50d2b4671eac 49
Prettner67 0:50d2b4671eac 50 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 51 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 52 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 53
Prettner67 0:50d2b4671eac 54 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 55 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 56 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 57 btns = cmd[0];
Prettner67 0:50d2b4671eac 58 if( btns & (0xFF))
Prettner67 0:50d2b4671eac 59 wert = false;
Prettner67 0:50d2b4671eac 60 else
Prettner67 0:50d2b4671eac 61 wert = true;
Prettner67 0:50d2b4671eac 62 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 63 return wert;
Prettner67 0:50d2b4671eac 64
Prettner67 0:50d2b4671eac 65 }
Prettner67 0:50d2b4671eac 66
Prettner67 0:50d2b4671eac 67 bool Start()
Prettner67 0:50d2b4671eac 68 {
Prettner67 0:50d2b4671eac 69 DEBUG_PRINT("Debug level: %d", (int) DEBUG);
Prettner67 0:50d2b4671eac 70 i2c.frequency(40000);
Prettner67 0:50d2b4671eac 71
Prettner67 0:50d2b4671eac 72 while(WaitUntilButtonPressed()) { }
Prettner67 0:50d2b4671eac 73 return true;
Prettner67 0:50d2b4671eac 74
Prettner67 0:50d2b4671eac 75 }
Prettner67 0:50d2b4671eac 76
Prettner67 0:50d2b4671eac 77 bool FrontIsClear()
Prettner67 0:50d2b4671eac 78 {
Prettner67 0:50d2b4671eac 79 char cmd[3];
Prettner67 0:50d2b4671eac 80 int16_t btns;
Prettner67 0:50d2b4671eac 81 bool wert;
Prettner67 0:50d2b4671eac 82
Prettner67 0:50d2b4671eac 83 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 84 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 85 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 86
Prettner67 0:50d2b4671eac 87 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 88 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 89 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 90 btns = cmd[0];
Prettner67 0:50d2b4671eac 91 if( btns & (BTN_FLL|BTN_FL|BTN_FM|BTN_FR|BTN_FRR))
Prettner67 0:50d2b4671eac 92 wert = false;
Prettner67 0:50d2b4671eac 93 else
Prettner67 0:50d2b4671eac 94 wert = true;
Prettner67 0:50d2b4671eac 95 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 96 return wert;
Prettner67 0:50d2b4671eac 97
Prettner67 0:50d2b4671eac 98 }
Prettner67 0:50d2b4671eac 99
Prettner67 0:50d2b4671eac 100 void RotateRight()
Prettner67 0:50d2b4671eac 101 {
Prettner67 0:50d2b4671eac 102 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 103 MotorL_FORWARD = MotorR_REVERSE = 1;
Prettner67 0:50d2b4671eac 104 wait_ms(2000);
Prettner67 0:50d2b4671eac 105 MotorL_FORWARD = MotorR_REVERSE = 0;
Prettner67 0:50d2b4671eac 106 MotorL_EN = MotorR_EN = 0;
Prettner67 0:50d2b4671eac 107
Prettner67 0:50d2b4671eac 108 }
Prettner67 0:50d2b4671eac 109
Prettner67 0:50d2b4671eac 110 void RotateLeft()
Prettner67 0:50d2b4671eac 111 {
Prettner67 0:50d2b4671eac 112 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 113 MotorR_FORWARD = MotorL_REVERSE = 1;
Prettner67 0:50d2b4671eac 114 wait_ms(2000);
Prettner67 0:50d2b4671eac 115 MotorR_FORWARD = MotorL_REVERSE = 0;
Prettner67 0:50d2b4671eac 116 MotorL_EN = MotorR_EN = 0;
Prettner67 0:50d2b4671eac 117
Prettner67 0:50d2b4671eac 118 }
Prettner67 0:50d2b4671eac 119
Prettner67 0:50d2b4671eac 120 void MoveTime(int c)
Prettner67 0:50d2b4671eac 121 {
Prettner67 0:50d2b4671eac 122 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 123 MotorL_FORWARD = MotorR_FORWARD = 1;
Prettner67 0:50d2b4671eac 124 wait_ms(c);
Prettner67 0:50d2b4671eac 125 MotorL_FORWARD = MotorR_FORWARD = 0;
Prettner67 0:50d2b4671eac 126 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 127
Prettner67 0:50d2b4671eac 128 }
Prettner67 0:50d2b4671eac 129
Prettner67 0:50d2b4671eac 130 void BackTime(int c)
Prettner67 0:50d2b4671eac 131 {
Prettner67 0:50d2b4671eac 132 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 133 MotorL_REVERSE = MotorR_REVERSE = 1;
Prettner67 0:50d2b4671eac 134 wait_ms(c);
Prettner67 0:50d2b4671eac 135 MotorL_EN = MotorR_EN = 0;
Prettner67 0:50d2b4671eac 136 MotorL_REVERSE = MotorR_REVERSE = 0;
Prettner67 0:50d2b4671eac 137
Prettner67 0:50d2b4671eac 138 }
Prettner67 0:50d2b4671eac 139
Prettner67 0:50d2b4671eac 140 void TurnRightTime(int c)
Prettner67 0:50d2b4671eac 141 {
Prettner67 0:50d2b4671eac 142 MotorL_EN = 1;
Prettner67 0:50d2b4671eac 143 MotorL_FORWARD = 1;
Prettner67 0:50d2b4671eac 144 wait_ms(c);
Prettner67 0:50d2b4671eac 145 MotorL_FORWARD = 0;
Prettner67 0:50d2b4671eac 146 MotorL_EN = 0;
Prettner67 0:50d2b4671eac 147
Prettner67 0:50d2b4671eac 148 }
Prettner67 0:50d2b4671eac 149
Prettner67 0:50d2b4671eac 150 void TrnLeftTime(int c)
Prettner67 0:50d2b4671eac 151 {
Prettner67 0:50d2b4671eac 152 MotorR_EN = 1;
Prettner67 0:50d2b4671eac 153 MotorR_FORWARD = 1;
Prettner67 0:50d2b4671eac 154 wait_ms(c);
Prettner67 0:50d2b4671eac 155 MotorR_FORWARD = 0;
Prettner67 0:50d2b4671eac 156 MotorR_EN = 0;
Prettner67 0:50d2b4671eac 157
Prettner67 0:50d2b4671eac 158 }
Prettner67 0:50d2b4671eac 159
Prettner67 0:50d2b4671eac 160 void RotateRightTime(int c)
Prettner67 0:50d2b4671eac 161 {
Prettner67 0:50d2b4671eac 162 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 163 MotorL_FORWARD = MotorR_REVERSE = 1;
Prettner67 0:50d2b4671eac 164 wait_ms(c);
Prettner67 0:50d2b4671eac 165 MotorL_FORWARD = MotorR_REVERSE = 0;
Prettner67 0:50d2b4671eac 166 MotorL_EN = MotorR_EN = 0;
Prettner67 0:50d2b4671eac 167
Prettner67 0:50d2b4671eac 168 }
Prettner67 0:50d2b4671eac 169
Prettner67 0:50d2b4671eac 170 void RotateLeftTime(int c)
Prettner67 0:50d2b4671eac 171 {
Prettner67 0:50d2b4671eac 172 MotorR_EN = MotorL_EN = 1;
Prettner67 0:50d2b4671eac 173 MotorR_FORWARD = MotorL_REVERSE = 1;
Prettner67 0:50d2b4671eac 174 wait_ms(c);
Prettner67 0:50d2b4671eac 175 MotorR_FORWARD = MotorL_REVERSE = 0;
Prettner67 0:50d2b4671eac 176 MotorL_EN = MotorR_EN = 0;
Prettner67 0:50d2b4671eac 177
Prettner67 0:50d2b4671eac 178 }
Prettner67 0:50d2b4671eac 179
Prettner67 0:50d2b4671eac 180 bool BackIsClear()
Prettner67 0:50d2b4671eac 181 {
Prettner67 0:50d2b4671eac 182 char cmd[3];
Prettner67 0:50d2b4671eac 183 int16_t btns;
Prettner67 0:50d2b4671eac 184 bool wert;
Prettner67 0:50d2b4671eac 185
Prettner67 0:50d2b4671eac 186 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 187 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 188 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 189
Prettner67 0:50d2b4671eac 190 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 191 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 192 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 193 btns = cmd[0];
Prettner67 0:50d2b4671eac 194 if( btns & (BTN_BL|BTN_BM|BTN_BR))
Prettner67 0:50d2b4671eac 195 wert = false;
Prettner67 0:50d2b4671eac 196 else
Prettner67 0:50d2b4671eac 197 wert = true;
Prettner67 0:50d2b4671eac 198 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 199 return wert;
Prettner67 0:50d2b4671eac 200
Prettner67 0:50d2b4671eac 201 }
Prettner67 0:50d2b4671eac 202
Prettner67 0:50d2b4671eac 203 bool ButtonBackLeft()
Prettner67 0:50d2b4671eac 204 {
Prettner67 0:50d2b4671eac 205 char cmd[3];
Prettner67 0:50d2b4671eac 206 int16_t btns;
Prettner67 0:50d2b4671eac 207 bool wert;
Prettner67 0:50d2b4671eac 208
Prettner67 0:50d2b4671eac 209 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 210 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 211 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 212
Prettner67 0:50d2b4671eac 213 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 214 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 215 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 216 btns = cmd[0];
Prettner67 0:50d2b4671eac 217 if( btns & (BTN_BL))
Prettner67 0:50d2b4671eac 218 wert = false;
Prettner67 0:50d2b4671eac 219 else
Prettner67 0:50d2b4671eac 220 wert = true;
Prettner67 0:50d2b4671eac 221 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 222 return wert;
Prettner67 0:50d2b4671eac 223
Prettner67 0:50d2b4671eac 224 }
Prettner67 0:50d2b4671eac 225
Prettner67 0:50d2b4671eac 226 bool ButtonBackMid()
Prettner67 0:50d2b4671eac 227 {
Prettner67 0:50d2b4671eac 228 char cmd[3];
Prettner67 0:50d2b4671eac 229 int16_t btns;
Prettner67 0:50d2b4671eac 230 bool wert;
Prettner67 0:50d2b4671eac 231
Prettner67 0:50d2b4671eac 232 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 233 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 234 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 235
Prettner67 0:50d2b4671eac 236 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 237 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 238 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 239 btns = cmd[0];
Prettner67 0:50d2b4671eac 240 if( btns & (BTN_BM))
Prettner67 0:50d2b4671eac 241 wert = false;
Prettner67 0:50d2b4671eac 242 else
Prettner67 0:50d2b4671eac 243 wert = true;
Prettner67 0:50d2b4671eac 244 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 245 return wert;
Prettner67 0:50d2b4671eac 246 }
Prettner67 0:50d2b4671eac 247
Prettner67 0:50d2b4671eac 248 bool ButtonBackRight()
Prettner67 0:50d2b4671eac 249 {
Prettner67 0:50d2b4671eac 250 char cmd[3];
Prettner67 0:50d2b4671eac 251 int16_t btns;
Prettner67 0:50d2b4671eac 252 bool wert;
Prettner67 0:50d2b4671eac 253
Prettner67 0:50d2b4671eac 254 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 255 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 256 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 257
Prettner67 0:50d2b4671eac 258 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 259 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 260 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 261 btns = cmd[0];
Prettner67 0:50d2b4671eac 262 if( btns & (BTN_BR))
Prettner67 0:50d2b4671eac 263 wert = false;
Prettner67 0:50d2b4671eac 264 else
Prettner67 0:50d2b4671eac 265 wert = true;
Prettner67 0:50d2b4671eac 266 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 267 return wert;
Prettner67 0:50d2b4671eac 268 }
Prettner67 0:50d2b4671eac 269
Prettner67 0:50d2b4671eac 270 bool ButtonFrontLeftLeft()
Prettner67 0:50d2b4671eac 271 {
Prettner67 0:50d2b4671eac 272 char cmd[3];
Prettner67 0:50d2b4671eac 273 int16_t btns;
Prettner67 0:50d2b4671eac 274 bool wert;
Prettner67 0:50d2b4671eac 275
Prettner67 0:50d2b4671eac 276 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 277 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 278 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 279
Prettner67 0:50d2b4671eac 280 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 281 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 282 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 283 btns = cmd[0];
Prettner67 0:50d2b4671eac 284 if( btns & (BTN_FLL))
Prettner67 0:50d2b4671eac 285 wert = false;
Prettner67 0:50d2b4671eac 286 else
Prettner67 0:50d2b4671eac 287 wert = true;
Prettner67 0:50d2b4671eac 288 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 289 return wert;
Prettner67 0:50d2b4671eac 290 }
Prettner67 0:50d2b4671eac 291
Prettner67 0:50d2b4671eac 292 bool ButtonFrontLeft()
Prettner67 0:50d2b4671eac 293 {
Prettner67 0:50d2b4671eac 294 char cmd[3];
Prettner67 0:50d2b4671eac 295 int16_t btns;
Prettner67 0:50d2b4671eac 296 bool wert;
Prettner67 0:50d2b4671eac 297
Prettner67 0:50d2b4671eac 298 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 299 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 300 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 301
Prettner67 0:50d2b4671eac 302 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 303 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 304 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 305 btns = cmd[0];
Prettner67 0:50d2b4671eac 306 if( btns & (BTN_FL))
Prettner67 0:50d2b4671eac 307 wert = false;
Prettner67 0:50d2b4671eac 308 else
Prettner67 0:50d2b4671eac 309 wert = true;
Prettner67 0:50d2b4671eac 310 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 311 return wert;
Prettner67 0:50d2b4671eac 312 }
Prettner67 0:50d2b4671eac 313
Prettner67 0:50d2b4671eac 314 bool ButtonFrontMid()
Prettner67 0:50d2b4671eac 315 {
Prettner67 0:50d2b4671eac 316 char cmd[3];
Prettner67 0:50d2b4671eac 317 int16_t btns;
Prettner67 0:50d2b4671eac 318 bool wert;
Prettner67 0:50d2b4671eac 319
Prettner67 0:50d2b4671eac 320 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 321 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 322 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 323
Prettner67 0:50d2b4671eac 324 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 325 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 326 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 327 btns = cmd[0];
Prettner67 0:50d2b4671eac 328 if( btns & (BTN_FM))
Prettner67 0:50d2b4671eac 329 wert = false;
Prettner67 0:50d2b4671eac 330 else
Prettner67 0:50d2b4671eac 331 wert = true;
Prettner67 0:50d2b4671eac 332 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 333 return wert;
Prettner67 0:50d2b4671eac 334 }
Prettner67 0:50d2b4671eac 335
Prettner67 0:50d2b4671eac 336 bool ButtonFrontRight()
Prettner67 0:50d2b4671eac 337 {
Prettner67 0:50d2b4671eac 338 char cmd[3];
Prettner67 0:50d2b4671eac 339 int16_t btns;
Prettner67 0:50d2b4671eac 340 bool wert;
Prettner67 0:50d2b4671eac 341
Prettner67 0:50d2b4671eac 342 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 343 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 344 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 345
Prettner67 0:50d2b4671eac 346 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 347 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 348 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 349 btns = cmd[0];
Prettner67 0:50d2b4671eac 350 if( btns & (BTN_FR))
Prettner67 0:50d2b4671eac 351 wert = false;
Prettner67 0:50d2b4671eac 352 else
Prettner67 0:50d2b4671eac 353 wert = true;
Prettner67 0:50d2b4671eac 354 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 355 return wert;
Prettner67 0:50d2b4671eac 356 }
Prettner67 0:50d2b4671eac 357
Prettner67 0:50d2b4671eac 358 bool ButtonFrontRightRight()
Prettner67 0:50d2b4671eac 359 {
Prettner67 0:50d2b4671eac 360 char cmd[3];
Prettner67 0:50d2b4671eac 361 int16_t btns;
Prettner67 0:50d2b4671eac 362 bool wert;
Prettner67 0:50d2b4671eac 363
Prettner67 0:50d2b4671eac 364 cmd[0] = 0x06;
Prettner67 0:50d2b4671eac 365 cmd[1] = 0x00;
Prettner67 0:50d2b4671eac 366 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 367
Prettner67 0:50d2b4671eac 368 cmd[0]=0x01;
Prettner67 0:50d2b4671eac 369 i2c.write(addr, cmd, 1);
Prettner67 0:50d2b4671eac 370 i2c.read(addr|1, cmd, 1);
Prettner67 0:50d2b4671eac 371 btns = cmd[0];
Prettner67 0:50d2b4671eac 372 if( btns & (BTN_FRR))
Prettner67 0:50d2b4671eac 373 wert = false;
Prettner67 0:50d2b4671eac 374 else
Prettner67 0:50d2b4671eac 375 wert = true;
Prettner67 0:50d2b4671eac 376 DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
Prettner67 0:50d2b4671eac 377 return wert;
Prettner67 0:50d2b4671eac 378 }
Prettner67 0:50d2b4671eac 379
Prettner67 0:50d2b4671eac 380 void TurnLedOn(int16_t led)
Prettner67 0:50d2b4671eac 381 {
Prettner67 0:50d2b4671eac 382 char cmd[3];
Prettner67 0:50d2b4671eac 383
Prettner67 0:50d2b4671eac 384 cmd[0] = 0x02;
Prettner67 0:50d2b4671eac 385 cmd[1] = ~led;
Prettner67 0:50d2b4671eac 386 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 387 wait(0.5);
Prettner67 0:50d2b4671eac 388 }
Prettner67 0:50d2b4671eac 389
Prettner67 0:50d2b4671eac 390 void TurnLedOff(int16_t led)
Prettner67 0:50d2b4671eac 391 {
Prettner67 0:50d2b4671eac 392 char cmd[3];
Prettner67 0:50d2b4671eac 393
Prettner67 0:50d2b4671eac 394 cmd[0] = 0x02;
Prettner67 0:50d2b4671eac 395 cmd[1] = ~led;
Prettner67 0:50d2b4671eac 396 i2c.write(addr, cmd, 2);
Prettner67 0:50d2b4671eac 397 wait(0.5);
Prettner67 0:50d2b4671eac 398 }
Prettner67 0:50d2b4671eac 399
Prettner67 0:50d2b4671eac 400 bool MoveTicks(int c)
Prettner67 0:50d2b4671eac 401 {
Prettner67 0:50d2b4671eac 402 int count = 0, left = 0, right = 0;
Prettner67 0:50d2b4671eac 403
Prettner67 0:50d2b4671eac 404 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 405
Prettner67 0:50d2b4671eac 406 while(c--)
Prettner67 0:50d2b4671eac 407 {
Prettner67 0:50d2b4671eac 408 MotorL_FORWARD = MotorR_FORWARD = 1;
Prettner67 0:50d2b4671eac 409
Prettner67 0:50d2b4671eac 410 if(SensorL == 1)
Prettner67 0:50d2b4671eac 411 left++;
Prettner67 0:50d2b4671eac 412 if(SensorR == 1)
Prettner67 0:50d2b4671eac 413 right++;
Prettner67 0:50d2b4671eac 414
Prettner67 0:50d2b4671eac 415 count++;
Prettner67 0:50d2b4671eac 416 wait_ms(10);
Prettner67 0:50d2b4671eac 417 }
Prettner67 0:50d2b4671eac 418
Prettner67 0:50d2b4671eac 419 MotorL_FORWARD = MotorR_FORWARD = 0;
Prettner67 0:50d2b4671eac 420 MotorL_EN = MotorR_EN = 0;
Prettner67 0:50d2b4671eac 421
Prettner67 0:50d2b4671eac 422 return true;
Prettner67 0:50d2b4671eac 423
Prettner67 0:50d2b4671eac 424 }
Prettner67 0:50d2b4671eac 425
Prettner67 0:50d2b4671eac 426 bool BackTicks(int c)
Prettner67 0:50d2b4671eac 427 {
Prettner67 0:50d2b4671eac 428 int count = 0, left = 0, right = 0;
Prettner67 0:50d2b4671eac 429
Prettner67 0:50d2b4671eac 430 MotorL_EN = MotorR_EN = 1;
Prettner67 0:50d2b4671eac 431
Prettner67 0:50d2b4671eac 432 while(c--)
Prettner67 0:50d2b4671eac 433 {
Prettner67 0:50d2b4671eac 434 MotorL_REVERSE = MotorR_REVERSE = 1;
Prettner67 0:50d2b4671eac 435
Prettner67 0:50d2b4671eac 436 if(SensorL == 1)
Prettner67 0:50d2b4671eac 437 left++;
Prettner67 0:50d2b4671eac 438 if(SensorR == 1)
Prettner67 0:50d2b4671eac 439 right++;
Prettner67 0:50d2b4671eac 440
Prettner67 0:50d2b4671eac 441 count++;
Prettner67 0:50d2b4671eac 442 wait_ms(10);
Prettner67 0:50d2b4671eac 443 }
Prettner67 0:50d2b4671eac 444
Prettner67 0:50d2b4671eac 445 MotorL_REVERSE = MotorR_REVERSE = 0;
Prettner67 0:50d2b4671eac 446 MotorL_EN = MotorR_EN = 0;
Prettner67 0:50d2b4671eac 447
Prettner67 0:50d2b4671eac 448 return true;
Prettner67 0:50d2b4671eac 449
Prettner67 0:50d2b4671eac 450 }
Prettner67 0:50d2b4671eac 451
Prettner67 0:50d2b4671eac 452 bool MotorRightMoveTicks(int c)
Prettner67 0:50d2b4671eac 453 {
Prettner67 0:50d2b4671eac 454 int count = 0, right = 0;
Prettner67 0:50d2b4671eac 455
Prettner67 0:50d2b4671eac 456 MotorR_EN = 1;
Prettner67 0:50d2b4671eac 457
Prettner67 0:50d2b4671eac 458 while(c--)
Prettner67 0:50d2b4671eac 459 {
Prettner67 0:50d2b4671eac 460 MotorR_FORWARD = 1;
Prettner67 0:50d2b4671eac 461
Prettner67 0:50d2b4671eac 462 if(SensorR == 1)
Prettner67 0:50d2b4671eac 463 right++;
Prettner67 0:50d2b4671eac 464
Prettner67 0:50d2b4671eac 465 count++;
Prettner67 0:50d2b4671eac 466 wait_ms(10);
Prettner67 0:50d2b4671eac 467 }
Prettner67 0:50d2b4671eac 468
Prettner67 0:50d2b4671eac 469 MotorR_FORWARD = 0;
Prettner67 0:50d2b4671eac 470 MotorR_EN = 0;
Prettner67 0:50d2b4671eac 471
Prettner67 0:50d2b4671eac 472 return true;
Prettner67 0:50d2b4671eac 473
Prettner67 0:50d2b4671eac 474 }
Prettner67 0:50d2b4671eac 475
Prettner67 0:50d2b4671eac 476 bool MotorRightBackTicks(int c)
Prettner67 0:50d2b4671eac 477 {
Prettner67 0:50d2b4671eac 478 int count = 0, right = 0;
Prettner67 0:50d2b4671eac 479
Prettner67 0:50d2b4671eac 480 MotorR_EN = 1;
Prettner67 0:50d2b4671eac 481
Prettner67 0:50d2b4671eac 482 while(c--)
Prettner67 0:50d2b4671eac 483 {
Prettner67 0:50d2b4671eac 484 MotorR_REVERSE = 1;
Prettner67 0:50d2b4671eac 485
Prettner67 0:50d2b4671eac 486 if(SensorR == 1)
Prettner67 0:50d2b4671eac 487 right++;
Prettner67 0:50d2b4671eac 488
Prettner67 0:50d2b4671eac 489 count++;
Prettner67 0:50d2b4671eac 490 wait_ms(10);
Prettner67 0:50d2b4671eac 491 }
Prettner67 0:50d2b4671eac 492
Prettner67 0:50d2b4671eac 493 MotorR_REVERSE = 0;
Prettner67 0:50d2b4671eac 494 MotorR_EN = 0;
Prettner67 0:50d2b4671eac 495
Prettner67 0:50d2b4671eac 496 return true;
Prettner67 0:50d2b4671eac 497
Prettner67 0:50d2b4671eac 498 }
Prettner67 0:50d2b4671eac 499
Prettner67 0:50d2b4671eac 500 bool MotorLeftMoveTicks(int c)
Prettner67 0:50d2b4671eac 501 {
Prettner67 0:50d2b4671eac 502 int count = 0, right = 0;
Prettner67 0:50d2b4671eac 503
Prettner67 0:50d2b4671eac 504 MotorL_EN = 1;
Prettner67 0:50d2b4671eac 505
Prettner67 0:50d2b4671eac 506 while(c--)
Prettner67 0:50d2b4671eac 507 {
Prettner67 0:50d2b4671eac 508 MotorL_FORWARD = 1;
Prettner67 0:50d2b4671eac 509
Prettner67 0:50d2b4671eac 510 if(SensorR == 1)
Prettner67 0:50d2b4671eac 511 right++;
Prettner67 0:50d2b4671eac 512
Prettner67 0:50d2b4671eac 513 count++;
Prettner67 0:50d2b4671eac 514 wait_ms(10);
Prettner67 0:50d2b4671eac 515 }
Prettner67 0:50d2b4671eac 516
Prettner67 0:50d2b4671eac 517 MotorL_FORWARD = 0;
Prettner67 0:50d2b4671eac 518 MotorL_EN = 0;
Prettner67 0:50d2b4671eac 519
Prettner67 0:50d2b4671eac 520 return true;
Prettner67 0:50d2b4671eac 521
Prettner67 0:50d2b4671eac 522 }
Prettner67 0:50d2b4671eac 523
Prettner67 0:50d2b4671eac 524 bool MotorLeftBackTicks(int c)
Prettner67 0:50d2b4671eac 525 {
Prettner67 0:50d2b4671eac 526 int count = 0, right = 0;
Prettner67 0:50d2b4671eac 527
Prettner67 0:50d2b4671eac 528 MotorL_EN = 1;
Prettner67 0:50d2b4671eac 529
Prettner67 0:50d2b4671eac 530 while(c--)
Prettner67 0:50d2b4671eac 531 {
Prettner67 0:50d2b4671eac 532 MotorL_REVERSE = 1;
Prettner67 0:50d2b4671eac 533
Prettner67 0:50d2b4671eac 534 if(SensorR == 1)
Prettner67 0:50d2b4671eac 535 right++;
Prettner67 0:50d2b4671eac 536
Prettner67 0:50d2b4671eac 537 count++;
Prettner67 0:50d2b4671eac 538 wait_ms(10);
Prettner67 0:50d2b4671eac 539 }
Prettner67 0:50d2b4671eac 540
Prettner67 0:50d2b4671eac 541 MotorL_REVERSE = 0;
Prettner67 0:50d2b4671eac 542 MotorL_EN = 0;
Prettner67 0:50d2b4671eac 543
Prettner67 0:50d2b4671eac 544 return true;
Prettner67 0:50d2b4671eac 545
Prettner67 0:50d2b4671eac 546 }