feito
Robot.cpp@0:c25c4b67b6a1, 2021-05-03 (annotated)
- Committer:
- yaaqobhpt
- Date:
- Mon May 03 14:49:18 2021 +0000
- Revision:
- 0:c25c4b67b6a1
- Child:
- 3:892592b2c2ea
ok-V1
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| yaaqobhpt | 0:c25c4b67b6a1 | 1 | #include "Robot.h" |
| yaaqobhpt | 0:c25c4b67b6a1 | 2 | #include "mbed.h" |
| yaaqobhpt | 0:c25c4b67b6a1 | 3 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 4 | I2C i2c(I2C_SDA, I2C_SCL ); |
| yaaqobhpt | 0:c25c4b67b6a1 | 5 | const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal). |
| yaaqobhpt | 0:c25c4b67b6a1 | 6 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 7 | int16_t countsLeft = 0; |
| yaaqobhpt | 0:c25c4b67b6a1 | 8 | int16_t countsRight = 0; |
| yaaqobhpt | 0:c25c4b67b6a1 | 9 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 10 | void setSpeeds(int16_t leftSpeed, int16_t rightSpeed) |
| yaaqobhpt | 0:c25c4b67b6a1 | 11 | { |
| yaaqobhpt | 0:c25c4b67b6a1 | 12 | char buffer[5]; |
| yaaqobhpt | 0:c25c4b67b6a1 | 13 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 14 | buffer[0] = 0xA1; |
| yaaqobhpt | 0:c25c4b67b6a1 | 15 | memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed)); |
| yaaqobhpt | 0:c25c4b67b6a1 | 16 | memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed)); |
| yaaqobhpt | 0:c25c4b67b6a1 | 17 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 18 | i2c.write(addr8bit, buffer, 5); // 5 bytes |
| yaaqobhpt | 0:c25c4b67b6a1 | 19 | } |
| yaaqobhpt | 0:c25c4b67b6a1 | 20 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 21 | void setLeftSpeed(int16_t speed) |
| yaaqobhpt | 0:c25c4b67b6a1 | 22 | { |
| yaaqobhpt | 0:c25c4b67b6a1 | 23 | char buffer[3]; |
| yaaqobhpt | 0:c25c4b67b6a1 | 24 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 25 | buffer[0] = 0xA2; |
| yaaqobhpt | 0:c25c4b67b6a1 | 26 | memcpy(&buffer[1], &speed, sizeof(speed)); |
| yaaqobhpt | 0:c25c4b67b6a1 | 27 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 28 | i2c.write(addr8bit, buffer, 3); // 3 bytes |
| yaaqobhpt | 0:c25c4b67b6a1 | 29 | } |
| yaaqobhpt | 0:c25c4b67b6a1 | 30 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 31 | void setRightSpeed(int16_t speed) |
| yaaqobhpt | 0:c25c4b67b6a1 | 32 | { |
| yaaqobhpt | 0:c25c4b67b6a1 | 33 | char buffer[3]; |
| yaaqobhpt | 0:c25c4b67b6a1 | 34 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 35 | buffer[0] = 0xA3; |
| yaaqobhpt | 0:c25c4b67b6a1 | 36 | memcpy(&buffer[1], &speed, sizeof(speed)); |
| yaaqobhpt | 0:c25c4b67b6a1 | 37 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 38 | i2c.write(addr8bit, buffer, 3); // 3 bytes |
| yaaqobhpt | 0:c25c4b67b6a1 | 39 | } |
| yaaqobhpt | 0:c25c4b67b6a1 | 40 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 41 | void getCounts() |
| yaaqobhpt | 0:c25c4b67b6a1 | 42 | { |
| yaaqobhpt | 0:c25c4b67b6a1 | 43 | char write_buffer[2]; |
| yaaqobhpt | 0:c25c4b67b6a1 | 44 | char read_buffer[4]; |
| yaaqobhpt | 0:c25c4b67b6a1 | 45 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 46 | write_buffer[0] = 0xA0; |
| yaaqobhpt | 0:c25c4b67b6a1 | 47 | i2c.write(addr8bit, write_buffer, 1); wait_us(100); |
| yaaqobhpt | 0:c25c4b67b6a1 | 48 | i2c.read( addr8bit, read_buffer, 4); |
| yaaqobhpt | 0:c25c4b67b6a1 | 49 | countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1])); |
| yaaqobhpt | 0:c25c4b67b6a1 | 50 | countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3])); |
| yaaqobhpt | 0:c25c4b67b6a1 | 51 | } |
| yaaqobhpt | 0:c25c4b67b6a1 | 52 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 53 | void getCountsAndReset() |
| yaaqobhpt | 0:c25c4b67b6a1 | 54 | { |
| yaaqobhpt | 0:c25c4b67b6a1 | 55 | char write_buffer[2]; |
| yaaqobhpt | 0:c25c4b67b6a1 | 56 | char read_buffer[4]; |
| yaaqobhpt | 0:c25c4b67b6a1 | 57 | |
| yaaqobhpt | 0:c25c4b67b6a1 | 58 | write_buffer[0] = 0xA4; |
| yaaqobhpt | 0:c25c4b67b6a1 | 59 | i2c.write(addr8bit, write_buffer, 1); wait_us(100); |
| yaaqobhpt | 0:c25c4b67b6a1 | 60 | i2c.read( addr8bit, read_buffer, 4); |
| yaaqobhpt | 0:c25c4b67b6a1 | 61 | countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1])); |
| yaaqobhpt | 0:c25c4b67b6a1 | 62 | countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3])); |
| yaaqobhpt | 0:c25c4b67b6a1 | 63 | } |